feat: add release-to-move TCP gizmo control in Webots

Add a native Webots marker for setting the active tool's TCP position and
orientation. Dragging edits the preview; releasing the mouse submits one
complete, checked joint trajectory with configured TCP speed limits.
Unreachable targets reset the marker to the current TCP.

Preserve coordinate, joint, and control-file commands through motion
reservations and a controller action proxy. Give the gizmo lowest priority
and finish its committed trajectory before dispatching queued regular
commands, including waits longer than the former two-second service timeout.

Place the patron TCP at its working end, 55.2 mm along local Z, and use the
flange TCP when no tool is attached. Add digital_twin.tcp_gizmo configuration,
ROS interfaces, launch integration, and regression coverage.

Document operation, configuration, tool attachment, speed limits, and
troubleshooting in both Russian and English.

Validation: 42 Python tests and 8 C++ tests passed. Webots regressions covered
one action per release, translation and rotation with and without a tool,
unreachable targets, and ordinary commands waiting for gizmo completion.

src/iiwa_bringup/launch/supported/moveit_nodes.py

@@ -1,5 +1,6 @@
 from launch_ros.actions import Node
 from moveit_configs_utils import MoveItConfigsBuilder
+from .tcp_gizmo_nodes import make_tcp_gizmo_nodes
 
 
 def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
@@ -46,8 +47,9 @@ def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
                 "default_frame": settings.planning.default_frame,
                 "default_planner": settings.planning.default_planner,
                 "planning_attempts": settings.planning.planning_attempts,
+                "tcp_gizmo_enabled": use_sim_time and settings.digital_twin.tcp_gizmo.enabled,
             },
         ],
     )
 
-    return moveit_configs, [move_group, move_to_pose_server]
+    return moveit_configs, [move_group, move_to_pose_server, *make_tcp_gizmo_nodes(settings, common_params, use_sim_time)]

src/iiwa_bringup/launch/supported/tcp_gizmo_nodes.py

@@ -0,0 +1,67 @@
+"""TCP teleoperation exists only in a simulated, explicitly enabled launch."""
+from dataclasses import asdict
+import tempfile
+import xml.etree.ElementTree as ET
+
+import yaml
+from launch_ros.actions import Node
+from webots_ros2_driver.webots_controller import WebotsController
+
+
+def make_tcp_gizmo_nodes(settings, common_params, simulate):
+    cfg = settings.digital_twin.tcp_gizmo
+    if not simulate or not cfg.enabled:
+        return []
+
+    # The robot's Webots placement maps base_link into the Webots world.
+    robot = ET.Element('robot', name='tcp_gizmo')
+    ET.SubElement(robot, 'link', name='gizmo_base')
+    plugin = ET.SubElement(ET.SubElement(robot, 'webots'), 'plugin',
+                           type='iiwa_utils.tcp_gizmo.WebotsTcpGizmo')
+    for key, value in {
+        'translation': settings.digital_twin.webots.transform,
+        'rotation': settings.digital_twin.webots.rotation,
+        'frame': 'base_link',
+        'state_timeout': cfg.state_timeout,
+    }.items():
+        ET.SubElement(plugin, key).text = str(value)
+    with tempfile.NamedTemporaryFile(mode='w', prefix='iiwa_tcp_gizmo_', suffix='.yaml', delete=False) as stream:
+        yaml.safe_dump({'/**': {'ros__parameters': {
+            'robot_description': ET.tostring(robot, encoding='unicode'),
+            'set_robot_state_publisher': False,
+            'use_sim_time': True,
+        }}}, stream)
+        driver_params = stream.name
+
+    servo = {
+        'move_group_name': settings.planning.planning_group,
+        'publish_period': cfg.publish_period,
+        'max_expected_latency': max(0.1, 3 * cfg.publish_period),
+        'command_in_type': 'speed_units',
+        'scale': {'linear': cfg.max_linear_speed, 'rotational': cfg.max_angular_speed},
+        'incoming_command_timeout': cfg.command_timeout,
+        'is_primary_planning_scene_monitor': False,
+        'monitored_planning_scene_topic': '/monitored_planning_scene',
+        'joint_topic': '/joint_states',
+        'check_collisions': True,
+        'check_octomap_collisions': False,
+        'publish_joint_positions': True,
+        'publish_joint_velocities': False,
+        'use_smoothing': True,
+        'joint_limit_margins': [0.05],
+    }
+    return [
+        Node(package='iiwa_planning', executable='tcp_gizmo', name='tcp_gizmo',
+             output='screen', parameters=[*common_params, {
+                 **asdict(cfg), 'simulation': True,
+                 # Tool xacros define tcp at the working end; without a tool
+                 # it coincides with link_ee. General pose commands may use a
+                 # different planning.pose_link, but the handle controls TCP.
+                 'pose_link': 'tcp',
+                 'planning_group': settings.planning.planning_group,
+                 'default_frame': 'base_link', 'moveit_servo': servo,
+             }]),
+        Node(package='iiwa_utils', executable='tcp_gizmo_spawner', output='screen'),
+        WebotsController(robot_name='tcp_gizmo',
+                         parameters=[{'use_sim_time': True}, driver_params]),
+    ]

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

@@ -22,6 +22,7 @@ def _spawn_setup(context, *args, **kwargs):
     world = LaunchConfiguration("world").perform(context)
     controller = LaunchConfiguration("controller").perform(context)
     initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context)
+    gizmo_enabled = LaunchConfiguration("tcp_gizmo_enabled", default="false").perform(context) == "true"
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -64,6 +65,10 @@ def _spawn_setup(context, *args, **kwargs):
 
     driver = WebotsController(
         robot_name=robot_name,
+        remappings=[
+            ('/iiwa_arm_controller/joint_trajectory', '/cobot/tcp_gizmo/controller_command'),
+            ('/iiwa_arm_controller/follow_joint_trajectory', '/cobot/tcp_gizmo/follow_joint_trajectory'),
+        ] if gizmo_enabled else [],
         parameters=[
             {"use_sim_time": True},  # dict → triggers --ros-args prefix
             params_file,             # file → --params-file (robot_description + controllers)