From 2723baac2fd6ff52791e7549a80ba5efc847774e Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Thu, 19 Dec 2024 15:48:37 +0900 Subject: [PATCH 01/20] aip launch fix for XX1 Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/CMakeLists.txt | 13 + aip_urdf_compiler/package.xml | 17 + aip_urdf_compiler/scripts/compile_xacro.py | 384 ++++++++++++++++++ .../templates/sensor_unit.xacro.template | 31 ++ .../templates/sensors.xacro.template | 31 ++ aip_xx1_description/CMakeLists.txt | 32 ++ .../config/sensor_kit_calibration.yaml | 13 + .../config/sensors_calibration.yaml | 5 + aip_xx1_description/package.xml | 4 +- aip_xx1_description/urdf/.gitignore | 1 + aip_xx1_description/urdf/sensor_kit.xacro | 228 ----------- aip_xx1_description/urdf/sensors.xacro | 72 ---- 12 files changed, 530 insertions(+), 301 deletions(-) create mode 100644 aip_urdf_compiler/CMakeLists.txt create mode 100644 aip_urdf_compiler/package.xml create mode 100644 aip_urdf_compiler/scripts/compile_xacro.py create mode 100644 aip_urdf_compiler/templates/sensor_unit.xacro.template create mode 100644 aip_urdf_compiler/templates/sensors.xacro.template create mode 100644 aip_xx1_description/urdf/.gitignore delete mode 100644 aip_xx1_description/urdf/sensor_kit.xacro delete mode 100644 aip_xx1_description/urdf/sensors.xacro diff --git a/aip_urdf_compiler/CMakeLists.txt b/aip_urdf_compiler/CMakeLists.txt new file mode 100644 index 00000000..131f6df4 --- /dev/null +++ b/aip_urdf_compiler/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_urdf_compiler) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +# Export the package's share directory path +set(aip_urdf_compiler_SHARE_DIR "share/${PROJECT_NAME}") +ament_auto_package(INSTALL_TO_SHARE + templates + scripts +) diff --git a/aip_urdf_compiler/package.xml b/aip_urdf_compiler/package.xml new file mode 100644 index 00000000..ff8aab61 --- /dev/null +++ b/aip_urdf_compiler/package.xml @@ -0,0 +1,17 @@ + + + aip_urdf_compiler + 0.1.0 + The aip_urdf_compiler package + + Yukihiro Saito + Yuxuan Liu + Apache 2 + + ament_cmake_auto + + + + ament_cmake + + diff --git a/aip_urdf_compiler/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_xacro.py new file mode 100644 index 00000000..9936e4c5 --- /dev/null +++ b/aip_urdf_compiler/scripts/compile_xacro.py @@ -0,0 +1,384 @@ +import enum +import functools +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: + 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.type: str = transformation.get("type", "") + + self.name = self.child_frame.replace("_base_link", "").replace("_link", "") + + except KeyError as e: + print(f"Error: Key {e} not 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}\" + parent=\"{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] = {} + + 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 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 obtain_link_type(link: Transformation) -> LinkType: + if len(link.type) > 0: + # use explicit type string to obtain link + link_type_lower = link.type.lower() + + # Check each enum value for a match + for type_enum in LinkType: + if link_type_lower == type_enum.value.lower(): + return type_enum + # if there is no match, or the type is not defined: + return determine_link_type(link.child_frame) + + +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(macro_type: str, transform: Transformation) -> str: + if macro_type == "monocular_camera_macro": + extra = """fps=\"30\" + width=\"800\" + height=\"400\" + namespace=\"\" + fov=\"1.3\"""" + elif macro_type == "imu_macro": + extra = """fps=\"100\" + namespace=\"\"""" + else: + extra = "" + return BASE_STRING.format( + 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: + 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.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()) + + # 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 = {} + render_meta_data["default_config_path"] = f"$(find {project_name})/config" + render_meta_data["sensor_calibration_yaml_path"] = "$(arg config_dir)/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 = obtain_link_type(transform) + 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( + { + "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 = {} + 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["joint_unit_name"] = sensor_unit["name"] + 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 = obtain_link_type(transform) + include_text.add(link_dicts[link_type]["including_file"]) + print(transform.child_frame) + 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="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, + ) diff --git a/aip_urdf_compiler/templates/sensor_unit.xacro.template b/aip_urdf_compiler/templates/sensor_unit.xacro.template new file mode 100644 index 00000000..f0b44710 --- /dev/null +++ b/aip_urdf_compiler/templates/sensor_unit.xacro.template @@ -0,0 +1,31 @@ + + + + {% for item in isolated_sensors_includes %} + + {% endfor %} + + + + + + + + + + + + + + + + + + + + {% for item in isolated_sensors %} + {{ item }} + {% endfor %} + + + diff --git a/aip_urdf_compiler/templates/sensors.xacro.template b/aip_urdf_compiler/templates/sensors.xacro.template new file mode 100644 index 00000000..318e48bd --- /dev/null +++ b/aip_urdf_compiler/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/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index 50723262..e703ec94 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -2,9 +2,41 @@ cmake_minimum_required(VERSION 3.5) project(aip_xx1_description) find_package(ament_cmake_auto REQUIRED) +find_package(aip_urdf_compiler REQUIRED) ament_auto_find_build_dependencies() +find_package(PythonInterp REQUIRED) # cspell: ignore Interp + +# Get the package share directory +set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") +# Set the correct paths +set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") +set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") +set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") +set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") + +message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") +message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") + +# Verify that the required files exist +if(NOT EXISTS "${PYTHON_SCRIPT}") + message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") +endif() + +# Create a custom command to run the Python script +add_custom_target(xacro_compilation ALL + DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag +) + +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag + COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DEPENDS ${PYTHON_SCRIPT} + COMMENT "Running Python script for URDF creation" +) + ament_auto_package(INSTALL_TO_SHARE urdf config diff --git a/aip_xx1_description/config/sensor_kit_calibration.yaml b/aip_xx1_description/config/sensor_kit_calibration.yaml index ab417b52..db7de0ff 100644 --- a/aip_xx1_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_description/config/sensor_kit_calibration.yaml @@ -6,6 +6,7 @@ sensor_kit_base_link: roll: -0.025 pitch: 0.315 yaw: 1.035 + type: monocular_camera camera1/camera_link: x: -0.10731 y: -0.56343 @@ -13,6 +14,7 @@ sensor_kit_base_link: roll: -0.025 pitch: 0.32 yaw: -2.12 + type: monocular_camera camera2/camera_link: x: 0.10731 y: -0.56343 @@ -20,6 +22,7 @@ sensor_kit_base_link: roll: -0.00 pitch: 0.335 yaw: -1.04 + type: monocular_camera camera3/camera_link: x: -0.10731 y: 0.56343 @@ -27,6 +30,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.325 yaw: 2.0943951 + type: monocular_camera camera4/camera_link: x: 0.07356 y: 0.0 @@ -34,6 +38,7 @@ sensor_kit_base_link: roll: 0.0 pitch: -0.03 yaw: -0.005 + type: monocular_camera camera5/camera_link: x: -0.07356 y: 0.0 @@ -41,6 +46,7 @@ sensor_kit_base_link: roll: 0.0 pitch: -0.01 yaw: 3.125 + type: monocular_camera camera6/camera_link: x: 0.05 y: 0.0175 @@ -48,6 +54,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: monocular_camera camera7/camera_link: x: 0.05 y: -0.0175 @@ -55,6 +62,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: monocular_camera velodyne_top_base_link: x: 0.0 y: 0.0 @@ -62,6 +70,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 1.575 + type: VLS-128.urdf velodyne_left_base_link: x: 0.0 y: 0.56362 @@ -69,6 +78,7 @@ sensor_kit_base_link: roll: -0.02 pitch: 0.71 yaw: 1.575 + type: VLP-16.urdf velodyne_right_base_link: x: 0.0 y: -0.56362 @@ -76,6 +86,7 @@ sensor_kit_base_link: roll: -0.01 pitch: 0.71 yaw: -1.580 + type: VLP-16.urdf gnss_link: x: -0.1 y: 0.0 @@ -83,6 +94,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: imu tamagawa/imu_link: x: 0.0 y: 0.0 @@ -90,3 +102,4 @@ sensor_kit_base_link: roll: 3.14159265359 pitch: 0.0 yaw: 3.14159265359 + type: imu diff --git a/aip_xx1_description/config/sensors_calibration.yaml b/aip_xx1_description/config/sensors_calibration.yaml index e8c4b75e..c7de4df6 100644 --- a/aip_xx1_description/config/sensors_calibration.yaml +++ b/aip_xx1_description/config/sensors_calibration.yaml @@ -6,6 +6,7 @@ base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: radar sensor_kit_base_link: x: 0.9 y: 0.0 @@ -13,6 +14,7 @@ base_link: roll: -0.001 pitch: 0.015 yaw: -0.0364 + type: units livox_front_right_base_link: x: 3.290 y: -0.65485 @@ -20,6 +22,7 @@ base_link: roll: 0.0 pitch: 0.0 yaw: -0.872664444 + type: livox_horizon livox_front_left_base_link: x: 3.290 y: 0.65485 @@ -27,6 +30,7 @@ base_link: roll: -0.021 pitch: 0.05 yaw: 0.872664444 + type: livox_horizon velodyne_rear_base_link: x: -0.358 y: 0.0 @@ -34,3 +38,4 @@ base_link: roll: -0.02 pitch: 0.7281317 yaw: 3.141592 + type: VLP-16.urdf diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index b35b48e0..8225d75b 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -8,8 +8,10 @@ Apache 2 ament_cmake_auto - + ament_cmake + velodyne_description + aip_urdf_compiler ament_cmake diff --git a/aip_xx1_description/urdf/.gitignore b/aip_xx1_description/urdf/.gitignore new file mode 100644 index 00000000..e1e98315 --- /dev/null +++ b/aip_xx1_description/urdf/.gitignore @@ -0,0 +1 @@ +*.xacro 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 17eb5471b0a2a09454203ad8bdb33536c255147b Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Thu, 19 Dec 2024 15:54:54 +0900 Subject: [PATCH 02/20] aip_xx1_gen2 example Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_gen2_description/CMakeLists.txt | 32 +++ .../config/sensor_kit_calibration.yaml | 16 ++ .../config/sensors_calibration.yaml | 9 + aip_xx1_gen2_description/package.xml | 1 + aip_xx1_gen2_description/urdf/.gitignore | 1 + .../urdf/sensor_kit.xacro | 253 ------------------ aip_xx1_gen2_description/urdf/sensors.xacro | 103 ------- 7 files changed, 59 insertions(+), 356 deletions(-) create mode 100644 aip_xx1_gen2_description/urdf/.gitignore delete mode 100644 aip_xx1_gen2_description/urdf/sensor_kit.xacro delete mode 100644 aip_xx1_gen2_description/urdf/sensors.xacro diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt index 549de0f8..f7e38a63 100644 --- a/aip_xx1_gen2_description/CMakeLists.txt +++ b/aip_xx1_gen2_description/CMakeLists.txt @@ -2,9 +2,41 @@ cmake_minimum_required(VERSION 3.5) project(aip_xx1_gen2_description) find_package(ament_cmake_auto REQUIRED) +find_package(aip_urdf_compiler REQUIRED) ament_auto_find_build_dependencies() +find_package(PythonInterp REQUIRED) # cspell: ignore Interp + +# Get the package share directory +set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") +# Set the correct paths +set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") +set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") +set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") +set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") + +message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") +message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") + +# Verify that the required files exist +if(NOT EXISTS "${PYTHON_SCRIPT}") + message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") +endif() + +# Create a custom command to run the Python script +add_custom_target(xacro_compilation ALL + DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag +) + +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag + COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DEPENDS ${PYTHON_SCRIPT} + COMMENT "Running Python script for URDF creation" +) + ament_auto_package(INSTALL_TO_SHARE urdf config diff --git a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml index 88288533..a0d91928 100644 --- a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml @@ -6,6 +6,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: monocular_camera camera1/camera_link: x: 0.372 # Design Value y: 0.045 # Design Value @@ -13,6 +14,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: monocular_camera camera2/camera_link: x: 0.372 # Design Value y: -0.045 # Design Value @@ -20,6 +22,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: monocular_camera camera3/camera_link: x: 0.133 # Design Value y: 0.498 # Design Value @@ -27,6 +30,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.872665 # Design Value + type: monocular_camera camera4/camera_link: x: 0.133 # Design Value y: -0.498 # Design Value @@ -34,6 +38,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -0.872665 # Design Value + type: monocular_camera camera5/camera_link: x: 0.095 # Design Value y: 0.524 # Design Value @@ -41,6 +46,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 1.0472 # Design Value + type: monocular_camera camera6/camera_link: x: 0.095 # Design Value y: -0.524 # Design Value @@ -48,6 +54,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -1.0472 # Design Value + type: monocular_camera camera7/camera_link: x: -0.345 # Design Value y: 0.244 # Design Value @@ -55,6 +62,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.70526 # Design Value + type: monocular_camera camera8/camera_link: x: -0.345 # Design Value y: -0.244 # Design Value @@ -62,6 +70,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -2.70526 # Design Value + type: monocular_camera camera9/camera_link: x: -0.362 # Design Value y: 0.202 # Design Value @@ -69,6 +78,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.79253 # Design Value + type: monocular_camera camera10/camera_link: x: -0.362 # Design Value y: -0.202 # Design Value @@ -76,6 +86,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -2.79253 # Design Value + type: monocular_camera hesai_top_base_link: x: 0.0 # Design Value y: 0.0 # Design Value @@ -83,6 +94,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 4.36332298038 # Design Value + type: pandar_ot128 hesai_left_base_link: x: 0.0 # Design Value y: 0.564 # Design Value @@ -90,6 +102,7 @@ sensor_kit_base_link: roll: 0.872665 # Design Value pitch: 0.0 # Design Value yaw: 3.14159265359 # Design Value + type: pandar_xt32 hesai_right_base_link: x: 0.0 # Design Value y: -0.564 # Design Value @@ -97,6 +110,7 @@ sensor_kit_base_link: roll: 0.69813132679 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: pandar_xt32 gnss_link: x: -0.279 # Design Value y: 0.0 # Design Value @@ -104,6 +118,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: imu tamagawa/imu_link: x: -0.129 # Design Value y: 0.0 # Design Value @@ -111,3 +126,4 @@ sensor_kit_base_link: roll: 3.14159265359 pitch: 0.0 # Design Value yaw: 3.14159265359 # Design Value + type: imu diff --git a/aip_xx1_gen2_description/config/sensors_calibration.yaml b/aip_xx1_gen2_description/config/sensors_calibration.yaml index a57d3ea9..4fb70f70 100644 --- a/aip_xx1_gen2_description/config/sensors_calibration.yaml +++ b/aip_xx1_gen2_description/config/sensors_calibration.yaml @@ -6,6 +6,7 @@ base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: units hesai_front_left_base_link: x: 3.373 # Design Value y: 0.740 # Design Value @@ -13,6 +14,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.44346132679 # Design Value + type: pandar_xt32 hesai_front_right_base_link: x: 3.373 # Design Value y: -0.740 # Design Value @@ -20,6 +22,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.69813132679 # Design Value + type: pandar_xt32 # velodyne_rear_base_link: #unused # x: -0.358 # y: 0.0 @@ -34,6 +37,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: radar front_right/radar_link: x: 3.384 # Design Value y: -0.7775 # Design Value @@ -41,6 +45,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -1.22173 # Design Value + type: radar front_left/radar_link: x: 3.384 # Design Value y: 0.7775 # Design Value @@ -48,6 +53,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 1.22173 # Design Value + type: radar rear_center/radar_link: x: -0.858 # Design Value y: 0.0 # Design Value @@ -55,6 +61,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 3.141592 # Design Value + type: radar rear_right/radar_link: x: -0.782 # Design Value y: -0.761 # Design Value @@ -62,6 +69,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -2.0944 # Design Value + type: radar rear_left/radar_link: x: -0.782 # Design Value y: 0.761 # Design Value @@ -69,3 +77,4 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.0944 # Design Value + type: radar diff --git a/aip_xx1_gen2_description/package.xml b/aip_xx1_gen2_description/package.xml index 9b010d72..480e5307 100644 --- a/aip_xx1_gen2_description/package.xml +++ b/aip_xx1_gen2_description/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto velodyne_description + aip_urdf_compiler ament_cmake diff --git a/aip_xx1_gen2_description/urdf/.gitignore b/aip_xx1_gen2_description/urdf/.gitignore new file mode 100644 index 00000000..e1e98315 --- /dev/null +++ b/aip_xx1_gen2_description/urdf/.gitignore @@ -0,0 +1 @@ +*.xacro diff --git a/aip_xx1_gen2_description/urdf/sensor_kit.xacro b/aip_xx1_gen2_description/urdf/sensor_kit.xacro deleted file mode 100644 index 137b3589..00000000 --- a/aip_xx1_gen2_description/urdf/sensor_kit.xacro +++ /dev/null @@ -1,253 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_gen2_description/urdf/sensors.xacro b/aip_xx1_gen2_description/urdf/sensors.xacro deleted file mode 100644 index 79c2c15f..00000000 --- a/aip_xx1_gen2_description/urdf/sensors.xacro +++ /dev/null @@ -1,103 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - From 0b2d64f01f0cdd0f314dd2f5760eb926ff21b724 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Thu, 19 Dec 2024 19:07:45 +0900 Subject: [PATCH 03/20] clean up Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/CMakeLists.txt | 1 - aip_urdf_compiler/scripts/compile_xacro.py | 19 ++++++++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/aip_urdf_compiler/CMakeLists.txt b/aip_urdf_compiler/CMakeLists.txt index 131f6df4..24b99f08 100644 --- a/aip_urdf_compiler/CMakeLists.txt +++ b/aip_urdf_compiler/CMakeLists.txt @@ -6,7 +6,6 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() # Export the package's share directory path -set(aip_urdf_compiler_SHARE_DIR "share/${PROJECT_NAME}") ament_auto_package(INSTALL_TO_SHARE templates scripts diff --git a/aip_urdf_compiler/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_xacro.py index 9936e4c5..7a408c87 100644 --- a/aip_urdf_compiler/scripts/compile_xacro.py +++ b/aip_urdf_compiler/scripts/compile_xacro.py @@ -182,10 +182,16 @@ def base_string_func(macro_type: str, transform: Transformation) -> str: namespace=\"\"""" else: extra = "" + if "pandar" in transform.type or "livox" in transform.type or "camera" in transform.type: + # In commen sensor description, LiDAR and camera macros will automatically be attached with a "base_link" name. + # So we need to strip away + child_frame = transform.name.replace("_base_link", "").replace("_link", "") + else: + child_frame = transform.child_frame return BASE_STRING.format( type=macro_type, base_frame=transform.base_frame, - child_frame=transform.child_frame, + child_frame=child_frame, # pandar x=transform.serialize_single("x"), y=transform.serialize_single("y"), z=transform.serialize_single("z"), @@ -200,7 +206,7 @@ def VLP16_func(transform: Transformation) -> str: return VLD_STRING.format( type="VLP-16", base_frame=transform.base_frame, - child_frame=transform.child_frame, + child_frame=transform.name, x=transform.serialize_single("x"), y=transform.serialize_single("y"), z=transform.serialize_single("z"), @@ -214,7 +220,7 @@ def VLS128_func(transform: Transformation) -> str: return VLD_STRING.format( type="VLS-128", base_frame=transform.base_frame, - child_frame=transform.child_frame, + child_frame=transform.name, x=transform.serialize_single("x"), y=transform.serialize_single("y"), z=transform.serialize_single("z"), @@ -276,7 +282,10 @@ def VLS128_func(transform: Transformation) -> str: def main( - template_directory: str, calibration_directory: str, output_directory: str, project_name: str + template_directory: str, + calibration_directory: str, + output_directory: str, + project_name: str, ): os.makedirs(output_directory, exist_ok=True) # Load the template @@ -348,7 +357,7 @@ def main( for _, transform in sensor_unit_calib.transforms.items(): link_type: LinkType = obtain_link_type(transform) include_text.add(link_dicts[link_type]["including_file"]) - print(transform.child_frame) + print(transform.name) 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 From 5a9e75a18ebd40b6a837576f2400f1586b6f6879 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 12:12:00 +0900 Subject: [PATCH 04/20] feat(aip_urdf_compiler): significantly improve documentation; add additional script for comparing different xacro files for testing; add optional keywords for frame_id Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/scripts/compile_xacro.py | 111 ++++++++- aip_urdf_compiler/tests/xml_diff.py | 274 +++++++++++++++++++++ 2 files changed, 375 insertions(+), 10 deletions(-) create mode 100644 aip_urdf_compiler/tests/xml_diff.py diff --git a/aip_urdf_compiler/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_xacro.py index 7a408c87..a82a2f6e 100644 --- a/aip_urdf_compiler/scripts/compile_xacro.py +++ b/aip_urdf_compiler/scripts/compile_xacro.py @@ -1,13 +1,36 @@ +#!/usr/bin/python3 +""" +XACRO Compiler Script. + +This script compiles XACRO files for robot sensor configurations. It processes calibration data +from YAML files and generates XACRO files that define the sensor transforms and configurations +for a robot's URDF description. + +The script handles various types of sensors including cameras, IMUs, LiDARs (Velodyne, Pandar, Livox), +and radar units, generating appropriate XACRO macros for each sensor type. +""" + import enum import functools import os +from typing import Callable from typing import Dict +from typing import Union from jinja2 import Template import yaml def load_yaml(file_path: str) -> Dict: + """ + Load and parse a YAML file. + + Args: + file_path (str): Path to the YAML file + + Returns: + Dict: Parsed YAML content or None if parsing fails + """ with open(file_path, "r") as stream: try: return yaml.safe_load(stream) @@ -17,7 +40,25 @@ def load_yaml(file_path: str) -> Dict: class Transformation: + """ + Represents a coordinate transformation between two frames. + + Stores translation (x,y,z) and rotation (roll,pitch,yaw) parameters + along with frame information and sensor type. + """ + def __init__(self, transformation: Dict, base_frame: str, child_frame: str): + """ + Initialize a transformation from a dictionary of parameters. + + Args: + transformation (Dict): Dictionary containing transformation parameters + base_frame (str): Name of the parent/base frame + child_frame (str): Name of the child frame + + Raises: + KeyError: If required transformation parameters are missing + """ try: self.x = transformation["x"] self.y = transformation["y"] @@ -31,14 +72,47 @@ def __init__(self, transformation: Dict, base_frame: str, child_frame: str): self.name = self.child_frame.replace("_base_link", "").replace("_link", "") + if len(self.type) == 0: + self.type = determine_link_type(self.name) + + self.frame_id: str = transformation.get("frame_id", "") + if len(self.frame_id) == 0: + if ( + "pandar" in self.type + or "livox" in self.type + or "camera" in self.type + or "vls" in self.type.lower() + or "vlp" in self.type.lower() + ): + # For common sensor descriptions, LiDAR and camera macros will automatically + # be attached with a "base_link" name + self.frame_id = self.name + else: + self.frame_id = self.child_frame + except KeyError as e: print(f"Error: Key {e} not in transformation dictionary") raise e def serialize_single(self, key: str) -> str: + """ + Generate a serialized string for a single transformation parameter. + + Args: + key (str): Parameter key (x, y, z, roll, pitch, or yaw) + + Returns: + str: Serialized parameter string for use in XACRO template + """ return f"${{calibration['{self.base_frame}']['{self.child_frame}']['{key}']}}" def serialize(self) -> str: + """ + Generate a complete serialized string for all transformation parameters. + + Returns: + str: Complete serialized transformation string for XACRO template + """ return f""" name=\"{self.name}\" parent=\"{self.base_frame}\" @@ -52,7 +126,22 @@ def serialize(self) -> str: class Calibration: + """ + Represents a complete set of calibration data for all sensors. + + Contains transformations for all sensors relative to a single base frame. + """ + def __init__(self, calibration: Dict): + """ + Initialize calibration data from a dictionary. + + Args: + calibration (Dict): Dictionary containing calibration data + + Raises: + AssertionError: If calibration format is invalid + """ self.base_dict: Dict = calibration assert len(calibration.keys()) == 1, "Calibration file should have only one base frame" assert isinstance( @@ -95,6 +184,7 @@ class LinkType(enum.Enum): def obtain_link_type(link: Transformation) -> LinkType: + """Output the LinkType of the target link.""" if len(link.type) > 0: # use explicit type string to obtain link link_type_lower = link.type.lower() @@ -108,6 +198,7 @@ def obtain_link_type(link: Transformation) -> LinkType: def determine_link_type(link_name: str) -> LinkType: + """Produce a guess of the type of the link based on its name.""" if "cam" in link_name: return LinkType.CAMERA @@ -182,16 +273,10 @@ def base_string_func(macro_type: str, transform: Transformation) -> str: namespace=\"\"""" else: extra = "" - if "pandar" in transform.type or "livox" in transform.type or "camera" in transform.type: - # In commen sensor description, LiDAR and camera macros will automatically be attached with a "base_link" name. - # So we need to strip away - child_frame = transform.name.replace("_base_link", "").replace("_link", "") - else: - child_frame = transform.child_frame return BASE_STRING.format( type=macro_type, base_frame=transform.base_frame, - child_frame=child_frame, # pandar + child_frame=transform.frame_id, # pandar x=transform.serialize_single("x"), y=transform.serialize_single("y"), z=transform.serialize_single("z"), @@ -206,7 +291,7 @@ def VLP16_func(transform: Transformation) -> str: return VLD_STRING.format( type="VLP-16", base_frame=transform.base_frame, - child_frame=transform.name, + child_frame=transform.frame_id, x=transform.serialize_single("x"), y=transform.serialize_single("y"), z=transform.serialize_single("z"), @@ -220,7 +305,7 @@ def VLS128_func(transform: Transformation) -> str: return VLD_STRING.format( type="VLS-128", base_frame=transform.base_frame, - child_frame=transform.name, + child_frame=transform.frame_id, x=transform.serialize_single("x"), y=transform.serialize_single("y"), z=transform.serialize_single("z"), @@ -230,7 +315,13 @@ def VLS128_func(transform: Transformation) -> str: ) -link_dicts = { +""" +link_dicts maps the LinkType to its required include files and the template strings. +including_file is the path to the required sub module xacro +string_api is a function that outputs a template string from a transform +""" + +link_dicts: Dict[LinkType, Dict[str, Union[str, Callable[[Transformation], str]]]] = { LinkType.CAMERA: { "including_file": "$(find camera_description)/urdf/monocular_camera.xacro", "string_api": functools.partial(base_string_func, "monocular_camera_macro"), diff --git a/aip_urdf_compiler/tests/xml_diff.py b/aip_urdf_compiler/tests/xml_diff.py new file mode 100644 index 00000000..8dd92a34 --- /dev/null +++ b/aip_urdf_compiler/tests/xml_diff.py @@ -0,0 +1,274 @@ +#!/usr/bin/python3 +""" +XML/Xacro Difference Analyzer. + +This script analyzes and compares two XML/Xacro files, specifically designed for ROS URDF/Xacro files +containing sensor configurations. It identifies and reports differences in included files, sensor configurations, +and parameter changes between two versions of a file. + +The analyzer is particularly useful for: +- Tracking changes in sensor configurations +- Validating URDF/Xacro file modifications +- Documenting sensor setup modifications +- Quality assurance of robot configuration changes + +Author: [Your Name] +Date: [Current Date] +""" + +import argparse +from collections import OrderedDict +from typing import Dict +from typing import List +import xml.etree.ElementTree as ET + + +class XacroAnalyzer: + """ + A class to analyze differences between two Xacro/XML files. + + This class provides functionality to compare two Xacro files and identify changes + in includes, sensor configurations, and parameters. + + Attributes: + original_text (str): Content of the original file + new_text (str): Content of the new file + original_root (ET.Element): XML tree root of the original file + new_root (ET.Element): XML tree root of the new file + ns (dict): Namespace dictionary for Xacro XML parsing + """ + + def __init__(self, original_file: str, new_file: str): + """ + Initialize the XacroAnalyzer with two files to compare. + + Args: + original_file (str): Path to the original Xacro file + new_file (str): Path to the new Xacro file + """ + self.original_text = self._read_file(original_file) + self.new_text = self._read_file(new_file) + self.original_root = ET.fromstring(self.original_text) + self.new_root = ET.fromstring(self.new_text) + self.ns = {"xacro": "http://ros.org/wiki/xacro"} + + @staticmethod + def _read_file(filename: str) -> str: + """ + Read content from a file. + + Args: + filename (str): Path to the file to read + + Returns: + str: Content of the file + """ + with open(filename, "r", encoding="utf-8") as f: + return f.read() + + def _get_includes(self, root: ET.Element) -> List[str]: + """ + Extract all include statements from an XML root. + + Args: + root (ET.Element): XML root element to search + + Returns: + List[str]: Sorted list of included filenames + """ + includes = [] + for include in root.findall(".//xacro:include", self.ns): + includes.append(include.get("filename")) + return sorted(includes) + + def _sort_params(self, params: Dict) -> OrderedDict: + """ + Sort parameters in a standardized order with common parameters first. + + Args: + params (Dict): Dictionary of parameters to sort + + Returns: + OrderedDict: Sorted parameters with common parameters first + """ + common_params = ["name", "parent", "x", "y", "z", "roll", "pitch", "yaw"] + sorted_params = OrderedDict() + + # Add common params in specific order + for param in common_params: + if param in params: + sorted_params[param] = params[param] + + # Add remaining params alphabetically + for param in sorted(params.keys()): + if param not in common_params: + sorted_params[param] = params[param] + + return sorted_params + + def _get_sensors(self, root: ET.Element) -> Dict[str, List[Dict]]: + """ + Extract all sensor configurations from an XML root. + + Args: + root (ET.Element): XML root element to search + + Returns: + Dict[str, List[Dict]]: Dictionary of sensor types mapping to their configurations + """ + sensors = {"cameras": [], "lidars": [], "imus": [], "radars": []} + + sensor_patterns = { + "cameras": ["camera", "monocular"], + "lidars": ["hesai", "velodyne", "pandar", "lidar"], + "imus": ["imu", "gnss"], + "radars": ["radar"], + } + + for macro in root.findall(".//*[@name]"): + name = macro.get("name", "") + params = dict(macro.attrib) + + sensor_type = None + for type_name, patterns in sensor_patterns.items(): + if any(pattern in name.lower() for pattern in patterns): + sensor_type = type_name + break + + if sensor_type: + sensors[sensor_type].append({"name": name, "params": self._sort_params(params)}) + + for sensor_type in sensors: + sensors[sensor_type].sort(key=lambda x: x["name"]) + return sensors + + def _format_params(self, params: OrderedDict) -> str: + """ + Format parameters for readable output. + + Args: + params (OrderedDict): Parameters to format + + Returns: + str: Formatted parameter string + """ + return "\n".join([f' {k}="{v}"' for k, v in params.items()]) + + def analyze_differences(self) -> str: + """ + Analyze and report differences between the two Xacro files. + + This method performs a comprehensive comparison of: + - Included files + - Sensor configurations + - Parameter changes + + Returns: + str: Formatted report of all differences found + """ + output = [] + + # 1. Compare includes + output.append("# Key Differences\n") + output.append("## 1. Include Files") + original_includes = self._get_includes(self.original_root) + new_includes = self._get_includes(self.new_root) + + added_includes = set(new_includes) - set(original_includes) + removed_includes = set(original_includes) - set(new_includes) + + if added_includes or removed_includes: + output.append("### Changes:") + if added_includes: + output.append("**Added:**") + for inc in sorted(added_includes): + output.append(f"- {inc}") + if removed_includes: + output.append("**Removed:**") + for inc in sorted(removed_includes): + output.append(f"- {inc}") + output.append("") + + # 2. Compare sensors + original_sensors = self._get_sensors(self.original_root) + new_sensors = self._get_sensors(self.new_root) + + output.append("## 2. Sensor Configuration Changes") + + for sensor_type in ["cameras", "lidars", "imus", "radars"]: + orig_sensor = original_sensors[sensor_type] + new_sensor = new_sensors[sensor_type] + + if orig_sensor or new_sensor: + output.append(f"\n### {sensor_type.title()}") + + # Compare sensor names + orig_names = [s["name"] for s in orig_sensor] + new_names = [s["name"] for s in new_sensor] + + if orig_names != new_names: + output.append("#### Name Changes:") + output.append( + f"**Original ({len(orig_names)})**: " + ", ".join(sorted(orig_names)) + ) + output.append(f"**New ({len(new_names)})**: " + ", ".join(sorted(new_names))) + + # Compare parameters + if orig_sensor and new_sensor: + output.append("\n#### Parameter Changes:") + + # Compare parameters of first sensor of each type + orig_params = orig_sensor[0]["params"] + new_params = new_sensor[0]["params"] + + added_params = set(new_params.keys()) - set(orig_params.keys()) + removed_params = set(orig_params.keys()) - set(new_params.keys()) + changed_params = { + k: (orig_params[k], new_params[k]) + for k in set(orig_params.keys()) & set(new_params.keys()) + if orig_params[k] != new_params[k] + } + + if added_params: + output.append("**Added parameters:**") + for param in sorted(added_params): + output.append(f'- {param}: "{new_params[param]}"') + + if removed_params: + output.append("**Removed parameters:**") + for param in sorted(removed_params): + output.append(f'- {param}: "{orig_params[param]}"') + + if changed_params: + output.append("**Modified parameters:**") + for param, (old_val, new_val) in sorted(changed_params.items()): + output.append(f'- {param}: "{old_val}" → "{new_val}"') + + return "\n".join(output) + + +def main(): + # Create argument parser + parser = argparse.ArgumentParser( + description="Compare two XACRO files and analyze their differences" + ) + + # Add arguments + parser.add_argument( + "--original", "-o", required=True, help="Path to the original sensors.xacro file" + ) + + parser.add_argument("--new", "-n", required=True, help="Path to the new sensors.xacro file") + + # Parse arguments + args = parser.parse_args() + + # Create analyzer instance with provided file paths + analyzer = XacroAnalyzer(args.original, args.new) + + # Print analysis results + print(analyzer.analyze_differences()) + + +if __name__ == "__main__": + main() From 596d5b01a0b098d685b38416fc9cc58f713a952a Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 12:12:43 +0900 Subject: [PATCH 05/20] fix(xx1_des): add frame_id for certain sensors to restore the original look Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/config/sensor_kit_calibration.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/aip_xx1_description/config/sensor_kit_calibration.yaml b/aip_xx1_description/config/sensor_kit_calibration.yaml index db7de0ff..69c94144 100644 --- a/aip_xx1_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_description/config/sensor_kit_calibration.yaml @@ -95,6 +95,7 @@ sensor_kit_base_link: pitch: 0.0 yaw: 0.0 type: imu + frame_id: gnss tamagawa/imu_link: x: 0.0 y: 0.0 @@ -103,3 +104,4 @@ sensor_kit_base_link: pitch: 0.0 yaw: 3.14159265359 type: imu + frame_id: tamagawa/imu From c6095b6cef04e20124cdc23127668c8994377ca7 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 12:13:03 +0900 Subject: [PATCH 06/20] fix(xx1_gen2_des): add frame_id for certain sensors to restore the original look Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_gen2_description/config/sensor_kit_calibration.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml index a0d91928..d6025d6f 100644 --- a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml @@ -103,6 +103,7 @@ sensor_kit_base_link: pitch: 0.0 # Design Value yaw: 3.14159265359 # Design Value type: pandar_xt32 + frame_id: hesai_side_left hesai_right_base_link: x: 0.0 # Design Value y: -0.564 # Design Value @@ -111,6 +112,7 @@ sensor_kit_base_link: pitch: 0.0 # Design Value yaw: 0.0 # Design Value type: pandar_xt32 + frame_id: hesai_side_right gnss_link: x: -0.279 # Design Value y: 0.0 # Design Value @@ -119,6 +121,7 @@ sensor_kit_base_link: pitch: 0.0 # Design Value yaw: 0.0 # Design Value type: imu + frame_id: gnss tamagawa/imu_link: x: -0.129 # Design Value y: 0.0 # Design Value @@ -127,3 +130,4 @@ sensor_kit_base_link: pitch: 0.0 # Design Value yaw: 3.14159265359 # Design Value type: imu + frame_id: tamagawa/imu From bc97d66011f4b3dadf7255ed6015a0cac24fc6f4 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 12:41:21 +0900 Subject: [PATCH 07/20] create a cmake macro to simplify adaptation of other packages Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/CMakeLists.txt | 13 ++++++-- .../cmake/aip_cmake_urdf_compile.cmake | 32 +++++++++++++++++++ 2 files changed, 42 insertions(+), 3 deletions(-) create mode 100644 aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake diff --git a/aip_urdf_compiler/CMakeLists.txt b/aip_urdf_compiler/CMakeLists.txt index 24b99f08..459dd61d 100644 --- a/aip_urdf_compiler/CMakeLists.txt +++ b/aip_urdf_compiler/CMakeLists.txt @@ -5,8 +5,15 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +# Install cmake directory +install( + DIRECTORY cmake templates scripts + DESTINATION share/${PROJECT_NAME} +) + # Export the package's share directory path -ament_auto_package(INSTALL_TO_SHARE - templates - scripts + +# Add the config extras +ament_package( + CONFIG_EXTRAS "cmake/aip_cmake_urdf_compile.cmake" ) diff --git a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake new file mode 100644 index 00000000..d38f03c7 --- /dev/null +++ b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake @@ -0,0 +1,32 @@ + + + +macro(aip_cmake_urdf_compile) + # Set the correct paths + set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") + set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") + set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") + set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") + set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") + + message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") + message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") + + # Verify that the required files exist + if(NOT EXISTS "${PYTHON_SCRIPT}") + message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") + endif() + + # Create a custom command to run the Python script + add_custom_target(xacro_compilation ALL + DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag + ) + + add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag + COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DEPENDS ${PYTHON_SCRIPT} + COMMENT "Running Python script for URDF creation" + ) +endmacro() From c2fd4878172baf945040ae498c3f18262e60fd1c Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 12:41:43 +0900 Subject: [PATCH 08/20] xx1_des utilize the new macro Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/CMakeLists.txt | 57 +++++++++++++++--------------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/aip_xx1_description/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index e703ec94..7f60033c 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -8,34 +8,35 @@ ament_auto_find_build_dependencies() find_package(PythonInterp REQUIRED) # cspell: ignore Interp -# Get the package share directory -set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") -# Set the correct paths -set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") -set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") -set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") -set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") - -message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") -message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") - -# Verify that the required files exist -if(NOT EXISTS "${PYTHON_SCRIPT}") - message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") -endif() - -# Create a custom command to run the Python script -add_custom_target(xacro_compilation ALL - DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag -) - -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag - COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - DEPENDS ${PYTHON_SCRIPT} - COMMENT "Running Python script for URDF creation" -) +# # Get the package share directory +# set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") +# # Set the correct paths +# set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") +# set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") +# set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") +# set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") + +# message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") +# message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") + +# # Verify that the required files exist +# if(NOT EXISTS "${PYTHON_SCRIPT}") +# message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") +# endif() + +# # Create a custom command to run the Python script +# add_custom_target(xacro_compilation ALL +# DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag +# ) + +# add_custom_command( +# OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag +# COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} +# WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +# DEPENDS ${PYTHON_SCRIPT} +# COMMENT "Running Python script for URDF creation" +# ) +aip_cmake_urdf_compile() ament_auto_package(INSTALL_TO_SHARE urdf From 8634e743de8bc6cd7c216c536f2a6cfbe86dfdcd Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 12:41:53 +0900 Subject: [PATCH 09/20] xx1_gen2 utilize the new macro Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_gen2_description/CMakeLists.txt | 29 +------------------------ 1 file changed, 1 insertion(+), 28 deletions(-) diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt index f7e38a63..d20cc2c5 100644 --- a/aip_xx1_gen2_description/CMakeLists.txt +++ b/aip_xx1_gen2_description/CMakeLists.txt @@ -8,34 +8,7 @@ ament_auto_find_build_dependencies() find_package(PythonInterp REQUIRED) # cspell: ignore Interp -# Get the package share directory -set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") -# Set the correct paths -set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") -set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") -set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") -set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") - -message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") -message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") - -# Verify that the required files exist -if(NOT EXISTS "${PYTHON_SCRIPT}") - message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") -endif() - -# Create a custom command to run the Python script -add_custom_target(xacro_compilation ALL - DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag -) - -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag - COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - DEPENDS ${PYTHON_SCRIPT} - COMMENT "Running Python script for URDF creation" -) +aip_cmake_urdf_compile() ament_auto_package(INSTALL_TO_SHARE urdf From 3f54a6f18598c66f1a0481120a48954c6579ec0b Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 13:08:48 +0900 Subject: [PATCH 10/20] fixbug Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_xx1_description/CMakeLists.txt | 28 ---------------------------- 1 file changed, 28 deletions(-) diff --git a/aip_xx1_description/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index 7f60033c..8e5d88df 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -8,34 +8,6 @@ ament_auto_find_build_dependencies() find_package(PythonInterp REQUIRED) # cspell: ignore Interp -# # Get the package share directory -# set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") -# # Set the correct paths -# set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") -# set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") -# set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") -# set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") - -# message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") -# message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") - -# # Verify that the required files exist -# if(NOT EXISTS "${PYTHON_SCRIPT}") -# message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") -# endif() - -# # Create a custom command to run the Python script -# add_custom_target(xacro_compilation ALL -# DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag -# ) - -# add_custom_command( -# OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag -# COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} -# WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} -# DEPENDS ${PYTHON_SCRIPT} -# COMMENT "Running Python script for URDF creation" -# ) aip_cmake_urdf_compile() ament_auto_package(INSTALL_TO_SHARE From b54e73b240aeb21302476bace87a74b6247dc208 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 13:10:08 +0900 Subject: [PATCH 11/20] fix debug Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake | 1 + aip_xx1_description/CMakeLists.txt | 2 -- aip_xx1_gen2_description/CMakeLists.txt | 2 -- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake index d38f03c7..70c13c47 100644 --- a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake +++ b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake @@ -3,6 +3,7 @@ macro(aip_cmake_urdf_compile) # Set the correct paths + find_package(PythonInterp REQUIRED) # cspell: ignore Interp set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") diff --git a/aip_xx1_description/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index 8e5d88df..362fb6da 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -6,8 +6,6 @@ find_package(aip_urdf_compiler REQUIRED) ament_auto_find_build_dependencies() -find_package(PythonInterp REQUIRED) # cspell: ignore Interp - aip_cmake_urdf_compile() ament_auto_package(INSTALL_TO_SHARE diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt index d20cc2c5..782df039 100644 --- a/aip_xx1_gen2_description/CMakeLists.txt +++ b/aip_xx1_gen2_description/CMakeLists.txt @@ -6,8 +6,6 @@ find_package(aip_urdf_compiler REQUIRED) ament_auto_find_build_dependencies() -find_package(PythonInterp REQUIRED) # cspell: ignore Interp - aip_cmake_urdf_compile() ament_auto_package(INSTALL_TO_SHARE From 51ad4bdbf417502ec61a494dc45ded6cc3576bf4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 20 Dec 2024 04:12:12 +0000 Subject: [PATCH 12/20] ci(pre-commit): autofix --- aip_urdf_compiler/package.xml | 1 - aip_xx1_description/package.xml | 6 +++--- aip_xx1_gen2_description/package.xml | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/aip_urdf_compiler/package.xml b/aip_urdf_compiler/package.xml index ff8aab61..1238ae8e 100644 --- a/aip_urdf_compiler/package.xml +++ b/aip_urdf_compiler/package.xml @@ -10,7 +10,6 @@ ament_cmake_auto - ament_cmake diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index 8225d75b..73954095 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -7,11 +7,11 @@ Yukihiro Saito Apache 2 - ament_cmake_auto ament_cmake - - velodyne_description + ament_cmake_auto + aip_urdf_compiler + velodyne_description ament_cmake diff --git a/aip_xx1_gen2_description/package.xml b/aip_xx1_gen2_description/package.xml index 480e5307..e1f2774f 100644 --- a/aip_xx1_gen2_description/package.xml +++ b/aip_xx1_gen2_description/package.xml @@ -9,8 +9,8 @@ ament_cmake_auto - velodyne_description aip_urdf_compiler + velodyne_description ament_cmake From 0203f72e33fee59231715aa6d5ff1e980df3dd6e Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 20 Dec 2024 13:16:19 +0900 Subject: [PATCH 13/20] add requirement on jinja2 Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/aip_urdf_compiler/package.xml b/aip_urdf_compiler/package.xml index ff8aab61..1a0c129e 100644 --- a/aip_urdf_compiler/package.xml +++ b/aip_urdf_compiler/package.xml @@ -9,6 +9,7 @@ Apache 2 ament_cmake_auto + python3-jinja2 From 877c9f551ac87d9869dadcd2086ac5a90c583710 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Mon, 23 Dec 2024 14:37:08 +0900 Subject: [PATCH 14/20] add GNSS type; improve error handling; fix velodyne sensor names Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/scripts/compile_xacro.py | 35 ++++++++++++++----- .../tests/{xml_diff.py => xacro_diff.py} | 0 .../config/sensor_kit_calibration.yaml | 8 ++--- .../config/sensors_calibration.yaml | 2 +- .../config/sensor_kit_calibration.yaml | 2 +- 5 files changed, 32 insertions(+), 15 deletions(-) rename aip_urdf_compiler/tests/{xml_diff.py => xacro_diff.py} (100%) diff --git a/aip_urdf_compiler/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_xacro.py index a82a2f6e..ba49c66d 100644 --- a/aip_urdf_compiler/scripts/compile_xacro.py +++ b/aip_urdf_compiler/scripts/compile_xacro.py @@ -31,12 +31,21 @@ def load_yaml(file_path: str) -> Dict: Returns: Dict: Parsed YAML content or None if parsing fails """ - with open(file_path, "r") as stream: - try: - return yaml.safe_load(stream) - except yaml.YAMLError as exc: - print(exc) - return None + try: + with open(file_path, "r") as stream: + content = yaml.safe_load(stream) + if content is None: + raise ValueError(f"YAML file is empty or invalid: {file_path}") + if not isinstance(content, dict): + raise ValueError(f"YAML file must contain a dictionary: {file_path}") + return content + + except FileNotFoundError: + raise FileNotFoundError(f"YAML file not found: {file_path}") + except yaml.YAMLError as exc: + raise yaml.YAMLError(f"Failed to parse YAML file {file_path}: {str(exc)}") + except Exception as e: # Add general exception handling + raise RuntimeError(f"Unexpected error reading YAML file {file_path}: {str(e)}") class Transformation: @@ -177,9 +186,10 @@ class LinkType(enum.Enum): PANDAR_XT32 = "pandar_xt32" PANDAR_QT = "pandar_qt" PANDAR_QT128 = "pandar_qt128" - VELODYNE16 = "VLP-16.urdf" - VLS128 = "VLS-128.urdf" + VELODYNE16 = "velodyne_16" + VLS128 = "velodyne_128" RADAR = "radar" + GNSS = "gnss" JOINT_UNITS = "units" @@ -202,9 +212,12 @@ 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: return LinkType.IMU + if "gnss" in link_name: + return LinkType.GNSS + if "livox" in link_name: return LinkType.LIVOX @@ -330,6 +343,10 @@ def VLS128_func(transform: Transformation) -> str: "including_file": "$(find imu_description)/urdf/imu.xacro", "string_api": functools.partial(base_string_func, "imu_macro"), }, + LinkType.GNSS: { # for now, GNSS will also use the imu xacro files. + "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, diff --git a/aip_urdf_compiler/tests/xml_diff.py b/aip_urdf_compiler/tests/xacro_diff.py similarity index 100% rename from aip_urdf_compiler/tests/xml_diff.py rename to aip_urdf_compiler/tests/xacro_diff.py diff --git a/aip_xx1_description/config/sensor_kit_calibration.yaml b/aip_xx1_description/config/sensor_kit_calibration.yaml index 69c94144..bc589de3 100644 --- a/aip_xx1_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_description/config/sensor_kit_calibration.yaml @@ -70,7 +70,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 1.575 - type: VLS-128.urdf + type: velodyne_128 velodyne_left_base_link: x: 0.0 y: 0.56362 @@ -78,7 +78,7 @@ sensor_kit_base_link: roll: -0.02 pitch: 0.71 yaw: 1.575 - type: VLP-16.urdf + type: velodyne_16 velodyne_right_base_link: x: 0.0 y: -0.56362 @@ -86,7 +86,7 @@ sensor_kit_base_link: roll: -0.01 pitch: 0.71 yaw: -1.580 - type: VLP-16.urdf + type: velodyne_16 gnss_link: x: -0.1 y: 0.0 @@ -94,7 +94,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 - type: imu + type: gnss frame_id: gnss tamagawa/imu_link: x: 0.0 diff --git a/aip_xx1_description/config/sensors_calibration.yaml b/aip_xx1_description/config/sensors_calibration.yaml index c7de4df6..bf8dce3a 100644 --- a/aip_xx1_description/config/sensors_calibration.yaml +++ b/aip_xx1_description/config/sensors_calibration.yaml @@ -38,4 +38,4 @@ base_link: roll: -0.02 pitch: 0.7281317 yaw: 3.141592 - type: VLP-16.urdf + type: velodyne_16 diff --git a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml index d6025d6f..218231a4 100644 --- a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml @@ -120,7 +120,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value - type: imu + type: gnss frame_id: gnss tamagawa/imu_link: x: -0.129 # Design Value From ddbc8f7ec86070f18ba29829bc6ff740505e3c83 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Mon, 23 Dec 2024 15:30:04 +0900 Subject: [PATCH 15/20] added read me documentation, improve the warning and debugging outputs Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/readme.md | 121 +++++++++++++++++++++ aip_urdf_compiler/scripts/compile_xacro.py | 14 ++- 2 files changed, 131 insertions(+), 4 deletions(-) create mode 100644 aip_urdf_compiler/readme.md diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md new file mode 100644 index 00000000..011ca13b --- /dev/null +++ b/aip_urdf_compiler/readme.md @@ -0,0 +1,121 @@ +# aip_urdf_compiler + +## Overview + +The aip_urdf_compiler package provides tools for dynamically generating URDF (Unified Robot Description Format) files from configuration files during the build process. It simplifies sensor model management by automatically URDF models from sensor configurations. + +## Key Features + +- Dynamic URDF generation during colcon build +- Automated sensor transform processing +- Support for multiple sensor types and configurations + +## Usage + +### Package Integration + +To use aip_urdf_compiler in your description package: + +- Add the dependency in `package.xml`: + + ```xml + aip_urdf_compiler + ``` + +- Add the following to `CMakeLists.txt`: + + ```cmake + find_package(aip_urdf_compiler REQUIRED) + aip_cmake_urdf_compile() + ``` + +- Configure your sensors in `config/sensors.yaml` with required meta values: + + - `type`: Sensor type identifier + - `frame_id`: TF frame identifier + +- Clean up existing `.xacro` files and add to `.gitignore`: + + ```gitignore + # In your URDF folder + *.xacro + ``` + +## Architecture + +### Components + +1. **aip_urdf_compiler** + + - Main package handling URDF generation + - Processes configuration files + - Manages build-time compilation + +2. **aip_cmake_urdf_compile** + + - CMake macro implementation + - Creates build targets + - Ensures URDF regeneration on each build + +3. **compile_xacro.py** + - Configuration parser + - Transform processor + - URDF generator + +### Compilation Process + +1. **Configuration Reading** + + - Parses `config/sensors.yaml` + - Extracts transformation data + - Validates configurations + +2. **Transform Processing** + + - Processes each sensor transform + - Determines sensor types and frame IDs + - Generates appropriate macro strings + - Creates `sensors.xacro` + +3. **Joint Unit Processing** + - Handles joint unit transforms + - Processes related YAML files + - Generates separate URDF xacro files + +## Adding New Sensors + +1. Add sensor descriptions (xacro files) in either: + + - Your target package + - `common_sensor_description` package + +2. Update the following in `compile_xacro.py`: + - `LinkType` enumeration + - `link_dict` mapping + +## Troubleshooting + +### Debug Logs + +Check build logs for debugging information: + +```bash +cat $workspace/log/build_/aip_{project}_description/streams.log +``` + +### Common Issues + +1. Missing sensor definitions + + - Ensure sensor type is defined in `LinkType` + - Verify xacro file exists in description package + +2. TF Trees errors + - Check frame_id values in sensors.yaml + - Verify transform chain completeness + +## Contributing + +1. Follow ROS coding standards +2. Test URDF generation with various configurations +3. Update documentation for new features diff --git a/aip_urdf_compiler/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_xacro.py index ba49c66d..c56c4eee 100644 --- a/aip_urdf_compiler/scripts/compile_xacro.py +++ b/aip_urdf_compiler/scripts/compile_xacro.py @@ -16,6 +16,7 @@ from typing import Callable from typing import Dict from typing import Union +import warnings from jinja2 import Template import yaml @@ -82,7 +83,10 @@ def __init__(self, transformation: Dict, base_frame: str, child_frame: str): self.name = self.child_frame.replace("_base_link", "").replace("_link", "") if len(self.type) == 0: - self.type = determine_link_type(self.name) + self.type = determine_link_type(self.name).value + warnings.warn( + f"Warning: Link type not explicitly defined for '{self.name}'. Determining type from link name and obtained {self.type}" + ) self.frame_id: str = transformation.get("frame_id", "") if len(self.frame_id) == 0: @@ -401,6 +405,7 @@ def main( base_template = Template(file.read()) # Render the template + print("Processing the main sensors_calibration.yaml") calibration_path = os.path.join(calibration_directory, "sensors_calibration.yaml") calib_yaml = load_yaml(calibration_path) calib = Calibration(calib_yaml) @@ -418,6 +423,7 @@ def main( for _, transform in calib.transforms.items(): link_type: LinkType = obtain_link_type(transform) if link_type == LinkType.JOINT_UNITS: + print(f"Collected joint sensor unit {transform.name}, which will be further rendered.") render_meta_data["sensor_units_includes"].append( link_dicts[link_type]["including_file"].format(filename=transform.name) ) @@ -430,6 +436,7 @@ def main( } ) else: + print(f"Collected {transform.name}.") include_text.add(link_dicts[link_type]["including_file"]) sensor_items.append(link_dicts[link_type]["string_api"](transform)) @@ -437,7 +444,6 @@ def main( render_meta_data["isolated_sensors"] = sensor_items rendered = base_template.render(render_meta_data) - print(rendered) print("=====================================") # Save the rendered template @@ -449,6 +455,7 @@ def main( sensor_units_template = Template(file.read()) for i, sensor_unit in enumerate(render_meta_data["sensor_units"]): + print(f"Processing {sensor_unit['name']}") sensor_unit_calib_path = os.path.join( calibration_directory, f"{sensor_unit['name']}_calibration.yaml" ) @@ -465,13 +472,12 @@ def main( for _, transform in sensor_unit_calib.transforms.items(): link_type: LinkType = obtain_link_type(transform) include_text.add(link_dicts[link_type]["including_file"]) - print(transform.name) + print(f"collected {transform.name}") 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("=====================================") From ef0bff697f7f35fa0cab4d2cab05a020824b6b79 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Mon, 23 Dec 2024 16:36:59 +0900 Subject: [PATCH 16/20] improve documentation Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/readme.md | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md index 011ca13b..01dffaeb 100644 --- a/aip_urdf_compiler/readme.md +++ b/aip_urdf_compiler/readme.md @@ -29,10 +29,10 @@ To use aip_urdf_compiler in your description package: aip_cmake_urdf_compile() ``` -- Configure your sensors in `config/sensors.yaml` with required meta values: +- Configure your sensors in `config/sensors.yaml` with metadata values (Note: this does not need to be added in `individual_params`): - - `type`: Sensor type identifier - - `frame_id`: TF frame identifier + - `type`: Required string, corresponding to the string value from [existing sensors](#existing-sensors) + - `frame_id`: Optional string, overwrites the TF frame ID. - Clean up existing `.xacro` files and add to `.gitignore`: @@ -41,6 +41,27 @@ To use aip_urdf_compiler in your description package: *.xacro ``` +### Existing Sensors + +```python +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 = "velodyne_16" + VLS128 = "velodyne_128" + RADAR = "radar" + GNSS = "gnss" + JOINT_UNITS = "units" +``` + ## Architecture ### Components From 415326c56425eec81c3ef012791b2c38fcafc70b Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Mon, 23 Dec 2024 16:39:18 +0900 Subject: [PATCH 17/20] fix grammar Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md index 01dffaeb..e593847a 100644 --- a/aip_urdf_compiler/readme.md +++ b/aip_urdf_compiler/readme.md @@ -29,7 +29,7 @@ To use aip_urdf_compiler in your description package: aip_cmake_urdf_compile() ``` -- Configure your sensors in `config/sensors.yaml` with metadata values (Note: this does not need to be added in `individual_params`): +- Configure your sensors in `config/sensors.yaml` with metadata values (Note: do not need to add metavalues in `individual_params`): - `type`: Required string, corresponding to the string value from [existing sensors](#existing-sensors) - `frame_id`: Optional string, overwrites the TF frame ID. From 1c21d10dd7fa383cf646d538b257e16ffbb19835 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Mon, 23 Dec 2024 17:04:36 +0900 Subject: [PATCH 18/20] fix cspell Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md index e593847a..02278575 100644 --- a/aip_urdf_compiler/readme.md +++ b/aip_urdf_compiler/readme.md @@ -29,7 +29,7 @@ To use aip_urdf_compiler in your description package: aip_cmake_urdf_compile() ``` -- Configure your sensors in `config/sensors.yaml` with metadata values (Note: do not need to add metavalues in `individual_params`): +- Configure your sensors in `config/sensors.yaml` with metadata values (Note: do not need to add meta values in `individual_params`): - `type`: Required string, corresponding to the string value from [existing sensors](#existing-sensors) - `frame_id`: Optional string, overwrites the TF frame ID. From 91ff3265d24ddf0835e87f3f7d12f06ffdfc6f39 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 27 Dec 2024 08:55:58 +0900 Subject: [PATCH 19/20] unify the name in aip_urdf_compiler to be URDF compile Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake | 2 +- aip_urdf_compiler/readme.md | 4 ++-- .../scripts/{compile_xacro.py => compile_urdf.py} | 0 aip_urdf_compiler/tests/{xacro_diff.py => urdf_diff.py} | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) rename aip_urdf_compiler/scripts/{compile_xacro.py => compile_urdf.py} (100%) rename aip_urdf_compiler/tests/{xacro_diff.py => urdf_diff.py} (99%) diff --git a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake index 70c13c47..aed90ac8 100644 --- a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake +++ b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake @@ -5,7 +5,7 @@ macro(aip_cmake_urdf_compile) # Set the correct paths find_package(PythonInterp REQUIRED) # cspell: ignore Interp set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") - set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_xacro.py") + set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_urdf.py") set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md index 02278575..71b0a887 100644 --- a/aip_urdf_compiler/readme.md +++ b/aip_urdf_compiler/readme.md @@ -78,7 +78,7 @@ class LinkType(enum.Enum): - Creates build targets - Ensures URDF regeneration on each build -3. **compile_xacro.py** +3. **compile_urdf.py** - Configuration parser - Transform processor - URDF generator @@ -105,7 +105,7 @@ class LinkType(enum.Enum): ## Adding New Sensors -1. Add sensor descriptions (xacro files) in either: +1. Add sensor descriptions (xacro module files) in either: - Your target package - `common_sensor_description` package diff --git a/aip_urdf_compiler/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_urdf.py similarity index 100% rename from aip_urdf_compiler/scripts/compile_xacro.py rename to aip_urdf_compiler/scripts/compile_urdf.py diff --git a/aip_urdf_compiler/tests/xacro_diff.py b/aip_urdf_compiler/tests/urdf_diff.py similarity index 99% rename from aip_urdf_compiler/tests/xacro_diff.py rename to aip_urdf_compiler/tests/urdf_diff.py index 8dd92a34..ead01163 100644 --- a/aip_urdf_compiler/tests/xacro_diff.py +++ b/aip_urdf_compiler/tests/urdf_diff.py @@ -1,6 +1,6 @@ #!/usr/bin/python3 """ -XML/Xacro Difference Analyzer. +URDF Model Difference Analyzer. This script analyzes and compares two XML/Xacro files, specifically designed for ROS URDF/Xacro files containing sensor configurations. It identifies and reports differences in included files, sensor configurations, From 13bb21f6bc866f97376906d5ebe44a584846697d Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Fri, 27 Dec 2024 08:56:39 +0900 Subject: [PATCH 20/20] fix bug Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake | 2 +- aip_urdf_compiler/readme.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake index aed90ac8..2b6c2450 100644 --- a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake +++ b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake @@ -15,7 +15,7 @@ macro(aip_cmake_urdf_compile) # Verify that the required files exist if(NOT EXISTS "${PYTHON_SCRIPT}") - message(FATAL_ERROR "Could not find compile_xacro.py at ${PYTHON_SCRIPT}") + message(FATAL_ERROR "Could not find compile_urdf.py at ${PYTHON_SCRIPT}") endif() # Create a custom command to run the Python script diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md index 71b0a887..24143490 100644 --- a/aip_urdf_compiler/readme.md +++ b/aip_urdf_compiler/readme.md @@ -110,7 +110,7 @@ class LinkType(enum.Enum): - Your target package - `common_sensor_description` package -2. Update the following in `compile_xacro.py`: +2. Update the following in `compile_urdf.py`: - `LinkType` enumeration - `link_dict` mapping