Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Jul 30, 2024
1 parent eb474d9 commit d9a4cce
Show file tree
Hide file tree
Showing 3 changed files with 126 additions and 90 deletions.
212 changes: 124 additions & 88 deletions aip_xx1_description/scripts/compile_xacro.py
View file
Original file line number Diff line number Diff line change
@@ -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:
Expand All @@ -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:
Expand Down Expand Up @@ -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"
Expand All @@ -78,8 +78,9 @@ 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"
Expand All @@ -93,44 +94,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 = """<xacro:{type}
name=\"{child_frame}\"
parent=\"{base_frame}\"
Expand All @@ -154,7 +157,8 @@ def determine_link_type(link_name:str)->LinkType:
/>
</xacro:{type}>"""

def base_string_func(type:str, transform:Transformation)->str:

def base_string_func(type: str, transform: Transformation) -> str:
if type == "monocular_camera_macro":
extra = """fps=\"30\"
width=\"800\"
Expand All @@ -167,80 +171,104 @@ def base_string_func(type:str, transform:Transformation)->str:
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=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
Expand All @@ -249,31 +277,34 @@ def main(template_directory:str, calibration_directory:str, output_directory:str
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["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
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
Expand All @@ -283,39 +314,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:
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["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")
Expand All @@ -325,5 +360,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)
# main(args.template_directory, args.calibration_directory, args.output_directory, args.project_name)
2 changes: 1 addition & 1 deletion aip_xx1_description/templates/sensor_unit.xacro.template
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,4 +28,4 @@
{% endfor %}
</xacro:macro>

</robot>
</robot>
2 changes: 1 addition & 1 deletion aip_xx1_description/urdf/.gitignore
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
*.xacro
*.xacro

0 comments on commit d9a4cce

Please sign in to comment.