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..70c13c47 --- /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_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() 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/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_xacro.py new file mode 100644 index 00000000..a82a2f6e --- /dev/null +++ b/aip_urdf_compiler/scripts/compile_xacro.py @@ -0,0 +1,484 @@ +#!/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) + except yaml.YAMLError as exc: + print(exc) + return None + + +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) + + 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 = "VLP-16.urdf" + VLS128 = "VLS-128.urdf" + RADAR = "radar" + 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 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.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.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.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("=====================================") + + 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/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() 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..69c94144 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,8 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: imu + 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..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..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..d6025d6f 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: imu + 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 @@ - - - - - - - - - - - - - - - - - - - - - - -