Refactor launch files and update package descriptions for KUKA iiwa7, transitioning from Gazebo to Webots simulation

2026-04-09 13:52:28 +10:00
parent 58f246e1f2
commit 845f4ca89c
16 changed files with 147 additions and 274 deletions
@@ -4,6 +4,8 @@ from launch.substitutions import LaunchConfiguration
 from launch.event_handlers import OnProcessExit
 from launch.actions import RegisterEventHandler
 from launch_ros.actions import Node
+from webots_ros2_driver.urdf_spawner import URDFSpawner
+

 from iiwa_utils import converter

@@ -11,27 +13,32 @@ from iiwa_utils import converter
 def _setup_controllers(context, *args, **kwargs):
    robot_name = LaunchConfiguration("robot_name").perform(context)
    description = LaunchConfiguration("description").perform(context)
+    transform = LaunchConfiguration("transform").perform(context)
+    rotation = LaunchConfiguration("rotation").perform(context)
    initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context)
+    controller_timer = LaunchConfiguration("controller_timer").perform(context)
    controller_path = LaunchConfiguration("controller_path").perform(context)
    simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")

+    xacro_args = {"initial_positions_file": initial_positions_file}
+    if simulate:
+        xacro_args["simulate"] = "true"
+
    robot_description = converter.load_robot_description(
        model_path=description,
        robot_name=robot_name,
-        xacro_args={"initial_positions_file": initial_positions_file},
+        xacro_args=xacro_args,
    )

-    # СИМУЛЯЦИЯ (Gazebo)
+    # Webots
    if simulate:
+        tmo = ["--controller-manager-timeout", str(controller_timer)]
+
        jsb = Node(
            package="controller_manager",
            executable="spawner",
            output="screen",
-            arguments=[
-                "joint_state_broadcaster",
-                "--controller-manager", "/controller_manager",
-                "--controller-manager-timeout", "30",
-            ],
+            arguments=["joint_state_broadcaster"] + tmo,
            parameters=[{"use_sim_time": True}],
        )

@@ -39,24 +46,29 @@ def _setup_controllers(context, *args, **kwargs):
            package="controller_manager",
            executable="spawner",
            output="screen",
-            arguments=[
-                "iiwa_arm_controller",
-                "--controller-manager", "/controller_manager",
-                "--controller-manager-timeout", "30",
-            ],
+            arguments=["iiwa_arm_controller"] + tmo,
            parameters=[{"use_sim_time": True}],
        )

-        jtc_after_jsb = RegisterEventHandler(
-            OnProcessExit(
-                target_action=jsb,
-                on_exit=[jtc],
-            )
+        torque_controller_spawner = Node(
+            package="controller_manager",
+            executable="spawner",
+            output="screen",
+            arguments=["forward_torque_controller",
+                    "--inactive"] + tmo,
+            parameters=[{"use_sim_time": True}]
        )

-        return [jsb, jtc_after_jsb]
+        spawner_urdf = URDFSpawner(
+            name=robot_name,
+            robot_description=robot_description,
+            translation=transform,
+            rotation=rotation,
+        )

-    # РЕАЛЬНЫЙ РОБОТ (FRI)
+        return [jsb, jtc, torque_controller_spawner, spawner_urdf]
+
+    # FRI
    else:
        ros2_control_node = Node(
            package="controller_manager",
@@ -114,4 +126,4 @@ def _setup_controllers(context, *args, **kwargs):


 def generate_launch_description():
-    return LaunchDescription([OpaqueFunction(function=_setup_controllers)])
+    return LaunchDescription([OpaqueFunction(function=_setup_controllers)])
@@ -1,90 +0,0 @@
-from ament_index_python import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import (
-    IncludeLaunchDescription,
-    OpaqueFunction,
-)
-from launch_ros.actions import Node
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
-
-
-def _spawn_setup(context, *args, **kwargs):
-    robot_name = LaunchConfiguration("robot_name").perform(context)
-    world = LaunchConfiguration("world").perform(context)
-    gazebo_config = LaunchConfiguration("gazebo_config").perform(context)
-    simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
-    # transform = LaunchConfiguration("transform").perform(context)
-    
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("ros_gz_sim"),
-                    "launch",
-                    "gz_sim.launch.py",
-                ]
-            )
-        ),
-        launch_arguments={
-            "gz_args": f"-r {world} --render-engine ogre",
-            "on_exit_shutdown": "true",
-            }.items(),
-    )
-
-    # Table Spawner
-    spawn_table = Node(
-        package="ros_gz_sim",
-        executable="create",
-        name="spawn_table",
-        output="screen",
-        arguments=[
-            "-file", str(
-                get_package_share_directory("iiwa_description")
-                + "/urdf/table/table.urdf"
-            ),
-            "-name",  "iiwa_table",
-            "-x", "0.0",
-            "-y", "0.0",
-            "-z", "0.0",
-        ],
-    )
-
-    # Robot Spawner
-    # TODO: добавить аргументы для transform и rotation
-    spawn_robot = Node(
-        package="ros_gz_sim",
-        executable="create",
-        name="spawn_iiwa7",
-        output="screen",
-        arguments=[
-            "-topic", "/robot_description",
-            "-name", robot_name,
-            "-x", "-0.25",
-            "-y", "0.0",
-            "-z", "0.81"
-        ],
-    )
-
-    gz_bridge = Node(
-        package="ros_gz_bridge",
-        executable="parameter_bridge",
-        name="gz_bridge",
-        output="screen",
-        parameters=[{
-            "config_file": gazebo_config,
-            "use_sim_time": simulate
-        }],
-    )
-
-    return [
-        gazebo,
-        spawn_table,
-        spawn_robot, 
-        gz_bridge
-    ]
-
-
-def generate_launch_description():
-    return LaunchDescription([OpaqueFunction(function=_spawn_setup)])
@@ -1,64 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import OpaqueFunction
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from webots_ros2_driver.urdf_spawner import URDFSpawner
-
-from iiwa_utils import converter
-
-
-def _setup_controllers(context, *args, **kwargs):
-    robot_name = LaunchConfiguration("robot_name").perform(context)
-    description = LaunchConfiguration("description").perform(context)
-    transform = LaunchConfiguration("transform").perform(context)
-    rotation = LaunchConfiguration("rotation").perform(context)
-    controller_timer = LaunchConfiguration("controller_timer").perform(context)
-    initial_positions_file = LaunchConfiguration("initial_positions_file").perform(
-        context
-    )
-
-    robot_description = converter.load_robot_description(
-        model_path=description,
-        robot_name=robot_name,
-        xacro_args={"initial_positions_file": initial_positions_file},
-    )
-
-    tmo = ["--controller-manager-timeout", str(controller_timer)]
-
-    jsb = Node(
-        package="controller_manager",
-        executable="spawner",
-        output="screen",
-        arguments=["joint_state_broadcaster"] + tmo,
-        parameters=[{"use_sim_time": True}],
-    )
-
-    jtc = Node(
-        package="controller_manager",
-        executable="spawner",
-        output="screen",
-        arguments=["iiwa_arm_controller"] + tmo,
-        parameters=[{"use_sim_time": True}],
-    )
-
-    torque_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        output="screen",
-        arguments=["forward_torque_controller",
-                   "--inactive"] + tmo,
-        parameters=[{"use_sim_time": True}]
-    )
-
-    spawner_urdf = URDFSpawner(
-        name=robot_name,
-        robot_description=robot_description,
-        translation=transform,
-        rotation=rotation,
-    )
-
-    return [jsb, jtc, torque_controller_spawner, spawner_urdf]
-
-
-def generate_launch_description():
-    return LaunchDescription([OpaqueFunction(function=_setup_controllers)])
@@ -1,30 +1,32 @@
 from launch import LaunchDescription
 from launch.actions import (
    EmitEvent,
-    IncludeLaunchDescription,
    OpaqueFunction,
    RegisterEventHandler,
-    TimerAction,
 )
-from launch.event_handlers import OnProcessExit, OnProcessStart
+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.substitutions import FindPackageShare
+from launch.substitutions import LaunchConfiguration
 from webots_ros2_driver.webots_controller import WebotsController
 from webots_ros2_driver.webots_launcher import WebotsLauncher

+from iiwa_utils import converter
+

 def _spawn_setup(context, *args, **kwargs):
    robot_name = LaunchConfiguration("robot_name").perform(context)
    description = LaunchConfiguration("description").perform(context)
    world = LaunchConfiguration("world").perform(context)
-    transform = LaunchConfiguration("transform").perform(context)
-    rotation = LaunchConfiguration("rotation").perform(context)
-    controller_timer = LaunchConfiguration("controller_timer").perform(context)
    controller = LaunchConfiguration("controller").perform(context)
-    initial_positions_file = LaunchConfiguration("initial_positions_file").perform(
-        context
+    initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context)
+
+    robot_description = converter.load_robot_description(
+        model_path=description,
+        robot_name=robot_name,
+        xacro_args={
+            "simulate": "true",
+            "initial_positions_file": initial_positions_file,
+        },
    )

    webots = WebotsLauncher(world=world, ros2_supervisor=True)
@@ -33,7 +35,7 @@ def _spawn_setup(context, *args, **kwargs):
        robot_name=robot_name,
        parameters=[
            {
-                "robot_description": description,
+                "robot_description": robot_description,
                "use_sim_time": True,
                "set_robot_state_publisher": False,
            },
@@ -42,36 +44,6 @@ def _spawn_setup(context, *args, **kwargs):
        respawn=True,
    )

-    controllers_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("iiwa_bringup"),
-                    "launch",
-                    "supported",
-                    "webots_controllers.launch.py",
-                ]
-            )
-        ),
-        launch_arguments={
-            "robot_name": robot_name,
-            "description": description,
-            "transform": transform,
-            "rotation": rotation,
-            "controller_timer": controller_timer,
-            "initial_positions_file": initial_positions_file,
-        }.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())])
    )
@@ -80,7 +52,6 @@ def _spawn_setup(context, *args, **kwargs):
        webots,
        webots._supervisor,
        driver,
-        spawn_on_driver_start,
        shutdown_on_webots_exit,
    ]