From 95a0b4c959dcb7a36a4d0bdd0ad3895cd56e1b84 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 30 Jul 2024 11:48:23 +0900 Subject: [PATCH 01/10] use template rendering for more flexible sensor configuration Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/CMakeLists.txt | 25 ++ aip_xx1_description/scripts/compile_xacro.py | 329 ++++++++++++++++++ .../templates/sensor_unit.xacro.template | 31 ++ .../templates/sensors.xacro.template | 31 ++ aip_xx1_description/urdf/.gitignore | 1 + aip_xx1_description/urdf/radar.xacro | 31 -- aip_xx1_description/urdf/sensor_kit.xacro | 228 ------------ aip_xx1_description/urdf/sensors.xacro | 72 ---- 8 files changed, 417 insertions(+), 331 deletions(-) create mode 100644 aip_xx1_description/scripts/compile_xacro.py create mode 100644 aip_xx1_description/templates/sensor_unit.xacro.template create mode 100644 aip_xx1_description/templates/sensors.xacro.template create mode 100644 aip_xx1_description/urdf/.gitignore delete mode 100644 aip_xx1_description/urdf/radar.xacro delete mode 100644 aip_xx1_description/urdf/sensor_kit.xacro delete mode 100644 aip_xx1_description/urdf/sensors.xacro diff --git a/aip_xx1_description/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index 50723262..7757d1fc 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -1,10 +1,35 @@ cmake_minimum_required(VERSION 3.5) project(aip_xx1_description) + find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +find_package(PythonInterp REQUIRED) +# Specify the path to your Python script +set(PYTHON_SCRIPT "${CMAKE_CURRENT_SOURCE_DIR}/scripts/compile_xacro.py") +set(PYTHON_TEMPLATE_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/templates") +set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") +set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") + +# Run the Python script +execute_process( + COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + RESULT_VARIABLE result + OUTPUT_VARIABLE output + ERROR_VARIABLE error +) +# message(WARNING "Python script output: ${output}") + +if(NOT result EQUAL 0) + message(FATAL_ERROR "Python script failed with error: ${error}") +else() + message(WARNING "Xacro Compiling Success") +endif() + + ament_auto_package(INSTALL_TO_SHARE urdf config diff --git a/aip_xx1_description/scripts/compile_xacro.py b/aip_xx1_description/scripts/compile_xacro.py new file mode 100644 index 00000000..d08c93aa --- /dev/null +++ b/aip_xx1_description/scripts/compile_xacro.py @@ -0,0 +1,329 @@ +from jinja2 import Template +import os +import enum +import functools +import yaml +from typing import Dict + + +def load_yaml(file_path: str) -> Dict: + with open(file_path, "r") as stream: + try: + return yaml.safe_load(stream) + except yaml.YAMLError as exc: + print(exc) + return None + +class Transformation: + def __init__(self, transformation: Dict, base_frame: str, child_frame: str): + try: + self.x = transformation["x"] + self.y = transformation["y"] + self.z = transformation["z"] + self.roll = transformation["roll"] + self.pitch = transformation["pitch"] + self.yaw = transformation["yaw"] + self.base_frame = base_frame + self.child_frame = child_frame + + self.name = self.child_frame.replace("_base_link", "").replace("_link", "") + + except KeyError as e: + print(f"Error: Key {e} not found in transformation dictionary") + raise e + + def serialize_single(self, key: str) -> str: + return f"${{calibration['{self.base_frame}']['{self.child_frame}']['{key}']}}" + + def serialize(self) -> str: + return f""" + name=\"{self.name}\" + parant=\"{self.base_frame}\" + x=\"{self.serialize_single('x')}\" + y=\"{self.serialize_single('y')}\" + z=\"{self.serialize_single('z')}\" + roll=\"{self.serialize_single('roll')}\" + pitch=\"{self.serialize_single('pitch')}\" + yaw=\"{self.serialize_single('yaw')}\" + """ + + +class Calibration: + def __init__(self, calibration: Dict): + self.base_dict: Dict = calibration + assert ( + len(calibration.keys()) == 1 + ), "Calibration file should have only one base frame" + assert isinstance( + list(calibration.keys())[0], str + ), "Calibration file should have only one base frame with key as a string" + self.base_frame: str = list(calibration.keys())[0] + + assert isinstance( + calibration[self.base_frame], dict + ), "Calibration file should have only one base frame with value as a dictionary" + + self.transforms: Dict[str, Transformation] = dict() + + for key in calibration[self.base_frame]: + assert isinstance(key, str), "child frames should be strings" + try: + self.transforms[key] = Transformation( + calibration[self.base_frame][key], self.base_frame, key + ) + except KeyError as e: + print(f"Error: Key {e} not found in calibration dictionary of {key}") + raise e + + +class LinkType(enum.Enum): + """ + Enum class for the type of the link + """ + CAMERA = "monocular_camera" + IMU = "imu" + LIVOX = "livox_horizon" + PANDAR_40P = "pandar_40p" + PANDAR_OT128 = "pandar_ot128" + PANDAR_XT32 = "pandar_xt32" + PANDAR_QT = "pandar_qt" + PANDAR_QT128 = "pandar_qt128" + VELODYNE16 = "VLP-16.urdf" + VLS128 = "VLS-128.urdf" + RADAR = "radar" + JOINT_UNITS = "units" + +def determine_link_type(link_name:str)->LinkType: + if 'cam' in link_name: + return LinkType.CAMERA + + if 'imu' in link_name or 'gnss' in link_name: + return LinkType.IMU + + if 'livox' in link_name: + return LinkType.LIVOX + + if 'velodyne' in link_name: + if 'top' in link_name: + return LinkType.VLS128 + else: + return LinkType.VELODYNE16 + + if 'radar' in link_name or 'ars' in link_name: + return LinkType.RADAR + + if 'pandar_40p' in link_name: + return LinkType.PANDAR_40P + + if 'pandar_qt' in link_name: + return LinkType.PANDAR_QT + + if 'hesai_top' in link_name: + return LinkType.PANDAR_OT128 + + if 'hesai_front' in link_name: + return LinkType.PANDAR_XT32 + + if 'hesai' in link_name: + return LinkType.PANDAR_XT32 + + else: + print(f"Link type not found for {link_name}, suspected to be a joint unit") + return LinkType.JOINT_UNITS + +BASE_STRING = """""" + +VLD_STRING = """ + + """ + +def base_string_func(type:str, transform:Transformation)->str: + if type == "monocular_camera_macro": + extra = """fps=\"30\" + width=\"800\" + height=\"400\" + namespace=\"\" + fov=\"1.3\"""" + elif type == "imu_macro": + extra = """fps=\"100\" + namespace=\"\"""" + else: + extra = "" + return BASE_STRING.format( + type=type, base_frame=transform.base_frame, child_frame=transform.child_frame, + x=transform.serialize_single('x'), y=transform.serialize_single('y'), z=transform.serialize_single('z'), + roll=transform.serialize_single('roll'), pitch=transform.serialize_single('pitch'), yaw=transform.serialize_single('yaw'), + extra=extra) + +def VLP16_func(transform:Transformation)->str: + return VLD_STRING.format( + type="VLP-16", base_frame=transform.base_frame, child_frame=transform.child_frame, + x=transform.serialize_single('x'), y=transform.serialize_single('y'), z=transform.serialize_single('z'), + roll=transform.serialize_single('roll'), pitch=transform.serialize_single('pitch'), yaw=transform.serialize_single('yaw') + ) + +def VLS128_func(transform:Transformation)->str: + return VLD_STRING.format( + type="VLS-128", base_frame=transform.base_frame, child_frame=transform.child_frame, + x=transform.serialize_single('x'), y=transform.serialize_single('y'), z=transform.serialize_single('z'), + roll=transform.serialize_single('roll'), pitch=transform.serialize_single('pitch'), yaw=transform.serialize_single('yaw') + ) + +link_dicts = { + LinkType.CAMERA:{ + "including_file": "$(find camera_description)/urdf/monocular_camera.xacro", + "string_api": functools.partial(base_string_func, "monocular_camera_macro") + }, + LinkType.IMU:{ + "including_file": "$(find imu_description)/urdf/imu.xacro", + "string_api": functools.partial(base_string_func, "imu_macro") + }, + LinkType.VELODYNE16:{ + "including_file": "$(find velodyne_description)/urdf/VLP-16.urdf.xacro", + "string_api": VLP16_func + }, + LinkType.VLS128:{ + "including_file": "$(find vls_description)/urdf/VLS-128.urdf.xacro", + "string_api": VLS128_func + }, + LinkType.PANDAR_40P:{ + "including_file": "$(find pandar_description)/urdf/pandar_40p.xacro", + "string_api": functools.partial(base_string_func, "Pandar40P") + }, + LinkType.PANDAR_OT128:{ + "including_file": "$(find pandar_description)/urdf/pandar_ot128.xacro", + "string_api": functools.partial(base_string_func, "PandarOT-128") + }, + LinkType.PANDAR_XT32:{ + "including_file": "$(find pandar_description)/urdf/pandar_xt32.xacro", + "string_api": functools.partial(base_string_func, "PandarXT-32") + }, + LinkType.PANDAR_QT:{ + "including_file": "$(find pandar_description)/urdf/pandar_qt.xacro", + "string_api": functools.partial(base_string_func, "PandarQT") + }, + LinkType.PANDAR_QT128:{ + "including_file": "$(find pandar_description)/urdf/pandar_qt128.xacro", + "string_api": functools.partial(base_string_func, "PandarQT-128") + }, + LinkType.LIVOX:{ + "including_file": "$(find livox_description)/urdf/livox_horizon.xacro", + "string_api": functools.partial(base_string_func, "livox_horizon_macro") + }, + LinkType.RADAR:{ + "including_file": "$(find radar_description)/urdf/radar.xacro", + "string_api": functools.partial(base_string_func, "radar_macro") + }, + LinkType.JOINT_UNITS:{ + "including_file": "{filename}.xacro", + } +} + + +def main(template_directory:str, calibration_directory:str, output_directory:str, project_name:str): + os.path.mkdirs(output_directory, exist_ok=True) + # Load the template + with open(os.path.join(template_directory, 'sensors.xacro.template'), 'r') as file: + base_template = Template(file.read()) + + # Render the template + calibration_path = os.path.join(calibration_directory, "sensors_calibration.yaml") + calib_yaml = load_yaml(calibration_path) + calib = Calibration(calib_yaml) + + render_meta_data = dict() + render_meta_data['default_config_path'] = f"$(find {project_name})/config" + render_meta_data["sensor_calibration_yaml_path"] = f"$(find {project_name})/config/sensors_calibration.yaml" + render_meta_data["sensor_units_includes"] = [] + render_meta_data["sensor_units"] = [] + render_meta_data["isolated_sensors_includes"] = [] + render_meta_data["isolated_sensors"] = [] + + + include_text = set() + sensor_items = [] + for _, transform in calib.transforms.items(): + link_type:LinkType = determine_link_type(transform.child_frame) + if link_type == LinkType.JOINT_UNITS: + render_meta_data["sensor_units_includes"].append(link_dicts[link_type]['including_file'].format(filename=transform.name)) + render_meta_data["sensor_units"].append( + dict( + base_frame=transform.base_frame, + child_frame=transform.child_frame, + macro_name=f"{transform.name}_macro", + name = transform.name + ) + ) + else: + include_text.add(link_dicts[link_type]['including_file']) + sensor_items.append(link_dicts[link_type]['string_api'](transform)) + + render_meta_data["isolated_sensors_includes"] = list(include_text) + render_meta_data["isolated_sensors"] = sensor_items + + rendered = base_template.render(render_meta_data) + print(rendered) + + print("=====================================") + # Save the rendered template + with open(os.path.join(output_directory, 'sensors.xacro'), 'w') as file: + file.write(rendered) + + + ## Write Sensor Units into separate files + with open(os.path.join(template_directory, 'sensor_unit.xacro.template'), 'r') as file: + sensor_units_template = Template(file.read()) + + for i, sensor_unit in enumerate(render_meta_data["sensor_units"]): + sensor_unit_calib_path = os.path.join(calibration_directory, f"{sensor_unit['name']}_calibration.yaml") + sensor_unit_calib_yaml = load_yaml(sensor_unit_calib_path) + sensor_unit_calib = Calibration(sensor_unit_calib_yaml) + sensor_unit_render_meta_data = dict() + sensor_unit_render_meta_data['unit_macro_name'] = sensor_unit['macro_name'] + sensor_unit_render_meta_data['default_config_path'] = render_meta_data['default_config_path'] + + sensor_unit_render_meta_data['current_base_link'] = sensor_unit_calib.base_frame + sensor_unit_isolated_sensors = [] + for _, transform in sensor_unit_calib.transforms.items(): + link_type:LinkType = determine_link_type(transform.child_frame) + include_text.add(link_dicts[link_type]['including_file']) + sensor_unit_isolated_sensors.append(link_dicts[link_type]['string_api'](transform)) + sensor_unit_render_meta_data["isolated_sensors_includes"] = list(include_text) + sensor_unit_render_meta_data["isolated_sensors"] = sensor_unit_isolated_sensors + + rendered = sensor_units_template.render(sensor_unit_render_meta_data) + print(rendered) + with open(os.path.join(output_directory, f'{sensor_unit["name"]}.xacro'), 'w') as file: + file.write(rendered) + print("=====================================") + + return 0 + +if __name__ == "__main__": + # import argparse + # parser = argparse.ArgumentParser(description="Compile xacro files from calibration files") + # parser.add_argument("--template_directory", type=str, help="Path to the template directory", required=True) + # parser.add_argument("--calibration_directory", type=str, help="Path to the calibration directory", required=True) + # parser.add_argument("--output_directory", type=str, help="Path to the output directory", required=True) + # parser.add_argument("--project_name", type=str, help="Name of the project", required=True) + # args = parser.parse_args() + from fire import Fire + Fire(main) + # main(args.template_directory, args.calibration_directory, args.output_directory, args.project_name) \ No newline at end of file diff --git a/aip_xx1_description/templates/sensor_unit.xacro.template b/aip_xx1_description/templates/sensor_unit.xacro.template new file mode 100644 index 00000000..9dd919d1 --- /dev/null +++ b/aip_xx1_description/templates/sensor_unit.xacro.template @@ -0,0 +1,31 @@ + + + + {% for item in isolated_sensors_includes %} + + {% endfor %} + + + + + + + + + + + + + + + + + + + + {% for item in isolated_sensors %} + {{ item }} + {% endfor %} + + + \ No newline at end of file diff --git a/aip_xx1_description/templates/sensors.xacro.template b/aip_xx1_description/templates/sensors.xacro.template new file mode 100644 index 00000000..318e48bd --- /dev/null +++ b/aip_xx1_description/templates/sensors.xacro.template @@ -0,0 +1,31 @@ + + + + + + + {% for item in sensor_units_includes %} + + {% endfor %} + {% for item in isolated_sensors_includes %} + + {% endfor %} + + {% for item in sensor_units %} + + + {% endfor %} + + {% for item in isolated_sensors %} + + {{ item }} + {% endfor %} + diff --git a/aip_xx1_description/urdf/.gitignore b/aip_xx1_description/urdf/.gitignore new file mode 100644 index 00000000..05ca38bc --- /dev/null +++ b/aip_xx1_description/urdf/.gitignore @@ -0,0 +1 @@ +*.xacro \ No newline at end of file diff --git a/aip_xx1_description/urdf/radar.xacro b/aip_xx1_description/urdf/radar.xacro deleted file mode 100644 index 8b0f8d4b..00000000 --- a/aip_xx1_description/urdf/radar.xacro +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_description/urdf/sensor_kit.xacro b/aip_xx1_description/urdf/sensor_kit.xacro deleted file mode 100644 index ef36763f..00000000 --- a/aip_xx1_description/urdf/sensor_kit.xacro +++ /dev/null @@ -1,228 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_description/urdf/sensors.xacro b/aip_xx1_description/urdf/sensors.xacro deleted file mode 100644 index 0484bdc3..00000000 --- a/aip_xx1_description/urdf/sensors.xacro +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - From eb474d9a0aeb12fa481e0ae5a2565dc35da8e02f Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 30 Jul 2024 11:49:32 +0900 Subject: [PATCH 02/10] fix temporary grammar Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/scripts/compile_xacro.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_xx1_description/scripts/compile_xacro.py b/aip_xx1_description/scripts/compile_xacro.py index d08c93aa..462fb12e 100644 --- a/aip_xx1_description/scripts/compile_xacro.py +++ b/aip_xx1_description/scripts/compile_xacro.py @@ -238,7 +238,7 @@ def VLS128_func(transform:Transformation)->str: def main(template_directory:str, calibration_directory:str, output_directory:str, project_name:str): - os.path.mkdirs(output_directory, exist_ok=True) + os.makedirs(output_directory, exist_ok=True) # Load the template with open(os.path.join(template_directory, 'sensors.xacro.template'), 'r') as file: base_template = Template(file.read()) From 44d8c80cb929da1c61382909cff3c652d5aa5ea8 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 30 Jul 2024 14:47:30 +0900 Subject: [PATCH 03/10] fix pre-commit Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/CMakeLists.txt | 2 +- aip_xx1_description/scripts/compile_xacro.py | 238 +++++++++++-------- 2 files changed, 137 insertions(+), 103 deletions(-) diff --git a/aip_xx1_description/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index 7757d1fc..1d62bf5e 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -6,7 +6,7 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(PythonInterp REQUIRED) +find_package(PythonInterp REQUIRED) # cspell: ignore Interp # Specify the path to your Python script set(PYTHON_SCRIPT "${CMAKE_CURRENT_SOURCE_DIR}/scripts/compile_xacro.py") set(PYTHON_TEMPLATE_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/templates") diff --git a/aip_xx1_description/scripts/compile_xacro.py b/aip_xx1_description/scripts/compile_xacro.py index 462fb12e..90473b31 100644 --- a/aip_xx1_description/scripts/compile_xacro.py +++ b/aip_xx1_description/scripts/compile_xacro.py @@ -1,10 +1,11 @@ -from jinja2 import Template -import os import enum import functools -import yaml +import os from typing import Dict +from jinja2 import Template +import yaml + def load_yaml(file_path: str) -> Dict: with open(file_path, "r") as stream: @@ -14,6 +15,7 @@ def load_yaml(file_path: str) -> Dict: print(exc) return None + class Transformation: def __init__(self, transformation: Dict, base_frame: str, child_frame: str): try: @@ -38,7 +40,7 @@ def serialize_single(self, key: str) -> str: def serialize(self) -> str: return f""" name=\"{self.name}\" - parant=\"{self.base_frame}\" + parent=\"{self.base_frame}\" x=\"{self.serialize_single('x')}\" y=\"{self.serialize_single('y')}\" z=\"{self.serialize_single('z')}\" @@ -51,9 +53,7 @@ def serialize(self) -> str: class Calibration: def __init__(self, calibration: Dict): self.base_dict: Dict = calibration - assert ( - len(calibration.keys()) == 1 - ), "Calibration file should have only one base frame" + assert len(calibration.keys()) == 1, "Calibration file should have only one base frame" assert isinstance( list(calibration.keys())[0], str ), "Calibration file should have only one base frame with key as a string" @@ -63,7 +63,7 @@ def __init__(self, calibration: Dict): calibration[self.base_frame], dict ), "Calibration file should have only one base frame with value as a dictionary" - self.transforms: Dict[str, Transformation] = dict() + self.transforms: Dict[str, Transformation] = {} for key in calibration[self.base_frame]: assert isinstance(key, str), "child frames should be strings" @@ -77,9 +77,8 @@ def __init__(self, calibration: Dict): class LinkType(enum.Enum): - """ - Enum class for the type of the link - """ + """Enum class for the type of the link.""" + CAMERA = "monocular_camera" IMU = "imu" LIVOX = "livox_horizon" @@ -93,44 +92,46 @@ class LinkType(enum.Enum): RADAR = "radar" JOINT_UNITS = "units" -def determine_link_type(link_name:str)->LinkType: - if 'cam' in link_name: + +def determine_link_type(link_name: str) -> LinkType: + if "cam" in link_name: return LinkType.CAMERA - - if 'imu' in link_name or 'gnss' in link_name: + + if "imu" in link_name or "gnss" in link_name: return LinkType.IMU - - if 'livox' in link_name: + + if "livox" in link_name: return LinkType.LIVOX - - if 'velodyne' in link_name: - if 'top' in link_name: + + if "velodyne" in link_name: + if "top" in link_name: return LinkType.VLS128 else: return LinkType.VELODYNE16 - - if 'radar' in link_name or 'ars' in link_name: + + if "radar" in link_name or "ars" in link_name: return LinkType.RADAR - - if 'pandar_40p' in link_name: + + if "pandar_40p" in link_name: return LinkType.PANDAR_40P - - if 'pandar_qt' in link_name: + + if "pandar_qt" in link_name: return LinkType.PANDAR_QT - - if 'hesai_top' in link_name: + + if "hesai_top" in link_name: return LinkType.PANDAR_OT128 - - if 'hesai_front' in link_name: + + if "hesai_front" in link_name: return LinkType.PANDAR_XT32 - if 'hesai' in link_name: + if "hesai" in link_name: return LinkType.PANDAR_XT32 - + else: print(f"Link type not found for {link_name}, suspected to be a joint unit") return LinkType.JOINT_UNITS + BASE_STRING = """LinkType: /> """ -def base_string_func(type:str, transform:Transformation)->str: - if type == "monocular_camera_macro": + +def base_string_func(macro_type: str, transform: Transformation) -> str: + if macro_type == "monocular_camera_macro": extra = """fps=\"30\" width=\"800\" height=\"400\" namespace=\"\" fov=\"1.3\"""" - elif type == "imu_macro": + elif macro_type == "imu_macro": extra = """fps=\"100\" namespace=\"\"""" else: extra = "" return BASE_STRING.format( - type=type, base_frame=transform.base_frame, child_frame=transform.child_frame, - x=transform.serialize_single('x'), y=transform.serialize_single('y'), z=transform.serialize_single('z'), - roll=transform.serialize_single('roll'), pitch=transform.serialize_single('pitch'), yaw=transform.serialize_single('yaw'), - extra=extra) + type=macro_type, + base_frame=transform.base_frame, + child_frame=transform.child_frame, + x=transform.serialize_single("x"), + y=transform.serialize_single("y"), + z=transform.serialize_single("z"), + roll=transform.serialize_single("roll"), + pitch=transform.serialize_single("pitch"), + yaw=transform.serialize_single("yaw"), + extra=extra, + ) + -def VLP16_func(transform:Transformation)->str: +def VLP16_func(transform: Transformation) -> str: return VLD_STRING.format( - type="VLP-16", base_frame=transform.base_frame, child_frame=transform.child_frame, - x=transform.serialize_single('x'), y=transform.serialize_single('y'), z=transform.serialize_single('z'), - roll=transform.serialize_single('roll'), pitch=transform.serialize_single('pitch'), yaw=transform.serialize_single('yaw') + type="VLP-16", + base_frame=transform.base_frame, + child_frame=transform.child_frame, + x=transform.serialize_single("x"), + y=transform.serialize_single("y"), + z=transform.serialize_single("z"), + roll=transform.serialize_single("roll"), + pitch=transform.serialize_single("pitch"), + yaw=transform.serialize_single("yaw"), ) -def VLS128_func(transform:Transformation)->str: + +def VLS128_func(transform: Transformation) -> str: return VLD_STRING.format( - type="VLS-128", base_frame=transform.base_frame, child_frame=transform.child_frame, - x=transform.serialize_single('x'), y=transform.serialize_single('y'), z=transform.serialize_single('z'), - roll=transform.serialize_single('roll'), pitch=transform.serialize_single('pitch'), yaw=transform.serialize_single('yaw') + type="VLS-128", + base_frame=transform.base_frame, + child_frame=transform.child_frame, + x=transform.serialize_single("x"), + y=transform.serialize_single("y"), + z=transform.serialize_single("z"), + roll=transform.serialize_single("roll"), + pitch=transform.serialize_single("pitch"), + yaw=transform.serialize_single("yaw"), ) + link_dicts = { - LinkType.CAMERA:{ + LinkType.CAMERA: { "including_file": "$(find camera_description)/urdf/monocular_camera.xacro", - "string_api": functools.partial(base_string_func, "monocular_camera_macro") + "string_api": functools.partial(base_string_func, "monocular_camera_macro"), }, - LinkType.IMU:{ + LinkType.IMU: { "including_file": "$(find imu_description)/urdf/imu.xacro", - "string_api": functools.partial(base_string_func, "imu_macro") + "string_api": functools.partial(base_string_func, "imu_macro"), }, - LinkType.VELODYNE16:{ + LinkType.VELODYNE16: { "including_file": "$(find velodyne_description)/urdf/VLP-16.urdf.xacro", - "string_api": VLP16_func + "string_api": VLP16_func, }, - LinkType.VLS128:{ + LinkType.VLS128: { "including_file": "$(find vls_description)/urdf/VLS-128.urdf.xacro", - "string_api": VLS128_func + "string_api": VLS128_func, }, - LinkType.PANDAR_40P:{ + LinkType.PANDAR_40P: { "including_file": "$(find pandar_description)/urdf/pandar_40p.xacro", - "string_api": functools.partial(base_string_func, "Pandar40P") + "string_api": functools.partial(base_string_func, "Pandar40P"), }, - LinkType.PANDAR_OT128:{ + LinkType.PANDAR_OT128: { "including_file": "$(find pandar_description)/urdf/pandar_ot128.xacro", - "string_api": functools.partial(base_string_func, "PandarOT-128") + "string_api": functools.partial(base_string_func, "PandarOT-128"), }, - LinkType.PANDAR_XT32:{ + LinkType.PANDAR_XT32: { "including_file": "$(find pandar_description)/urdf/pandar_xt32.xacro", - "string_api": functools.partial(base_string_func, "PandarXT-32") + "string_api": functools.partial(base_string_func, "PandarXT-32"), }, - LinkType.PANDAR_QT:{ + LinkType.PANDAR_QT: { "including_file": "$(find pandar_description)/urdf/pandar_qt.xacro", - "string_api": functools.partial(base_string_func, "PandarQT") + "string_api": functools.partial(base_string_func, "PandarQT"), }, - LinkType.PANDAR_QT128:{ + LinkType.PANDAR_QT128: { "including_file": "$(find pandar_description)/urdf/pandar_qt128.xacro", - "string_api": functools.partial(base_string_func, "PandarQT-128") + "string_api": functools.partial(base_string_func, "PandarQT-128"), }, - LinkType.LIVOX:{ + LinkType.LIVOX: { "including_file": "$(find livox_description)/urdf/livox_horizon.xacro", - "string_api": functools.partial(base_string_func, "livox_horizon_macro") + "string_api": functools.partial(base_string_func, "livox_horizon_macro"), }, - LinkType.RADAR:{ + LinkType.RADAR: { "including_file": "$(find radar_description)/urdf/radar.xacro", - "string_api": functools.partial(base_string_func, "radar_macro") + "string_api": functools.partial(base_string_func, "radar_macro"), }, - LinkType.JOINT_UNITS:{ + LinkType.JOINT_UNITS: { "including_file": "{filename}.xacro", - } + }, } -def main(template_directory:str, calibration_directory:str, output_directory:str, project_name:str): +def main( + template_directory: str, calibration_directory: str, output_directory: str, project_name: str +): os.makedirs(output_directory, exist_ok=True) # Load the template - with open(os.path.join(template_directory, 'sensors.xacro.template'), 'r') as file: + with open(os.path.join(template_directory, "sensors.xacro.template"), "r") as file: base_template = Template(file.read()) # Render the template @@ -248,32 +274,35 @@ def main(template_directory:str, calibration_directory:str, output_directory:str calib_yaml = load_yaml(calibration_path) calib = Calibration(calib_yaml) - render_meta_data = dict() - render_meta_data['default_config_path'] = f"$(find {project_name})/config" - render_meta_data["sensor_calibration_yaml_path"] = f"$(find {project_name})/config/sensors_calibration.yaml" + render_meta_data = {} + render_meta_data["default_config_path"] = f"$(find {project_name})/config" + render_meta_data[ + "sensor_calibration_yaml_path" + ] = f"$(find {project_name})/config/sensors_calibration.yaml" render_meta_data["sensor_units_includes"] = [] render_meta_data["sensor_units"] = [] render_meta_data["isolated_sensors_includes"] = [] render_meta_data["isolated_sensors"] = [] - include_text = set() sensor_items = [] for _, transform in calib.transforms.items(): - link_type:LinkType = determine_link_type(transform.child_frame) + link_type: LinkType = determine_link_type(transform.child_frame) if link_type == LinkType.JOINT_UNITS: - render_meta_data["sensor_units_includes"].append(link_dicts[link_type]['including_file'].format(filename=transform.name)) + render_meta_data["sensor_units_includes"].append( + link_dicts[link_type]["including_file"].format(filename=transform.name) + ) render_meta_data["sensor_units"].append( - dict( - base_frame=transform.base_frame, - child_frame=transform.child_frame, - macro_name=f"{transform.name}_macro", - name = transform.name - ) + { + "base_frame": transform.base_frame, + "child_frame": transform.child_frame, + "macro_name": f"{transform.name}_macro", + "name": transform.name, + } ) else: - include_text.add(link_dicts[link_type]['including_file']) - sensor_items.append(link_dicts[link_type]['string_api'](transform)) + include_text.add(link_dicts[link_type]["including_file"]) + sensor_items.append(link_dicts[link_type]["string_api"](transform)) render_meta_data["isolated_sensors_includes"] = list(include_text) render_meta_data["isolated_sensors"] = sensor_items @@ -283,39 +312,43 @@ def main(template_directory:str, calibration_directory:str, output_directory:str print("=====================================") # Save the rendered template - with open(os.path.join(output_directory, 'sensors.xacro'), 'w') as file: + with open(os.path.join(output_directory, "sensors.xacro"), "w") as file: file.write(rendered) - - ## Write Sensor Units into separate files - with open(os.path.join(template_directory, 'sensor_unit.xacro.template'), 'r') as file: + # Write Sensor Units into separate files + with open(os.path.join(template_directory, "sensor_unit.xacro.template"), "r") as file: sensor_units_template = Template(file.read()) for i, sensor_unit in enumerate(render_meta_data["sensor_units"]): - sensor_unit_calib_path = os.path.join(calibration_directory, f"{sensor_unit['name']}_calibration.yaml") + sensor_unit_calib_path = os.path.join( + calibration_directory, f"{sensor_unit['name']}_calibration.yaml" + ) sensor_unit_calib_yaml = load_yaml(sensor_unit_calib_path) sensor_unit_calib = Calibration(sensor_unit_calib_yaml) - sensor_unit_render_meta_data = dict() - sensor_unit_render_meta_data['unit_macro_name'] = sensor_unit['macro_name'] - sensor_unit_render_meta_data['default_config_path'] = render_meta_data['default_config_path'] + sensor_unit_render_meta_data = {} + sensor_unit_render_meta_data["unit_macro_name"] = sensor_unit["macro_name"] + sensor_unit_render_meta_data["default_config_path"] = render_meta_data[ + "default_config_path" + ] - sensor_unit_render_meta_data['current_base_link'] = sensor_unit_calib.base_frame + sensor_unit_render_meta_data["current_base_link"] = sensor_unit_calib.base_frame sensor_unit_isolated_sensors = [] for _, transform in sensor_unit_calib.transforms.items(): - link_type:LinkType = determine_link_type(transform.child_frame) - include_text.add(link_dicts[link_type]['including_file']) - sensor_unit_isolated_sensors.append(link_dicts[link_type]['string_api'](transform)) + link_type: LinkType = determine_link_type(transform.child_frame) + include_text.add(link_dicts[link_type]["including_file"]) + sensor_unit_isolated_sensors.append(link_dicts[link_type]["string_api"](transform)) sensor_unit_render_meta_data["isolated_sensors_includes"] = list(include_text) sensor_unit_render_meta_data["isolated_sensors"] = sensor_unit_isolated_sensors rendered = sensor_units_template.render(sensor_unit_render_meta_data) print(rendered) - with open(os.path.join(output_directory, f'{sensor_unit["name"]}.xacro'), 'w') as file: + with open(os.path.join(output_directory, f'{sensor_unit["name"]}.xacro'), "w") as file: file.write(rendered) print("=====================================") - + return 0 + if __name__ == "__main__": # import argparse # parser = argparse.ArgumentParser(description="Compile xacro files from calibration files") @@ -325,5 +358,6 @@ def main(template_directory:str, calibration_directory:str, output_directory:str # parser.add_argument("--project_name", type=str, help="Name of the project", required=True) # args = parser.parse_args() from fire import Fire + Fire(main) - # main(args.template_directory, args.calibration_directory, args.output_directory, args.project_name) \ No newline at end of file + # main(args.template_directory, args.calibration_directory, args.output_directory, args.project_name) From 80a68ef8b34b6134906a5f80d6dd5d5c97f7b22f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Jul 2024 05:48:51 +0000 Subject: [PATCH 04/10] ci(pre-commit): autofix --- aip_xx1_description/templates/sensor_unit.xacro.template | 2 +- aip_xx1_description/urdf/.gitignore | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_xx1_description/templates/sensor_unit.xacro.template b/aip_xx1_description/templates/sensor_unit.xacro.template index 9dd919d1..b0b639ff 100644 --- a/aip_xx1_description/templates/sensor_unit.xacro.template +++ b/aip_xx1_description/templates/sensor_unit.xacro.template @@ -28,4 +28,4 @@ {% endfor %} - \ No newline at end of file + diff --git a/aip_xx1_description/urdf/.gitignore b/aip_xx1_description/urdf/.gitignore index 05ca38bc..e1e98315 100644 --- a/aip_xx1_description/urdf/.gitignore +++ b/aip_xx1_description/urdf/.gitignore @@ -1 +1 @@ -*.xacro \ No newline at end of file +*.xacro From d46655de022faee221da33577209a6a74eebab5e Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 30 Jul 2024 14:50:21 +0900 Subject: [PATCH 05/10] fix pre-commit Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/scripts/compile_xacro.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_xx1_description/scripts/compile_xacro.py b/aip_xx1_description/scripts/compile_xacro.py index 90473b31..a66c27e8 100644 --- a/aip_xx1_description/scripts/compile_xacro.py +++ b/aip_xx1_description/scripts/compile_xacro.py @@ -31,7 +31,7 @@ def __init__(self, transformation: Dict, base_frame: str, child_frame: str): self.name = self.child_frame.replace("_base_link", "").replace("_link", "") except KeyError as e: - print(f"Error: Key {e} not found in transformation dictionary") + print(f"Error: Key {e} not in transformation dictionary") raise e def serialize_single(self, key: str) -> str: @@ -72,7 +72,7 @@ def __init__(self, calibration: Dict): calibration[self.base_frame][key], self.base_frame, key ) except KeyError as e: - print(f"Error: Key {e} not found in calibration dictionary of {key}") + print(f"Error: Key {e} not in calibration dictionary of {key}") raise e From 7a9652801f31fd0300646d8be9c03521b2ed3983 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 30 Jul 2024 14:55:08 +0900 Subject: [PATCH 06/10] add python3-jinja2 dependency Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index b35b48e0..da7edec8 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -8,6 +8,7 @@ Apache 2 ament_cmake_auto + python3-jinja2 velodyne_description From 629066e4b39ce3ffda70cd574ff2eb1cba280ca7 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 30 Jul 2024 15:02:37 +0900 Subject: [PATCH 07/10] add python3-fire dependency Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index da7edec8..08eac6aa 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto python3-jinja2 + python3-fire velodyne_description From 356de998f81a7abac60bddbb48c969aa81ce65a1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Jul 2024 06:02:48 +0000 Subject: [PATCH 08/10] ci(pre-commit): autofix --- aip_xx1_description/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index 08eac6aa..aa02c70c 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -8,8 +8,8 @@ Apache 2 ament_cmake_auto - python3-jinja2 python3-fire + python3-jinja2 velodyne_description From 1b86e837a369b048979d679cee13af666872f425 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 30 Jul 2024 15:08:59 +0900 Subject: [PATCH 09/10] use argparse instead of fire for launch Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/package.xml | 1 - aip_xx1_description/scripts/compile_xacro.py | 24 +++++++++++--------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index aa02c70c..da7edec8 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -8,7 +8,6 @@ Apache 2 ament_cmake_auto - python3-fire python3-jinja2 velodyne_description diff --git a/aip_xx1_description/scripts/compile_xacro.py b/aip_xx1_description/scripts/compile_xacro.py index a66c27e8..43dacd13 100644 --- a/aip_xx1_description/scripts/compile_xacro.py +++ b/aip_xx1_description/scripts/compile_xacro.py @@ -350,14 +350,16 @@ def main( if __name__ == "__main__": - # import argparse - # parser = argparse.ArgumentParser(description="Compile xacro files from calibration files") - # parser.add_argument("--template_directory", type=str, help="Path to the template directory", required=True) - # parser.add_argument("--calibration_directory", type=str, help="Path to the calibration directory", required=True) - # parser.add_argument("--output_directory", type=str, help="Path to the output directory", required=True) - # parser.add_argument("--project_name", type=str, help="Name of the project", required=True) - # args = parser.parse_args() - from fire import Fire - - Fire(main) - # main(args.template_directory, args.calibration_directory, args.output_directory, args.project_name) + import argparse + parser = argparse.ArgumentParser(description="Process four positional arguments.") + + # Add four positional arguments + parser.add_argument('template_directory', type=str, help='The first argument') + parser.add_argument('calibration_directory', type=str, help='The second argument') + parser.add_argument('output_directory', type=str, help='The third argument') + parser.add_argument('project_name', type=str, help='The fourth argument') + + # Parse the arguments + args = parser.parse_args() + + main(args.template_directory, args.calibration_directory, args.output_directory, args.project_name) From 2600a5f3c408b062c40d1a5d9d4a3cd26b94cc02 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Jul 2024 06:09:10 +0000 Subject: [PATCH 10/10] ci(pre-commit): autofix --- aip_xx1_description/scripts/compile_xacro.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/aip_xx1_description/scripts/compile_xacro.py b/aip_xx1_description/scripts/compile_xacro.py index 43dacd13..0128e6ac 100644 --- a/aip_xx1_description/scripts/compile_xacro.py +++ b/aip_xx1_description/scripts/compile_xacro.py @@ -351,15 +351,21 @@ def main( if __name__ == "__main__": import argparse + parser = argparse.ArgumentParser(description="Process four positional arguments.") - + # Add four positional arguments - parser.add_argument('template_directory', type=str, help='The first argument') - parser.add_argument('calibration_directory', type=str, help='The second argument') - parser.add_argument('output_directory', type=str, help='The third argument') - parser.add_argument('project_name', type=str, help='The fourth argument') + parser.add_argument("template_directory", type=str, help="The first argument") + parser.add_argument("calibration_directory", type=str, help="The second argument") + parser.add_argument("output_directory", type=str, help="The third argument") + parser.add_argument("project_name", type=str, help="The fourth argument") # Parse the arguments args = parser.parse_args() - main(args.template_directory, args.calibration_directory, args.output_directory, args.project_name) + main( + args.template_directory, + args.calibration_directory, + args.output_directory, + args.project_name, + )