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