diff --git a/aip_urdf_compiler/CMakeLists.txt b/aip_urdf_compiler/CMakeLists.txt new file mode 100644 index 00000000..459dd61d --- /dev/null +++ b/aip_urdf_compiler/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_urdf_compiler) + +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 + +# 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..2b6c2450 --- /dev/null +++ b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake @@ -0,0 +1,33 @@ + + + +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_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") + + 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_urdf.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() diff --git a/aip_urdf_compiler/package.xml b/aip_urdf_compiler/package.xml new file mode 100644 index 00000000..c6355e42 --- /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 + python3-jinja2 + + + ament_cmake + + diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md new file mode 100644 index 00000000..24143490 --- /dev/null +++ b/aip_urdf_compiler/readme.md @@ -0,0 +1,142 @@ +# 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 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. + +- Clean up existing `.xacro` files and add to `.gitignore`: + + ```gitignore + # In your URDF folder + *.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 + +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_urdf.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 module files) in either: + + - Your target package + - `common_sensor_description` package + +2. Update the following in `compile_urdf.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_urdf.py b/aip_urdf_compiler/scripts/compile_urdf.py new file mode 100644 index 00000000..c56c4eee --- /dev/null +++ b/aip_urdf_compiler/scripts/compile_urdf.py @@ -0,0 +1,507 @@ +#!/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 +import warnings + +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 + """ + 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: + """ + 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"] + 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", "") + + if len(self.type) == 0: + 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: + 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}\" + 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: + """ + 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( + 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 = "velodyne_16" + VLS128 = "velodyne_128" + RADAR = "radar" + GNSS = "gnss" + JOINT_UNITS = "units" + + +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() + + # 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: + """Produce a guess of the type of the link based on its name.""" + if "cam" in link_name: + return LinkType.CAMERA + + if "imu" in link_name: + return LinkType.IMU + + if "gnss" in link_name: + return LinkType.GNSS + + 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.frame_id, # pandar + 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.frame_id, + 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.frame_id, + 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 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"), + }, + LinkType.IMU: { + "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, + }, + 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 + 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) + + 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: + 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) + ) + 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: + print(f"Collected {transform.name}.") + 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("=====================================") + # 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"]): + print(f"Processing {sensor_unit['name']}") + 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(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) + 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_urdf_compiler/tests/urdf_diff.py b/aip_urdf_compiler/tests/urdf_diff.py new file mode 100644 index 00000000..ead01163 --- /dev/null +++ b/aip_urdf_compiler/tests/urdf_diff.py @@ -0,0 +1,274 @@ +#!/usr/bin/python3 +""" +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, +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() diff --git a/aip_xx1_description/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index 50723262..362fb6da 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -2,9 +2,12 @@ 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() +aip_cmake_urdf_compile() + 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..bc589de3 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: velodyne_128 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: velodyne_16 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: velodyne_16 gnss_link: x: -0.1 y: 0.0 @@ -83,6 +94,8 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: gnss + frame_id: gnss tamagawa/imu_link: x: 0.0 y: 0.0 @@ -90,3 +103,5 @@ sensor_kit_base_link: roll: 3.14159265359 pitch: 0.0 yaw: 3.14159265359 + type: imu + frame_id: tamagawa/imu diff --git a/aip_xx1_description/config/sensors_calibration.yaml b/aip_xx1_description/config/sensors_calibration.yaml index e8c4b75e..bf8dce3a 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: velodyne_16 diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index b35b48e0..73954095 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -7,8 +7,10 @@ Yukihiro Saito Apache 2 + ament_cmake ament_cmake_auto + aip_urdf_compiler velodyne_description 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 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt index 549de0f8..782df039 100644 --- a/aip_xx1_gen2_description/CMakeLists.txt +++ b/aip_xx1_gen2_description/CMakeLists.txt @@ -2,9 +2,12 @@ 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() +aip_cmake_urdf_compile() + 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..218231a4 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,8 @@ sensor_kit_base_link: roll: 0.872665 # Design Value 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 @@ -97,6 +111,8 @@ sensor_kit_base_link: roll: 0.69813132679 # Design Value 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 @@ -104,6 +120,8 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: gnss + frame_id: gnss tamagawa/imu_link: x: -0.129 # Design Value y: 0.0 # Design Value @@ -111,3 +129,5 @@ sensor_kit_base_link: roll: 3.14159265359 pitch: 0.0 # Design Value yaw: 3.14159265359 # Design Value + type: imu + frame_id: tamagawa/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..e1f2774f 100644 --- a/aip_xx1_gen2_description/package.xml +++ b/aip_xx1_gen2_description/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto + aip_urdf_compiler velodyne_description 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 @@ - - - - - - - - - - - - - - - - - - - - - - -