feat: refactor launch files to modularize node creation and improve simulation support

src/iiwa_bringup/launch/iiwa.launch.py

@@ -1,40 +1,26 @@
-import json
+import os
-from dataclasses import asdict
+import sys
+
+sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
 
 from launch import LaunchDescription
-from launch.actions import (
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
-    DeclareLaunchArgument,
-    EmitEvent,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.events import Shutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-from moveit_configs_utils import MoveItConfigsBuilder
-from webots_ros2_driver.webots_controller import WebotsController
 
 from iiwa_utils import converter, setting_loader
-from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf  # type: ignore
 
-
+from supported.moveit_nodes import make_moveit_nodes
-def _foxglove_params(fg, use_sim_time: bool) -> dict:
+from supported.rviz_nodes import make_rviz_nodes
-    params = asdict(fg)
+from supported.simulation_nodes import make_simulation_nodes
-    params.pop("enabled")
+from supported.optional_nodes import make_foxglove_node, make_web_server_node
-    params["use_sim_time"] = use_sim_time
-
-    return params
 
 
 def _runtime_setup(context, *args, **kwargs):
     setup = []
 
-    # Настройка параметров
     simulate = LaunchConfiguration("simulate").perform(context) in ("true", "1", "yes")
 
     settings = setting_loader.build_settings(
@@ -52,13 +38,13 @@ def _runtime_setup(context, *args, **kwargs):
     )
 
     description_path = settings.robot.description
+    use_sim_time = simulate
 
     if simulate:
         xacro_args = {
             "initial_positions_file": settings.controller.moveit.initial_positions,
             "simulate": "true",
         }
-        use_sim_time = True
     else:
         xacro_args = {
             "initial_positions_file": settings.controller.moveit.initial_positions,
@@ -67,7 +53,6 @@ def _runtime_setup(context, *args, **kwargs):
             "simulate": "false",
             "joint_position_tau": str(settings.robot.joint_position_tau),
         }
-        use_sim_time = False
 
     robot_description = converter.load_robot_description(
         model_path=description_path,
@@ -75,8 +60,8 @@ def _runtime_setup(context, *args, **kwargs):
         xacro_args=xacro_args,
     )
 
-    # Вызов нод
+    # Robot State Publisher
-    rsp_node = Node(
+    setup.append(Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         name="robot_state_publisher",
@@ -85,22 +70,15 @@ def _runtime_setup(context, *args, **kwargs):
             {"robot_description": robot_description},
             {"use_sim_time": use_sim_time},
         ],
-    )
+    ))
 
-    setup += [rsp_node]
+    # Webots симуляция
-
-    # webots spawn
     if simulate:
-        webots_launch = IncludeLaunchDescription(
+        setup.append(IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
+                PathJoinSubstitution([
-                    [
+                    FindPackageShare("iiwa_bringup"), "launch", "supported", "webots_spawn.launch.py",
-                        FindPackageShare("iiwa_bringup"),
+                ])
-                        "launch",
-                        "supported",
-                        "webots_spawn.launch.py",
-                    ]
-                )
             ),
             launch_arguments={
                 "robot_name": str(settings.robot.name),
@@ -112,11 +90,11 @@ def _runtime_setup(context, *args, **kwargs):
                 "controller": str(settings.controller.controller_path),
                 "initial_positions_file": str(settings.controller.moveit.initial_positions),
             }.items(),
-        )
+        ))
 
-        setup += [webots_launch]
+        setup += make_simulation_nodes(settings)
 
-    # Controller launch
+    # Controllers
     if simulate:
         controller_args = {
             "robot_name": settings.robot.name,
@@ -128,180 +106,52 @@ def _runtime_setup(context, *args, **kwargs):
             "rotation": str(settings.digital_twin.webots.rotation),
             "controller_timer": str(settings.digital_twin.webots.controller_timer),
         }
-
-        if settings.digital_twin.webots.cameras:
-            # Спавн камеры
-            camera_spawner_node = Node(
-                package="iiwa_utils",
-                executable="camera_spawner",
-                name="camera_spawner",
-                output="screen",
-                parameters=[{
-                    "camera_configs": json.dumps(settings.digital_twin.webots.cameras)
-                }],
-            )
-            setup.append(camera_spawner_node)
-
-            # WebotsController для каждой камеры
-            for cam_path in settings.digital_twin.webots.cameras:
-                cam_cfg = load_camera_config(cam_path)
-                urdf = build_ros_urdf(cam_cfg)
-
-                camera_controller = WebotsController(
-                    robot_name=f"{cam_cfg.name}_robot",
-                    parameters=[{
-                        "robot_description": urdf,
-                        "use_sim_time": True,
-                        "set_robot_state_publisher": False,
-                    }],
-                    respawn=True,
-                )
-                setup.append(camera_controller)
-
     else:
         controller_args = {
             "robot_name": settings.robot.name,
             "description": description_path,
             "initial_positions_file": settings.controller.moveit.initial_positions,
             "controller_path": settings.controller.controller_path,
+            "simulate": "false",
             "transform": str(settings.digital_twin.webots.transform),
             "rotation": str(settings.digital_twin.webots.rotation),
-            "simulate": "false",
             "controller_timer": str(settings.digital_twin.webots.controller_timer),
             "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
             "joint_position_tau": str(settings.robot.joint_position_tau),
             "controller": settings.robot.active_controller,
         }
 
-    controllers_launch = IncludeLaunchDescription(
+    setup.append(IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
+            PathJoinSubstitution([
-                [
+                FindPackageShare("iiwa_bringup"), "launch", "supported", "controllers.launch.py",
-                    FindPackageShare("iiwa_bringup"),
+            ])
-                    "launch",
-                    "supported",
-                    "controllers.launch.py",
-                ]
-            )
         ),
         launch_arguments={k: str(v) for k, v in controller_args.items()}.items(),
-    )
+    ))
 
-    # Moveit launch
+    # MoveIt
-    moveit_configs = (
+    moveit_configs, moveit_nodes = make_moveit_nodes(settings, robot_description, use_sim_time)
-        MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
+    setup += moveit_nodes
-        .robot_description(
-            file_path=description_path,
-            mappings={
-                "initial_positions_file": settings.controller.moveit.initial_positions
-            },
-        )
-        .robot_description_semantic(file_path=settings.controller.moveit.srdf)
-        .robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
-        .joint_limits(file_path=settings.controller.moveit.joint_limits)
-        .pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
-        .trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
-        .moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
-        .to_moveit_configs()
-    )
 
-    move_group = Node(
+    # RViz
-        package="moveit_ros_move_group",
+    setup += make_rviz_nodes(settings, moveit_configs, joint_limits_ros2, kinematics_ros2, use_sim_time)
-        executable="move_group",
-        output="screen",
-        parameters=[
-            moveit_configs.to_dict(),
-            {"robot_description": robot_description},
-            {"use_sim_time": use_sim_time},
-        ],
-    )
-
-    move_to_pose_server = Node(
-        package="iiwa_planning",
-        executable="move_to_pose_server",
-        output="screen",
-        parameters=[
-            moveit_configs.to_dict(),
-            {"robot_description": robot_description},
-            {"use_sim_time": use_sim_time},
-            {
-                "pose_link": settings.planning.pose_link,
-                "planning_group": settings.planning.planning_group,
-                "default_frame": settings.planning.default_frame,
-                "default_planner": settings.planning.default_planner,
-                "planning_attempts": settings.planning.planning_attempts,
-            },
-        ],
-    )
-
-    # Rviz launch
-    rviz_launch = Node(
-        condition=IfCondition(LaunchConfiguration("rviz")),
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        arguments=["-d", settings.digital_twin.rviz.config],
-        output="log",
-        parameters=[
-            moveit_configs.robot_description,
-            moveit_configs.robot_description_semantic,
-            moveit_configs.planning_pipelines,
-            joint_limits_ros2,
-            kinematics_ros2,
-            {"use_sim_time": use_sim_time},
-        ],
-    )
-
-    shutdown_on_rviz_exit = RegisterEventHandler(
-        OnProcessExit(
-            target_action=rviz_launch,
-            on_exit=[EmitEvent(event=Shutdown())],
-        )
-    )
-
-    setup += [
-        controllers_launch,
-        move_group,
-        move_to_pose_server,
-        rviz_launch,
-        shutdown_on_rviz_exit,
-    ]
 
+    # Опциональные сервисы
     if settings.foxglove.enabled:
-        foxglove_bridge = Node(
+        setup.append(make_foxglove_node(settings, use_sim_time))
-            package="foxglove_bridge",
-            executable="foxglove_bridge",
-            output="screen",
-            name="foxglove_bridge",
-            parameters=[_foxglove_params(settings.foxglove, use_sim_time)]
-        )
-
-        setup += [foxglove_bridge]
 
     if settings.web.enabled:
-        web_server = Node(
+        setup.append(make_web_server_node(settings, use_sim_time))
-            package="iiwa_web",
-            executable="iiwa_web_server",
-            output="screen",
-            name="iiwa_web_server",
-            parameters=[{
-                "host": settings.web.host,
-                "port": settings.web.port,
-                "endpoints_path": settings.web.endpoints,
-                "joint_limits_path": settings.web.joint_limits,
-                "use_sim_time": use_sim_time,
-            }],
-        )
-
-        setup += [web_server]
 
     return setup
 
+
 def generate_launch_description():
     declare_simulate = DeclareLaunchArgument(
         name="simulate",
         default_value="false",
-        description="true = Gazebo симуляция, false = реальный робот через FRI",
+        description="true = Webots симуляция, false = реальный робот через FRI",
     )
 
     declare_rviz = DeclareLaunchArgument(
@@ -318,12 +168,9 @@ def generate_launch_description():
         description="Путь к файлу настроек",
     )
 
-    runtime_setup = OpaqueFunction(function=_runtime_setup)
-
     return LaunchDescription([
         declare_simulate,
         declare_rviz,
         declare_setting,
-        runtime_setup,
+        OpaqueFunction(function=_runtime_setup),
     ])
-

src/iiwa_bringup/launch/supported/moveit_nodes.py

@@ -0,0 +1,53 @@
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
+    """Возвращает (moveit_configs, [move_group, move_to_pose_server])."""
+    moveit_configs = (
+        MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
+        .robot_description(
+            file_path=settings.robot.description,
+            mappings={
+                "initial_positions_file": settings.controller.moveit.initial_positions
+            },
+        )
+        .robot_description_semantic(file_path=settings.controller.moveit.srdf)
+        .robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
+        .joint_limits(file_path=settings.controller.moveit.joint_limits)
+        .pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
+        .trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
+        .moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
+        .to_moveit_configs()
+    )
+
+    common_params = [
+        moveit_configs.to_dict(),
+        {"robot_description": robot_description},
+        {"use_sim_time": use_sim_time},
+    ]
+
+    move_group = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=common_params,
+    )
+
+    move_to_pose_server = Node(
+        package="iiwa_planning",
+        executable="move_to_pose_server",
+        output="screen",
+        parameters=[
+            *common_params,
+            {
+                "pose_link": settings.planning.pose_link,
+                "planning_group": settings.planning.planning_group,
+                "default_frame": settings.planning.default_frame,
+                "default_planner": settings.planning.default_planner,
+                "planning_attempts": settings.planning.planning_attempts,
+            },
+        ],
+    )
+
+    return moveit_configs, [move_group, move_to_pose_server]

src/iiwa_bringup/launch/supported/optional_nodes.py

@@ -0,0 +1,33 @@
+from dataclasses import asdict
+
+from launch_ros.actions import Node
+
+
+def make_foxglove_node(settings, use_sim_time: bool) -> Node:
+    params = asdict(settings.foxglove)
+    params.pop("enabled")
+    params["use_sim_time"] = use_sim_time
+
+    return Node(
+        package="foxglove_bridge",
+        executable="foxglove_bridge",
+        output="screen",
+        name="foxglove_bridge",
+        parameters=[params],
+    )
+
+
+def make_web_server_node(settings, use_sim_time: bool) -> Node:
+    return Node(
+        package="iiwa_web",
+        executable="iiwa_web_server",
+        output="screen",
+        name="iiwa_web_server",
+        parameters=[{
+            "host": settings.web.host,
+            "port": settings.web.port,
+            "endpoints_path": settings.web.endpoints,
+            "joint_limits_path": settings.web.joint_limits,
+            "use_sim_time": use_sim_time,
+        }],
+    )

src/iiwa_bringup/launch/supported/rviz_nodes.py

@@ -0,0 +1,35 @@
+from launch.actions import EmitEvent, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def make_rviz_nodes(settings, moveit_configs, joint_limits_ros2, kinematics_ros2, use_sim_time: bool):
+    """Возвращает [rviz_launch, shutdown_on_rviz_exit]."""
+    rviz_launch = Node(
+        condition=IfCondition(LaunchConfiguration("rviz")),
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        arguments=["-d", settings.digital_twin.rviz.config],
+        output="log",
+        parameters=[
+            moveit_configs.robot_description,
+            moveit_configs.robot_description_semantic,
+            moveit_configs.planning_pipelines,
+            joint_limits_ros2,
+            kinematics_ros2,
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    shutdown_on_rviz_exit = RegisterEventHandler(
+        OnProcessExit(
+            target_action=rviz_launch,
+            on_exit=[EmitEvent(event=Shutdown())],
+        )
+    )
+
+    return [rviz_launch, shutdown_on_rviz_exit]

src/iiwa_bringup/launch/supported/simulation_nodes.py

@@ -0,0 +1,40 @@
+import json
+
+from launch_ros.actions import Node
+from webots_ros2_driver.webots_controller import WebotsController
+
+from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf  # type: ignore
+
+
+def make_simulation_nodes(settings) -> list:
+    """Возвращает ноды камер для симуляции: [camera_spawner, *camera_controllers].
+
+    Если камеры не заданы в настройках — возвращает пустой список.
+    """
+    if not settings.digital_twin.webots.cameras:
+        return []
+
+    camera_spawner = Node(
+        package="iiwa_utils",
+        executable="camera_spawner",
+        name="camera_spawner",
+        output="screen",
+        parameters=[{
+            "camera_configs": json.dumps(settings.digital_twin.webots.cameras)
+        }],
+    )
+
+    camera_controllers = []
+    for cam_path in settings.digital_twin.webots.cameras:
+        cam_cfg = load_camera_config(cam_path)
+        camera_controllers.append(WebotsController(
+            robot_name=f"{cam_cfg.name}_robot",
+            parameters=[{
+                "robot_description": build_ros_urdf(cam_cfg),
+                "use_sim_time": True,
+                "set_robot_state_publisher": False,
+            }],
+            respawn=True,
+        ))
+
+    return [camera_spawner, *camera_controllers]