Создан общий файл запуска digital_twin.launch.py, через который в дальнейшем будет запускаться весь цифровой двойник.

Добавлена папка `supported`, в которой будут размещаться файлы запуска подключаемые в основном файле.

Переименован контроллер на `iiwa_arm_controller`

Добавлен пакет `iiwa_bringup.utils`, в котором будут распологаться все вспомогательные функции, для  папки `launch`

В файле `README`, были добавлены подсказки для автора

README.md

@@ -9,3 +9,23 @@ sudo apt install -y ros-${ROS_DISTRO}-webots-ros2 \
                     ros-${ROS_DISTRO}-ros2-controllers \
                     ros-${ROS_DISTRO}-moveit-* \
 ```
+
+Установка moveit2 (внимательно проверяй)
+```bash
+sudo apt install -y build-essential \
+                    cmake \
+                    git \
+                    python3-colcon-common-extensions \
+                    python3-flake8 \
+                    python3-rosdep \
+                    python3-setuptools \
+                    python3-vcstool \
+                    wget
+
+git clone https://github.com/moveit/moveit2.git
+vcs import --recursive < moveit2/moveit2.repos
+sudo apt remove ros-$ROS_DISTRO-moveit*
+rosdep install -r --from-paths ./src/ --ignore-src --rosdistro $ROS_DISTRO --os=ubuntu:noble -y
+
+colcon build --mixin release
+```

src/iiwa_bringup/config/iiwa_controller.yaml

@@ -5,11 +5,11 @@ controller_manager:
     joint_state_broadcaster:
       type: "joint_state_broadcaster/JointStateBroadcaster"
 
-    joint_trajectory_controller:
+    iiwa_arm_controller:
       type: "joint_trajectory_controller/JointTrajectoryController"
 
 
-joint_trajectory_controller:
+iiwa_arm_controller:
   ros__parameters:
     joints:
       - joint1

src/iiwa_bringup/launch/digital_twin.launch.py

@@ -0,0 +1,156 @@
+from iiwa_bringup.utils import converter
+
+
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import (IncludeLaunchDescription, 
+                            DeclareLaunchArgument, 
+                            OpaqueFunction)
+
+
+PACKAGE = "iiwa_bringup"
+DESCRIPTION_PKG = "iiwa_description"
+
+
+def _runtime_setup(context, *args, **kwatgs):
+    setup = []
+
+    model_path = LaunchConfiguration('model').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    world_path = LaunchConfiguration('world').perform(context)
+    rviz_status = LaunchConfiguration('rviz').perform(context).lower() in ['true', '1', 'yes']
+    
+    transform = LaunchConfiguration('transform').perform(context)
+    rotation = LaunchConfiguration('rotation').perform(context)
+    timer = LaunchConfiguration('controller_timer').perform(context)
+
+    robot_description = converter.load_robot_description(model_path=model_path,
+                                                         robot_name=robot_name)
+
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{
+            "robot_description": robot_description,
+            "use_sim_time": False
+        }]
+    )
+    
+    webots_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare(PACKAGE),
+                'launch',
+                'supported', 'webots_spawn.launch.py'
+            ])
+        ),
+        launch_arguments={
+            'robot_name': robot_name,
+            'model': model_path,
+            'world': world_path,
+            'transform': transform,
+            'rotation': rotation,
+            'controller_timer': timer
+        }.items()
+    )
+
+
+    setup += [rsp_node, webots_launch]
+
+
+    if rviz_status:
+        rviz_launch = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution([FindPackageShare(PACKAGE),
+                "launch", "supported", 'rviz.launch.py'
+            ])),
+            launch_arguments={'package_name': PACKAGE}.items()
+        )
+
+        setup.append(rviz_launch)
+
+    return setup
+
+def generate_launch_description():
+    
+    # Объявление аргументов командной строки
+    declare_model_arg = DeclareLaunchArgument(
+        name="model",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(DESCRIPTION_PKG),
+            "urdf",
+            "iiwa7.urdf.xacro"
+        ]),
+        description="Path to robot xacro or urdf file (used to build robot_description)."
+    )
+
+    declare_robot_name_arg = DeclareLaunchArgument(
+        name="robot_name",
+        default_value="iiwa7",
+        description="Robot name (used for TF and naming spawned robot)."
+    )
+
+    declare_world_arg = DeclareLaunchArgument(
+        name="world",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(DESCRIPTION_PKG),
+            "worlds",
+            "iiwa.wbt"
+        ]),
+        description="Path to the Webots world (.wbt) to launch."
+    )
+
+    declare_controller_arg = DeclareLaunchArgument(
+        name="controller",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(PACKAGE),
+            "config",
+            "iiwa_controller.yaml"
+        ]),
+        description="Path to controllers YAML file (used by spawner and controller_manager)."
+    )
+
+    declare_transform_arg = DeclareLaunchArgument(
+        name="transform",
+        default_value="-0.25 0 0.79",
+        description="Translation applied when spawning the robot in the Webots world (x y z)."
+    )
+
+    declare_rotation_arg = DeclareLaunchArgument(
+        name="rotation",
+        default_value="0 0 1 0",
+        description="Rotation (axis-angle) applied when spawning the robot in the Webots world."
+    )
+
+    declare_rviz_arg = DeclareLaunchArgument(
+        name="rviz",
+        default_value="0",
+        description="If true|1|yes then launch RViz and joint_state_publisher_gui instead of controllers."
+    )
+
+    declare_controller_manager_arg = DeclareLaunchArgument(
+        name="controller_timer",
+        default_value="50",
+        description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout)."
+    )
+
+    runtime_setup = OpaqueFunction(
+        function=_runtime_setup
+    )
+
+    return LaunchDescription([
+        declare_model_arg,
+        declare_robot_name_arg,
+        declare_world_arg,
+        declare_controller_arg,
+        declare_transform_arg,
+        declare_rotation_arg,
+        declare_rviz_arg,
+        declare_controller_manager_arg,
+        runtime_setup
+    ])

src/iiwa_bringup/launch/supported/rviz.launch.py

@@ -0,0 +1,46 @@
+from launch_ros.actions import Node
+from launch.events import Shutdown
+from launch import LaunchDescription
+from launch.event_handlers import OnProcessExit
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import RegisterEventHandler, EmitEvent, DeclareLaunchArgument
+ 
+
+def generate_launch_description():
+
+    declare_package_arg = DeclareLaunchArgument(
+        'package_name',
+        default_value='iiwa_bringup',
+        description='Package name where RViz config is stored'
+    )
+
+    package_name = LaunchConfiguration('package_name')
+
+    rviz_config = PathJoinSubstitution([
+        FindPackageShare(package_name),
+        'config',
+        'rviz_iiwa.rviz'
+    ])
+
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d', rviz_config],
+        output='log'
+    )
+
+    # При закрытии rviz закрываем все
+    shutdown_on_rviz_exit = RegisterEventHandler(
+        OnProcessExit(
+            target_action=rviz,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )
+    )
+
+    return LaunchDescription([
+        declare_package_arg,
+        rviz,
+        shutdown_on_rviz_exit
+    ])

src/iiwa_bringup/launch/supported/webots_controllers.launch.py

@@ -0,0 +1,51 @@
+from launch_ros.actions import Node
+from webots_ros2_driver.urdf_spawner import URDFSpawner
+
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from iiwa_bringup.utils import converter
+
+
+def _setup_controllers(context, *args, **kwargs):
+    model_path = LaunchConfiguration('model').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    transform = LaunchConfiguration('transform').perform(context)
+    rotation = LaunchConfiguration('rotation').perform(context)
+    timer = LaunchConfiguration('controller_timer').perform(context)
+
+    robot_description = converter.load_robot_description(model_path=model_path,
+                                                         robot_name=robot_name)
+    
+    tmo = ['--controller-manager-timeout', str(timer)]
+
+    jsb = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=['joint_state_broadcaster'] + tmo,
+        parameters=[{'use_sim_time': False}],
+    )
+
+    jtc = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=['iiwa_arm_controller'] + tmo,
+        parameters=[{'use_sim_time': False}],
+    )
+
+    spawner_urdf = URDFSpawner(
+        name=robot_name,
+        robot_description=robot_description,
+        translation=transform,
+        rotation=rotation
+    )
+
+    return [jsb, jtc, spawner_urdf]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=_setup_controllers)
+    ])

src/iiwa_bringup/launch/supported/webots_spawn.launch.py

@@ -0,0 +1,90 @@
+from launch.events import Shutdown
+from launch import LaunchDescription
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import (IncludeLaunchDescription, 
+                            OpaqueFunction,
+                            TimerAction,
+                            RegisterEventHandler,
+                            EmitEvent)
+
+
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+from webots_ros2_driver.webots_controller import WebotsController
+
+
+def _spawn_setup(context, *args, **kwargs):
+    model_path = LaunchConfiguration('model').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    world_path = LaunchConfiguration('world').perform(context)
+    transform = LaunchConfiguration('transform').perform(context)
+    rotation = LaunchConfiguration('rotation').perform(context)
+    timer = LaunchConfiguration('controller_timer').perform(context)
+
+    webots = WebotsLauncher(world=world_path, 
+                            ros2_supervisor=True)
+    
+    driver = WebotsController(
+        robot_name=robot_name,
+        parameters=[
+            {
+                "robot_description": model_path,
+                "use_sim_time": False,
+                "set_robot_state_publisher": False
+            },
+            LaunchConfiguration('controller').perform(context)
+        ],
+        respawn=True
+    )
+
+    controllers_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('iiwa_bringup'),
+                'launch',
+                'supported',
+                'webots_controllers.launch.py'
+            ])
+        ),
+        launch_arguments={
+            'robot_name': robot_name,
+            'model': model_path,
+            'transform': transform,
+            'rotation': rotation,
+            'controller_timer': timer
+        }.items()
+    )
+
+    spawn_on_driver_start = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=driver,
+            on_start=lambda evt, ctx: [
+                TimerAction(period=5.0,
+                            actions=[controllers_launch]
+                            )
+            ]
+        )
+    )
+
+    shutdown_on_webots_exit = RegisterEventHandler(
+        OnProcessExit(
+            target_action=webots,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )
+    )
+
+    return [webots,
+            webots._supervisor, 
+            driver,
+            spawn_on_driver_start,
+            shutdown_on_webots_exit]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=_spawn_setup)
+    ])

src/iiwa_bringup/launch/webots.launch.py

@@ -1,245 +0,0 @@
-import xacro
-from pathlib import Path
-from launch.events import Shutdown
-from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    RegisterEventHandler,
-    OpaqueFunction,
-    EmitEvent,
-    TimerAction
-)
-from launch.event_handlers import OnProcessExit, OnProcessStart
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
-from launch_ros.actions import Node
-from webots_ros2_driver.webots_launcher import WebotsLauncher
-from webots_ros2_driver.urdf_spawner import URDFSpawner
-from webots_ros2_driver.webots_controller import WebotsController
-
-
-def _runtime_controller(robot_name: str,
-                        robot_description_str: str,
-                        transform_proto: str = "0 0 0",
-                        rotation_proto: str = "0 0 1 0",
-                        controller_manager_timer: int = 50):
-    tmo = ['--controller-manager-timeout', str(controller_manager_timer)]
-
-    jsb = Node(
-        package='controller_manager',
-        executable='spawner',
-        output='screen',
-        arguments=['joint_state_broadcaster'] + tmo,
-        parameters=[{'use_sim_time': False}],
-    )
-
-    jtc = Node(
-        package='controller_manager',
-        executable='spawner',
-        output='screen',
-        arguments=['joint_trajectory_controller'] + tmo, # название с yaml файла
-        parameters=[{'use_sim_time': False}],
-    )
-
-    spawner_urdf = URDFSpawner(
-        name=robot_name,
-        robot_description=robot_description_str,
-        translation=transform_proto,
-        rotation=rotation_proto
-    )
-
-    return [jsb, jtc, spawner_urdf]
-
-
-def _runtime_setup(context, *args, **kwargs):
-    # Получение параметров от пользователя при запуске launch
-    model = LaunchConfiguration('model').perform(context)
-    robot_name = LaunchConfiguration('robot_name').perform(context)
-    transform_proto = LaunchConfiguration('transform_proto').perform(context)
-    rotation_proto = LaunchConfiguration('rotation_proto').perform(context)
-    world_path = LaunchConfiguration('world').perform(context)
-    rviz_status = LaunchConfiguration('rviz').perform(context).lower() in ['true', '1', 'yes']
-    controller_manager = LaunchConfiguration('controller_manager').perform(context)
-
-    # Загрузка описания робота
-    model_path = Path(model)
-    suffix = model_path.suffix.lower()
-    if suffix == ".xacro":
-        robot_description_str = xacro.process_file(model, mappings={'name': str(robot_name)}).toxml()
-    elif suffix==".urdf":
-        robot_description_str = Path(model).read_text(encoding="utf-8")
-    else:
-        raise FileNotFoundError(f"Поддерживаются форматы файла: xacro/urdf")
-
-    # Запуск узлов
-    webots = WebotsLauncher(world=world_path, 
-                            ros2_supervisor=True)
-
-    rsp_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        name="robot_state_publisher",
-        output="screen",
-        parameters=[{
-            "robot_description": robot_description_str,
-            "use_sim_time": False
-        }]
-    )
-
-    # работа с драйверами
-    driver = WebotsController(
-        robot_name=robot_name,
-        parameters=[
-            {
-                "robot_description": model,
-                "use_sim_time": False,
-                "set_robot_state_publisher": False
-            },
-            LaunchConfiguration('controller').perform(context)
-        ],
-        respawn=True
-    )
-
-    # Во время запуска драйвера спавниться робот и ros2_controllers
-    spawn_on_driver_start = RegisterEventHandler(
-        event_handler=OnProcessStart(
-            target_action=driver,
-            on_start=lambda evt, ctx: [
-                TimerAction(period=2.0,
-                            actions=_runtime_controller(robot_name=robot_name,
-                                                        robot_description_str=robot_description_str,
-                                                        transform_proto=transform_proto,
-                                                        rotation_proto=rotation_proto,
-                                                        controller_manager_timer=controller_manager))
-            ]
-        )
-    )
-
-    # При закрытии webots закрываем все
-    shutdown_on_webots_exit = RegisterEventHandler(
-        OnProcessExit(
-            target_action=webots,
-            on_exit=[EmitEvent(event=Shutdown())]
-        )
-    )
-
-    setup = [webots, 
-             webots._supervisor, 
-             rsp_node, 
-             driver,
-             spawn_on_driver_start,
-             shutdown_on_webots_exit
-            ]
-
-
-    # Настройка rviz
-    if rviz_status:
-
-        rviz_config = PathJoinSubstitution([
-            FindPackageShare(kwargs['package_name']),
-            'config',
-            'rviz_iiwa.rviz'
-        ])
-
-        rviz = Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz2',
-            arguments=['-d', rviz_config],
-            output='log'
-        )
-
-        # При закрытии rviz закрываем все
-        shutdown_on_rviz_exit = RegisterEventHandler(
-            OnProcessExit(
-                target_action=rviz,
-                on_exit=[EmitEvent(event=Shutdown())]
-            )
-        )
-
-        setup += [rviz, shutdown_on_rviz_exit]
-
-    return setup
-
-
-def generate_launch_description():
-    package_name="iiwa_bringup"
-    description_pkg = "iiwa_description"
-
-    # Объявление аргументов командной строки
-    declare_model_arg = DeclareLaunchArgument(
-        name="model",
-        default_value=PathJoinSubstitution([
-            FindPackageShare(description_pkg),
-            "urdf",
-            "iiwa7.urdf.xacro"
-        ]),
-        description="Path to robot xacro or urdf file (used to build robot_description)."
-    )
-
-    declare_robot_name_arg = DeclareLaunchArgument(
-        name="robot_name",
-        default_value="iiwa7",
-        description="Robot name (used for TF and naming spawned robot)."
-    )
-
-    declare_world_arg = DeclareLaunchArgument(
-        name="world",
-        default_value=PathJoinSubstitution([
-            FindPackageShare(description_pkg),
-            "worlds",
-            "iiwa.wbt"
-        ]),
-        description="Path to the Webots world (.wbt) to launch."
-    )
-
-    declare_controller_arg = DeclareLaunchArgument(
-        name="controller",
-        default_value=PathJoinSubstitution([
-            FindPackageShare(package_name),
-            "config",
-            "iiwa_controller.yaml"
-        ]),
-        description="Path to controllers YAML file (used by spawner and controller_manager)."
-    )
-
-    declare_transform_arg = DeclareLaunchArgument(
-        name="transform_proto",
-        default_value="-0.25 0 0.79",
-        description="Translation applied when spawning the robot in the Webots world (x y z)."
-    )
-
-    declare_rotation_arg = DeclareLaunchArgument(
-        name="rotation_proto",
-        default_value="0 0 1 0",
-        description="Rotation (axis-angle) applied when spawning the robot in the Webots world."
-    )
-
-    declare_rviz_arg = DeclareLaunchArgument(
-        name="rviz",
-        default_value="0",
-        description="If true|1|yes then launch RViz and joint_state_publisher_gui instead of controllers."
-    )
-
-    declare_controller_manager_arg = DeclareLaunchArgument(
-        name="controller_manager",
-        default_value="50",
-        description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout)."
-    )
-
-    runtime_setup = OpaqueFunction(
-        function=_runtime_setup,
-        kwargs={'package_name': package_name}
-    )
-
-    return LaunchDescription([
-        declare_model_arg,
-        declare_robot_name_arg,
-        declare_world_arg,
-        declare_controller_arg,
-        declare_transform_arg,
-        declare_rotation_arg,
-        declare_rviz_arg,
-        declare_controller_manager_arg,
-        runtime_setup,
-    ])

src/iiwa_bringup/setup.py

@@ -1,65 +1,55 @@
 import os
-from setuptools import find_packages, setup
+from setuptools import setup
-
-EXCLUDES = {'.DS_Store', 'Thumbs.db'}
-EXCLUDE_DIRS = {'__pycache__', '.pytest_cache', '.git', '.idea'}
-
-def data_files_from_tree(src_dir: str, dst_root: str) -> list:
-    """
-    Собирает data_files в формате, подходящем для setuptools.
-    Возвращает список пар (dst_path, [file1, file2, ...]).
-    """
-    entries = []
-
-    for root, dirs, files in os.walk(src_dir):
-        # исключаем служебные директории
-        dirs[:] = [d for d in dirs if d not in EXCLUDE_DIRS]
-
-        file_list = [os.path.join(root, f) for f in files if f not in EXCLUDES]
-        if not file_list:
-            continue
-
-        rel = os.path.relpath(root, src_dir)
-        dst_dir = os.path.join(dst_root, rel) if rel != '.' else dst_root
-        entries.append((dst_dir, file_list))
-
-    return entries
 
 package_name = 'iiwa_bringup'
 
-resource_intsall_root = f"share/{package_name}/resource"
+# Собираем список пакетов: основной + вложенные (если они есть).
-launch_intsall_root = f"share/{package_name}/launch"
+packages = [package_name]
-config_intsall_root = f"share/{package_name}/config"
+# Если есть подпапка utils с __init__.py — зарегистрируем её как iiwa_bringup.utils
+if os.path.isdir('utils') and os.path.isfile(os.path.join('utils', '__init__.py')):
+    packages.append(f'{package_name}.utils')
 
+# Соответствие имени пакета -> директория на диске.
+# iiwa_bringup -> текущая папка '.'
+# iiwa_bringup.utils -> ./utils
+package_dir = {
+    package_name: '.',
+}
+if f'{package_name}.utils' in packages:
+    package_dir[f'{package_name}.utils'] = os.path.join('.', 'utils')
 
-data_files = [
+def data_files_from_tree(src_dir: str, dst_root: str) -> list:
-    ('share/' + package_name, ['package.xml']),
+    entries = []
-]
+    if not os.path.isdir(src_dir):
-
+        return entries
-data_files += data_files_from_tree('resource', resource_intsall_root)
+    EXCLUDE_DIRS = {'.git', '__pycache__', '.pytest_cache', '.idea'}
-data_files += data_files_from_tree('launch', launch_intsall_root)
+    EXCLUDES = {'.DS_Store', 'Thumbs.db'}
-data_files += data_files_from_tree('config', config_intsall_root)
+    for root, dirs, files in os.walk(src_dir):
-
+        dirs[:] = [d for d in dirs if d not in EXCLUDE_DIRS]
+        file_list = [os.path.join(root, f) for f in files if f not in EXCLUDES]
+        if not file_list:
+            continue
+        rel = os.path.relpath(root, src_dir)
+        dst_dir = os.path.join(dst_root, rel) if rel != '.' else dst_root
+        entries.append((dst_dir, file_list))
+    return entries
 
+data_files = [(f'share/{package_name}', ['package.xml'])]
+data_files += data_files_from_tree('launch', f'share/{package_name}/launch')
+data_files += data_files_from_tree('config', f'share/{package_name}/config')
+data_files += data_files_from_tree('resource', f'share/{package_name}/resource')
 
 setup(
     name=package_name,
     version='0.0.1',
-    packages=find_packages(exclude=['test']),
+    packages=packages,
+    package_dir=package_dir,
+    include_package_data=True,
     data_files=data_files,
     install_requires=['setuptools'],
-    zip_safe=True,
+    zip_safe=False,
     maintainer='Grabar Daniil',
     maintainer_email='grabardm@ml-dev.ru',
-    description='TODO: Package description',
+    description='iiwa bringup package',
     license='Apache-2.0',
-    extras_require={
-        'test': [
-            'pytest',
-        ],
-    },
-    entry_points={
-        'console_scripts': [
-        ],
-    },
 )

src/iiwa_bringup/utils/converter.py

@@ -0,0 +1,12 @@
+import xacro
+from pathlib import Path
+
+
+def load_robot_description(model_path: Path, robot_name: str) -> str:
+    suffix = Path(model_path).suffix.lower()
+    if suffix == ".xacro":
+        return xacro.process_file(model_path, mappings={'name': str(robot_name)}).toxml()
+    elif suffix == ".urdf":
+        return Path(model_path).read_text(encoding="utf-8")
+    else:
+        raise FileNotFoundError(f"Supported file formats: xacro/urdf")