|
| 1 | +import re |
| 2 | +import shutil |
| 3 | +from pathlib import Path |
| 4 | + |
| 5 | +from ament_index_python.packages import get_package_share_directory |
| 6 | + |
| 7 | +from launch import LaunchDescription |
| 8 | +from launch.actions import DeclareLaunchArgument, OpaqueFunction |
| 9 | +from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution |
| 10 | +from launch_ros.substitutions import FindPackageShare |
| 11 | +from launch.substitutions import FindExecutable |
| 12 | + |
| 13 | + |
| 14 | +def generate_launch_description() -> LaunchDescription: |
| 15 | + return LaunchDescription( |
| 16 | + [ |
| 17 | + DeclareLaunchArgument("save_dir", default_value="~/ceai_ws/aegis_urdf"), |
| 18 | + DeclareLaunchArgument("tf_prefix", default_value=""), |
| 19 | + DeclareLaunchArgument("mock_hardware", default_value="false"), |
| 20 | + DeclareLaunchArgument("disable_cell", default_value="false"), |
| 21 | + DeclareLaunchArgument("disable_cell_collision", default_value="false"), |
| 22 | + OpaqueFunction(function=evaluate_robot_description), |
| 23 | + ] |
| 24 | + ) |
| 25 | + |
| 26 | + |
| 27 | +def evaluate_robot_description(context, *args, **kwargs) -> list: |
| 28 | + save_dir = ( |
| 29 | + Path(LaunchConfiguration("save_dir").perform(context)).expanduser().resolve() |
| 30 | + ) |
| 31 | + save_dir.mkdir(exist_ok=True) |
| 32 | + urdf_path = save_dir / "aegis.urdf" |
| 33 | + |
| 34 | + tf_prefix = LaunchConfiguration("tf_prefix") |
| 35 | + mock_hardware = LaunchConfiguration("mock_hardware") |
| 36 | + disable_cell = LaunchConfiguration("disable_cell") |
| 37 | + disable_cell_collision = LaunchConfiguration("disable_cell_collision") |
| 38 | + |
| 39 | + robot_description_str = Command( |
| 40 | + [ |
| 41 | + PathJoinSubstitution([FindExecutable(name="xacro")]), |
| 42 | + " ", |
| 43 | + PathJoinSubstitution( |
| 44 | + [FindPackageShare("aegis_description"), "urdf", "aegis.urdf.xacro"] |
| 45 | + ), |
| 46 | + " ", |
| 47 | + "tf_prefix:=", |
| 48 | + tf_prefix, |
| 49 | + " ", |
| 50 | + "mock_hardware:=", |
| 51 | + mock_hardware, |
| 52 | + " ", |
| 53 | + "disable_cell:=", |
| 54 | + disable_cell, |
| 55 | + " ", |
| 56 | + "disable_cell_collision:=", |
| 57 | + disable_cell_collision, |
| 58 | + ] |
| 59 | + ).perform(context) |
| 60 | + urdf_str, files_paths = extract_files_paths(robot_description_str) |
| 61 | + print(f"> Found {len(files_paths)} 3D models to be copied") |
| 62 | + |
| 63 | + for file in files_paths: |
| 64 | + shutil.copy(src=file, dst=save_dir / file.name) |
| 65 | + print(f"> Copied all 3D model files files to '{save_dir}'") |
| 66 | + |
| 67 | + urdf_path.write_bytes(urdf_str.encode("utf-8")) |
| 68 | + print(f"> URDF generated to '{urdf_path}'") |
| 69 | + |
| 70 | + return [] |
| 71 | + |
| 72 | + |
| 73 | +def extract_files_paths(urdf_str: str) -> tuple[str, set[Path]]: |
| 74 | + # Regex to get: 1) package name 2) rel. 3D model path 3) 3D model filename |
| 75 | + pattern = r"filename=\"package://(\w+)/?(.+/)?([^/]+\.(?:stl|dae|obj|3ds))\"" |
| 76 | + matches = re.findall(pattern, urdf_str) |
| 77 | + |
| 78 | + files_paths = set() |
| 79 | + for pkg_name, rel_model_path, filename in matches: |
| 80 | + package_path = Path(get_package_share_directory(pkg_name)) |
| 81 | + file_path = package_path / rel_model_path / filename |
| 82 | + |
| 83 | + files_paths.add(file_path) |
| 84 | + urdf_str = urdf_str.replace(f"package://{pkg_name}/{rel_model_path}", "./") |
| 85 | + return urdf_str, files_paths |
0 commit comments