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