append spawn objects

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.

.gitignore

@@ -4,13 +4,15 @@ doc/lwc-doc/site/
 log/
 /install/
 /build/
-.vscode
-.idea
-venv
-.venv
-.claude
-.codex
-__pycache__
+.vscode/
+.idea/
+venv/
+.venv/
+.claude/
+.agents/
+.codex/
+__pycache__/
+.pytest_cache/
 
 *.egg-info
 **.FCBak

cobot-setting.yaml

@@ -1,15 +1,20 @@
 robot:
   name: "iiwa7"
-  ip: "192.170.10.2"
+  ip: "192.168.21.147"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.04   # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI
-  joint_velocity_tau: 0.01   # EMA фильтр скорости [с]: убирает выбросы конечных разностей
-  active_controller: "jtc"  # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController
+  joint_position_tau: 0.04  # EMA позиций FRI [с]; 0 отключает сглаживание
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 
 digital_twin:
+  tcp_gizmo:
+    enabled: true              # TCP-гизма в Webots; только в режиме симуляции
+    publish_period: 0.032      # период управления [с]
+    command_timeout: 0.3       # максимальный возраст цели при отпускании мыши [с]
+    state_timeout: 0.5         # допустимый возраст состояния робота [с]
+    max_linear_speed: 0.50     # верхний предел скорости TCP [м/с]
+    max_angular_speed: 0.4     # скорость вращения TCP [рад/с]
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     # world: pkg://iiwa_description/worlds/simple_world.wbt
@@ -17,6 +22,26 @@ digital_twin:
     rotation: "0 0 1 0"
     controller_timer: "50"
 
+    objects:
+      enabled: true
+      count: 1                    # точное общее количество; модели могут повторяться
+      models:
+        # - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+        #   rotation_deg: [0, 90, 0]  # X, Y, Z в градусах, относительно робота
+        # - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+        #   rotation_deg: [90, 0, 0]
+        - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+          rotation_deg: [90, 0, 0]
+        # - path: pkg://iiwa_description/objects/hammer/hammer4.obj
+        #   rotation_deg: [1.57, 0, 0]
+        # - path: pkg://iiwa_description/objects/hammer/hammer5.obj
+        #   rotation_deg: [1.57, 0, 0]
+      area:                       # метры в системе основания робота, до падения
+        x: [0.3, 0.7]
+        y: [-0.3, 0.3]
+        z: [0.1, 0.1]             # высота нижней точки модели
+
+
     cameras:
     - pkg://iiwa_config/config/cameras/d455_top.yaml
 

cobot/commands/robot_setup.py

@@ -143,13 +143,6 @@ _BLOCKS: List[_Block] = [
             _Field("fri_cycle_ms", "Цикл FRI (мс):", "10",
                    note="5 мс = 200 Гц, 10 мс = 100 Гц",
                    options=["10", "5"]),
-            _Field("active_controller", "Активный ROS-контроллер:", "jtc",
-                   note="jtc = JointTrajectoryController (MoveIt), forward = ForwardCommandController",
-                   options=["jtc", "forward"]),
-            _Field("joint_position_tau", "EMA tau фильтра положения (с):", "0.04",
-                   note="Сглаживает команды положения перед отправкой в FRI"),
-            _Field("joint_velocity_tau", "EMA tau фильтра скорости (с):", "0.01",
-                   note="Убирает выбросы из оценки скорости конечной разностью"),
         ],
     ),
 ]

cobot/commands/update.py

@@ -4,13 +4,23 @@ import argparse
 import subprocess
 from pathlib import Path
 
-from cobot import process, ui
+from cobot import process
 from cobot.ui import done
 from cobot.process import StepProgress
 
 _PROJECT_DIR = Path(__file__).parent.parent.parent
 
 
+def _plain(message: str) -> None:
+    """Print after replacing this command's own uv environment.
+
+    Rich modules loaded before ``uv tool install`` can no longer be complete after
+    uv replaces the editable tool environment. Do not render with Rich after that
+    command returns.
+    """
+    print(message, flush=True)
+
+
 def _git(*args: str) -> str:
     """Run a git command in the project dir and return its stripped stdout.
     Запускает git-команду в директории проекта и возвращает обрезанный stdout.
@@ -18,13 +28,12 @@ def _git(*args: str) -> str:
     return subprocess.check_output(["git", *args], cwd=_PROJECT_DIR, text=True).strip()
 
 
-def _update() -> None:
+def _update() -> int:
     """Fetch the current branch, show incoming commits, pull, and reinstall the cobot CLI.
     Progress: fetch (0-30 %), pull (30-80 %), reinstall (80-100 %).
     Получает текущую ветку, показывает входящие коммиты, делает pull и переустанавливает CLI.
     Прогресс: fetch (0-30 %), pull (30-80 %), переустановка (80-100 %).
     """
-    ok, fail_msg = True, ""
     with StepProgress("Обновление проекта") as p:
         try:
             branch = _git("rev-parse", "--abbrev-ref", "HEAD")
@@ -32,17 +41,15 @@ def _update() -> None:
 
             p.set(0, "Получение с удалённого репозитория...")
             rc = process.stream(["git", "fetch", "origin"], cwd=str(_PROJECT_DIR), on_line=p.log)
-            if rc not in (0, -9, -15):
-                done(False, "git fetch завершился с ошибкой")
-                return
+            if rc != 0:
+                done(False, f"git fetch завершился с кодом {rc}")
+                return 1
             p.set(30)
 
             behind = _git("rev-list", f"HEAD..origin/{branch}", "--count")
             if behind == "0":
-                p.set(100, "Уже актуально")
-                done(True, "Уже актуальная версия")
-                return
-
+                p.log("Исходники уже актуальны; проверка установки CLI...")
+            else:
                 p.raw(f"\n[bold]{behind} новых коммит(ов):[/bold]")
                 for line in _git("log", f"HEAD..origin/{branch}", "--oneline").splitlines():
                     p.log(line)
@@ -50,24 +57,33 @@ def _update() -> None:
                 p.set(30, "Применение изменений...")
                 rc = process.stream(["git", "pull", "origin", branch],
                                    cwd=str(_PROJECT_DIR), on_line=p.log)
-            if rc not in (0, -9, -15):
-                done(False, "git pull завершился с ошибкой")
-                return
+                if rc != 0:
+                    done(False, f"git pull завершился с кодом {rc}")
+                    return 1
             p.set(80)
+        except (subprocess.CalledProcessError, OSError) as exc:
+            done(False, str(exc))
+            return 1
 
-            p.set(80, "Переустановка cobot CLI...")
-            p.raw("\n[cyan]▸[/cyan] Переустановка cobot CLI...")
-            rc = process.stream(["uv", "tool", "install", "--editable", str(_PROJECT_DIR)],
-                               on_line=p.log)
-            if rc in (0, -9, -15):
-                p.raw("[green]✓[/green] cobot переустановлен")
-            else:
-                p.raw("[yellow]Предупреждение:[/yellow] переустановка не удалась")
-            p.set(100, "Готово")
-        except subprocess.CalledProcessError as exc:
-            ok, fail_msg = False, str(exc)
-
-    done(ok, "Проект обновлён" if ok else fail_msg)
+    # uv can replace the Rich package of this running CLI. Keep this stage outside
+    # StepProgress and only use built-in print afterwards.
+    _plain("\n▸ Переустановка cobot CLI...")
+    try:
+        rc = process.stream(
+            ["uv", "tool", "install", "--editable", str(_PROJECT_DIR)],
+            on_line=lambda line: _plain(f"  {line}"),
+        )
+    except OSError as exc:
+        _plain(f"✗ Не удалось запустить установщик: {exc}")
+        return 1
+    if rc != 0:
+        _plain(
+            f"✗ Установка CLI завершилась с кодом {rc}. Исходники обновлены, "
+            "но установка не завершена. Сохраните вывод выше и повторите cobot update."
+        )
+        return 1
+    _plain("✓ Проект обновлён и cobot переустановлен")
+    return 0
 
 
 def register(subparsers: argparse._SubParsersAction) -> None:
@@ -76,4 +92,4 @@ def register(subparsers: argparse._SubParsersAction) -> None:
 
 
 def run(args: argparse.Namespace) -> None:
-    _update()
+    raise SystemExit(_update())

cobot/test/test_update.py

@@ -0,0 +1,71 @@
+"""Update recovery checks; external git and installer operations are isolated."""
+
+import io
+import unittest
+from unittest.mock import patch
+
+from rich.console import Console
+
+from cobot.commands import update
+
+
+class UpdateTests(unittest.TestCase):
+    def run_update(self, codes, behind="1"):
+        output = io.StringIO()
+        commands = []
+
+        def stream(command, **kwargs):
+            commands.append(command)
+            result = next(codes, 0)
+            if isinstance(result, Exception):
+                raise result
+            return result
+
+        def git(*args):
+            return {"rev-parse": "controller", "rev-list": behind,
+                    "log": "abc123 Update CLI"}[args[0]]
+
+        console = Console(file=output, color_system=None)
+        with patch.object(update, "_git", side_effect=git), \
+                patch.object(update.process, "stream", side_effect=stream), \
+                patch.object(update, "_plain"), \
+                patch("cobot.process.console", console), \
+                patch("cobot.ui.console", console):
+            code = update._update()
+        return code, output.getvalue(), commands
+
+    def test_install_failure_reports_failure(self):
+        for install_code in (1, -9, -15):
+            with self.subTest(install_code=install_code):
+                code, output, _ = self.run_update(iter([0, 0, install_code]))
+                self.assertEqual(code, 1)
+                self.assertNotIn("Проект обновлён", output)
+
+    def test_current_checkout_still_retries_installation(self):
+        code, _, commands = self.run_update(iter([0, 0]), behind="0")
+        self.assertEqual([command[:2] for command in commands],
+                         [["git", "fetch"], ["uv", "tool"]])
+        self.assertEqual(code, 0)
+
+    def test_killed_git_stops_before_next_step(self):
+        for codes, expected_count in [([-9], 1), ([0, -15], 2)]:
+            with self.subTest(codes=codes):
+                code, output, commands = self.run_update(iter(codes))
+                self.assertNotEqual(code, 0)
+                self.assertEqual(len(commands), expected_count)
+                self.assertNotIn("Проект обновлён", output)
+
+    def test_missing_installer_reports_failure_without_traceback(self):
+        code, _, _ = self.run_update(
+            iter([0, 0, FileNotFoundError("uv not found")]))
+        self.assertNotEqual(code, 0)
+
+    def test_command_exits_nonzero_after_failed_installation(self):
+        with patch.object(update, "_update", return_value=1):
+            with self.assertRaises(SystemExit) as result:
+                update.run(None)
+        self.assertEqual(result.exception.code, 1)
+
+
+if __name__ == "__main__":
+    unittest.main()

doc/lwc-doc/docs/getting-started/concepts/simulation.en.md

@@ -1,8 +1,5 @@
 # Simulation (Webots)
 
-!!! info "Work in progress"
-    A detailed simulator guide is being prepared.
-
 **Webots** is an open-source robot simulator. LWC uses it as a digital twin of the KUKA LBR IIWA 7, allowing control algorithms to be developed and debugged without a physical robot.
 
 ## Key features
@@ -11,8 +8,127 @@
 - The simulation world is set in `cobot-setting.yaml` → `digital_twin.webots.world`.
 - Start it with `cobot run --simulate`.
 
+## TCP gizmo
+
+This simulation-only mode lets you specify the robot's target pose by dragging a marker at its tool center point (TCP) in the Webots window.
+
+1. Enable `digital_twin.tcp_gizmo.enabled: true` in `cobot-setting.yaml` and launch with `cobot run --simulate`.
+2. Wait until other robot motions finish. For an initial test, use the regular named-pose command to move to `work`.
+3. Select the TCP marker in Webots. Drag a translation arrow or rotation handle with the left mouse button. The robot remains stationary while you edit the target.
+4. Release the button to submit the target. One complete trajectory is sent to the controller, rather than a stream of actions during dragging.
+5. Wait for completion before dragging again. If the target cannot be reached or the checked path collides, the marker returns to the actual TCP.
+
+Regular coordinate, joint, and control-file commands remain available. They have priority over new gizmo gestures. An already started gizmo trajectory finishes before a waiting regular command proceeds, even when that wait exceeds two seconds.
+
+### Tool attachment and speed
+
+The marker follows the active tool's `tcp` frame. With `patron`, it is at the chuck's working end; with no tool, it is at the flange. Apply tool changes through `cobot robot-setup`, rebuild, and restart the simulation. New tool models must define `tcp` at their working point.
+
+`max_linear_speed` (m/s) and `max_angular_speed` (rad/s) are upper bounds. Short moves include acceleration and braking, and joint limits may make motion slower. Joint interpolation reaches the requested TCP pose but does not guarantee a straight Cartesian path.
+
+Set `digital_twin.tcp_gizmo.enabled: false` and restart to disable this mode. The [configuration reference](../configuration.md) describes all parameters. `command_timeout` checks the age of the submitted target; mouse release is detected directly and requires no waiting-time setting.
+
+### When the marker returns or does not respond
+
+- Wait for regular motions or a committed gizmo trajectory to finish before starting another drag.
+- For a rejected target, read `TCP gizmo rejected target: ...` in the launch output. Try a smaller displacement from a reachable posture such as `work`.
+- With missing or stale robot feedback, the gizmo cannot submit movement. Check that simulation is running and `/joint_states` updates.
+
 ## Differences from the real robot
 
-- There are no real safety constraints, so motion can be faster.
+- Motion still obeys configured joint and TCP limits; simulation does not reproduce all physical robot protections.
 - Physics is approximate, including inertia, friction, and elasticity.
 - FRI is not used; communication goes through the Webots ROS 2 driver.
+
+## Spawning textured objects
+
+Set `digital_twin.webots.objects` in `cobot-setting.yaml`:
+
+```yaml
+objects:
+  enabled: true
+  count: 5
+  models:
+    - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+      rotation_deg: [0, 0, 0]
+    - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+      rotation_deg: [0, 0, 0]
+  area:
+    x: [0.3, 0.7]
+    y: [-0.3, 0.3]
+    z: [0.1, 0.1]
+```
+
+Webots spawns exactly `count` instances once at startup, independently choosing
+from `models` with replacement. This list specifies allowed models, not individual
+quantities. An absent section, `enabled: false`, or `count: 0` disables spawning.
+Paths support `pkg://`, absolute paths and paths relative to the settings file.
+Currently only Wavefront OBJ is supported.
+
+Bounds are in metres in the robot base frame. The spawner uses the same
+`webots.transform` and `webots.rotation` (axis and angle in radians) as the robot:
+`p_world = t_robot + R_robot * p_local`. They constrain initial spawn positions;
+physics may move the objects afterwards. Moving the robot with the mouse after
+startup does not relocate existing objects.
+
+The spawn anchor is the bounding-box XY centre and bottom Z. Exported vertex
+offsets are compensated without modifying the original files, UVs, scale
+(1 unit = 1 metre); orientation is configured per model. Bounds constrain the anchor, not the entire
+object. Random placements may overlap; choose a sufficiently large area.
+
+`CadShape` loads OBJ materials through `mtllib` and textures through MTL. Preserve
+relative asset paths: for example, `hammer1.mtl` references
+`hammer1_basecolor.png`. The complete `objects` directory is installed with
+`iiwa_description`. Missing models, materials or textures fail before the first
+spawn request. Physics uses an approximate bounding Box and a mass of 0.5 kg,
+not calibrated hammer inertia or grasp geometry. Objects are not automatically
+added to the MoveIt Planning Scene.
+
+The `object_spawner` node calls `/Ros2Supervisor/spawn_node_from_string`
+(`webots_ros2_msgs/srv/SpawnNodeFromString`) once per object. A rejected request
+aborts remaining work. Timeouts are not retried to avoid duplicates. The node
+exits after success and only runs in simulation.
+
+### Simulation smoke test
+
+1. Build with `colcon build --base-paths src --packages-select iiwa_description iiwa_utils iiwa_bringup iiwa_config --symlink-install`.
+2. Run `source install/setup.bash`, then `cobot run local`, selecting Webots.
+3. Inspect the five nodes `spawned_object_0000` through `spawned_object_0004`, their
+   textures and the `All 5 objects spawned` log.
+4. Change `transform` and `rotation`, restart and confirm the spawn area translates
+   and rotates with the robot base. Pause before objects fall to inspect initial positions.
+5. Check `count: 0`, operation without cameras and rejection of missing models.
+
+Reference: [Webots CadShape](https://cyberbotics.com/doc/reference/cadshape).
+
+### Per-model rotation
+
+Each `models` entry accepts `path` and `rotation_deg: [X, Y, Z]`.
+Angles are fixed degrees: apply X, then Y, then Z about the robot base axes,
+`R_model = Rz * Ry * Rx` and `R_world = R_robot * R_model`. For example:
+
+```yaml
+models:
+  - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+    rotation_deg: [0, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+    rotation_deg: [90, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+    rotation_deg: [0, 0, 45]
+```
+
+The supplied OBJ hammers have their long axis along Z. `[0, 0, 0]` gives an
+initial vertical orientation, `[90, 0, 0]` lays the hammer sideways and
+`[0, 0, 45]` turns it around the vertical axis. Both configuration files now
+explicitly set `[0, 0, 0]` for each model; adjust each independently.
+Legacy path-only entries remain supported and imply zero rotation.
+
+Both the visible model and its collision Box rotate. Height is recalculated:
+`area.z` locates the bottom of the rotated collision bounds in the robot frame;
+X/Y locate their centre. Model rotation does not rotate the spawn area.
+Changes take effect on the next launch.
+
+This sets the initial orientation, not a constraint. Gravity and contact can
+still topple the hammer. Pause simulation at spawn to inspect it. Smoke-test
+two models with different angles, check the Box stays above `area.z`, and check
+that changing `webots.rotation` preserves each model's relative orientation.

doc/lwc-doc/docs/getting-started/concepts/simulation.md

@@ -1,8 +1,5 @@
 # Симуляция (Webots)
 
-!!! info "Раздел в разработке"
-    Подробное описание работы в симуляторе готовится.
-
 **Webots** — симулятор роботов с открытым исходным кодом. В LWC используется как цифровой двойник робота KUKA LBR IIWA 7: позволяет разрабатывать и отлаживать алгоритмы управления без доступа к физическому роботу.
 
 ## Ключевые особенности
@@ -11,8 +8,132 @@
 - Мир симуляции задаётся в `cobot-setting.yaml` → `digital_twin.webots.world`
 - Для запуска: `cobot run --simulate`
 
+## Перемещение за TCP
+
+В этом режиме цифрового двойника целевая поза робота задаётся перетаскиванием маркера рабочей точки инструмента (TCP) прямо в окне Webots.
+
+1. Включите `digital_twin.tcp_gizmo.enabled: true` в `cobot-setting.yaml` и запустите `cobot run --simulate`.
+2. Дождитесь завершения других движений. Для первой проверки удобно перейти в именованную позицию `work` обычной командой.
+3. Выберите маркер TCP в Webots. Удерживая левую кнопку мыши, тяните за стрелку перемещения или ручку вращения. Во время редактирования цели робот стоит.
+4. Отпустите кнопку: контроллеру отправится одна полная траектория, а не поток action-целей при перетаскивании.
+5. Дождитесь завершения движения перед следующим переносом. При недостижимой цели или столкновении на проверяемом пути маркер возвращается к фактическому TCP.
+
+Обычные команды координат, joints и файлы управления остаются доступны и имеют приоритет перед новым перетаскиванием. Уже начатое движение гизмы завершается до выполнения ожидающей обычной команды, в том числе если ожидание длится больше двух секунд.
+
+### Инструмент и скорость
+
+Маркер следует за фреймом `tcp` активного инструмента: у `patron` это рабочий торец патрона, без инструмента — фланец. При смене инструмента примените выбор через `cobot robot-setup`, пересоберите проект и перезапустите симуляцию. В моделях новых инструментов задавайте `tcp` в рабочей точке.
+
+`max_linear_speed` (м/с) и `max_angular_speed` (рад/с) задают верхние пределы. На коротких перемещениях есть разгон и торможение; ограничения суставов также могут снизить скорость. Интерполяция в суставах достигает заданной позы TCP, но не гарантирует прямолинейный декартов путь.
+
+Для отключения задайте `digital_twin.tcp_gizmo.enabled: false` и перезапустите систему. Все параметры описаны в [справочнике конфигурации](../configuration.md). `command_timeout` проверяет возраст поступившей цели: отпускание мыши определяется напрямую, настройка времени ожидания не требуется.
+
+### Если маркер возвращается или не реагирует
+
+- Дождитесь завершения обычной команды или уже начатой траектории гизмы перед новым перетаскиванием.
+- При отказе смотрите строку `TCP gizmo rejected target: ...` в выводе запуска. Попробуйте небольшое смещение из достижимой позы, например `work`.
+- При отсутствии или устаревании обратной связи перемещение недоступно. Проверьте, что симуляция работает и `/joint_states` обновляется.
+
 ## Отличия от реального робота
 
-- Нет ограничений по безопасности (можно двигаться быстрее)
+- Действуют настроенные ограничения суставов и TCP; симуляция не воспроизводит все защиты физического робота
 - Физика не идеальна — инерция, трение и упругость приближённые
 - FRI-протокол не задействован — связь идёт через Webots-драйвер ROS 2
+
+## Спавн объектов с текстурами
+
+В `cobot-setting.yaml` (секция `digital_twin.webots`) задайте:
+
+```yaml
+objects:
+  enabled: true
+  count: 5
+  models:
+    - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+      rotation_deg: [0, 0, 0]
+    - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+      rotation_deg: [0, 0, 0]
+  area:
+    x: [0.3, 0.7]
+    y: [-0.3, 0.3]
+    z: [0.1, 0.1]
+```
+
+При запуске Webots создаётся ровно `count` объектов: каждая модель независимо
+выбирается из `models`, повторения разрешены. `enabled: false`, `count: 0` или
+отсутствие секции отключает спавн. Список — разрешённые модели, а не количество
+экземпляров каждой модели. Пути могут быть `pkg://`, абсолютными или относительно
+файла настроек. Пока поддерживается Wavefront OBJ.
+
+Область задаётся в метрах в системе основания робота: применяется та же пара
+`webots.transform` / `webots.rotation` (ось и угол в радианах), что и при спавне
+робота. Формула: `p_world = t_robot + R_robot * p_local`. Это начальная область,
+а не ограничитель движения: физика затем может перемещать объекты. Перемещение
+робота мышью после запуска не переносит уже созданные предметы.
+
+Точка спавна — центр габаритов по X/Y и нижняя точка по Z. Смещение вершин
+в экспортированном OBJ компенсируется автоматически; исходные файлы, UV,
+масштаб (1 единица = 1 метр) сохраняется; ориентация задаётся отдельно для каждой модели. Диапазон ограничивает
+эту точку, а не весь габарит. Пересечения объектов при случайном размещении
+не исключаются: выбирайте достаточно просторную область.
+
+`CadShape` загружает материалы из `mtllib` и текстуры из MTL. Сохраняйте OBJ,
+MTL и PNG в исходной структуре каталогов: например, `hammer1.mtl` ссылается на
+`hammer1_basecolor.png`. Весь каталог `objects` устанавливается вместе с
+`iiwa_description`. Отсутствующие модели, MTL и текстуры вызывают ошибку до
+первого запроса спавна. Для физики используется упрощённый габаритный Box и
+масса 0.5 кг; это не точная модель инерции или захвата молотка. В MoveIt Planning
+Scene эти объекты автоматически не добавляются.
+
+Нода `object_spawner` вызывает
+`/Ros2Supervisor/spawn_node_from_string` (`webots_ros2_msgs/srv/SpawnNodeFromString`)
+однократно для каждого предмета. Отказ сервиса прерывает оставшийся спавн;
+тайм-аут не повторяется, чтобы не создавать дубликаты. Нода завершается после
+успеха. На физическом роботе она не запускается.
+
+### Проверка в симуляции
+
+1. Пересоберите пакеты:
+   `colcon build --base-paths src --packages-select iiwa_description iiwa_utils iiwa_bringup iiwa_config --symlink-install`.
+2. Выполните `source install/setup.bash`, затем `cobot run local` и выберите Webots.
+3. Проверьте пять узлов `spawned_object_0000` … `spawned_object_0004` в дереве сцены,
+   текстуры молотков и сообщение `All 5 objects spawned`.
+4. Измените `transform` и `rotation`, перезапустите: область должна переместиться
+   и повернуться вместе с основанием робота. Для проверки начальных координат
+   используйте паузу симулятора до падения объектов.
+5. Проверьте `count: 0`, запуск без камер и ошибку при несуществующей модели.
+
+Справочник: [Webots CadShape](https://cyberbotics.com/doc/reference/cadshape).
+
+### Индивидуальный поворот модели
+
+Каждая запись `models` может содержать `path` и `rotation_deg: [X, Y, Z]`.
+Углы фиксированные, в градусах; сначала применяется поворот вокруг X, затем Y,
+затем Z в системе основания робота: `R_model = Rz * Ry * Rx`,
+`R_world = R_robot * R_model`. Например:
+
+```yaml
+models:
+  - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+    rotation_deg: [0, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+    rotation_deg: [90, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+    rotation_deg: [0, 0, 45]
+```
+
+В предоставленных OBJ длинная ось молотка направлена по Z: `[0, 0, 0]`
+задаёт вертикальную начальную ориентацию, `[90, 0, 0]` кладёт молоток набок,
+а `[0, 0, 45]` поворачивает его вокруг вертикальной оси. В обоих конфигурационных
+файлах для всех моделей явно указано `[0, 0, 0]`; изменяйте каждую независимо.
+Записи только с путём остаются совместимыми и означают нулевой поворот.
+
+Поворачиваются и видимая модель, и её коллизионный Box. После поворота
+пересчитывается высота: `area.z` задаёт нижнюю точку повёрнутого коллизионного
+габарита в системе робота, а X/Y — его центр. Поворот модели не поворачивает
+саму область спавна. Изменения применяются при следующем запуске.
+
+Это начальная ориентация, а не фиксация: после появления гравитация и контакты
+могут опрокинуть молоток. Для проверки поставьте симуляцию на паузу при спавне.
+Проверьте разные углы у двух моделей, отсутствие погружения Box ниже `area.z`
+и сохранение локальных углов при изменении `webots.rotation`.

doc/lwc-doc/docs/getting-started/configuration.en.md

@@ -26,9 +26,6 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   fri_cycle_ms: 10
-  joint_position_tau: 0.04
-  joint_velocity_tau: 0.01
-  active_controller: "jtc"
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 ```
 
@@ -38,11 +35,11 @@ robot:
 | `ip` | KUKA controller IP address | **Change** to the actual controller address |
 | `port` | FRI UDP port | Default: `30200`; change only if the port conflicts |
 | `fri_cycle_ms` | FRI cycle: `5` ms = 200 Hz, `10` ms = 100 Hz | Use `10` for stable operation or `5` for high-precision tasks |
-| `joint_position_tau` | Position EMA filter [s], smoothing commands before transmission | Decrease for a faster response; increase if vibration occurs |
-| `joint_velocity_tau` | Velocity EMA filter [s], removing finite-difference spikes | Tune in the same way as `joint_position_tau` |
-| `active_controller` | Control mode: `jtc` (MoveIt / JointTrajectory) or `forward` (direct control) | Use `jtc` for most tasks |
+| `joint_position_tau` | FRI command EMA time constant, s | `0.04`; `0` disables smoothing |
 | `description` | Path to the robot URDF | Do not change |
 
+Motion always uses JointTrajectoryController. The position command is smoothed before FRI transmission with `robot.joint_position_tau`; no velocity filter is applied.
+
 ---
 
 ## `digital_twin` section — simulator
@@ -51,6 +48,13 @@ Configures the Webots environment and RViz visualization.
 
 ```yaml
 digital_twin:
+  tcp_gizmo:
+    enabled: true
+    publish_period: 0.032
+    command_timeout: 0.3
+    state_timeout: 0.5
+    max_linear_speed: 0.50
+    max_angular_speed: 0.4
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     transform: "-0.25 0 0.79"
@@ -70,6 +74,73 @@ digital_twin:
 | `webots.cameras` | List of YAML configurations for connected cameras |
 | `rviz.config` | Path to the RViz configuration |
 
+### D455 cameras in Webots
+
+The overhead camera (`config/cameras/d455_top.yaml`) and the `patron` tool camera
+use **640×480 at 30 FPS** for RGB and depth. Nominal VGA depth horizontal FOV
+is 75°; 87° applies to HD. RGB approximates a central 4:3 crop of a 1280×800
+sensor with a 90° horizontal FOV, resulting in about 79.61°. Use the individual
+camera's 640×480 calibration for an exact RGB FOV.
+
+The selected simulation depth range is 0.4–6 m with 1 mm quantization.
+These are not accuracy guarantees or the exact VGA minimum distance of a D455.
+This ideal model omits random noise, stereo matching, the IR projector,
+distortion and the physical RGB/depth offset. Webots derives vertical FOV from
+horizontal FOV and aspect ratio; it cannot be set independently.
+Matching an individual device requires its intrinsics and extrinsics.
+
+The `iiwa_utils.webots_camera.WebotsCamera` plugin maintains an average 30 Hz
+cadence in simulation time without accumulating timestep rounding errors.
+With `basicTimeStep: 32`, frame intervals are 32 or 64 ms; uniform 33.33 ms
+intervals are impossible on this grid. Wall-clock FPS depends on simulation
+speed. The world's physics timestep does not need changing.
+
+Existing image topic names are preserved:
+
+| Camera | RGB (`bgra8`) | Depth (`32FC1`, metres) |
+|---|---|---|
+| Overhead | `/d455_top/image_raw/image_color` | `/d455_top/depth/image_raw/image` |
+| Tool | `/camera/color/image_color` | `/camera/depth/image` |
+
+Each stream also publishes `camera_info` with the virtual camera matrix and
+matching timestamp. Depth offers `point_cloud` when subscribed: XYZ in metres
+using optical axes (X right, Y down, Z forward), with NaN for missing points.
+Frame IDs remain `d455_top_link` and `camera_optical_frame`;
+the plugin neither broadcasts TF nor aligns depth to RGB.
+
+Smoke test: start `cobot run local` → **Webots simulator**, subscribe to the four
+image topics, check image dimensions, matching `CameraInfo` timestamps, and the
+average rate from frame timestamps over 10 seconds of simulation time.
+`ros2 topic hz /camera/depth/image` measures wall-clock delivery, so it can report
+less than 30 when the simulation runs slowly.
+
+Sources: [D400/D455 datasheet](https://dev.realsenseai.com/download/42003/),
+[D455 RGB modes](https://support.realsenseai.com/hc/en-us/community/posts/13718098480019-How-to-resample-D455-RGB-images-from-1280-720-to-640-480),
+[Webots RangeFinder](https://cyberbotics.com/doc/reference/rangefinder).
+
+### TCP gizmo in Webots
+
+Select the TCP marker, translate or rotate it with the native Webots gizmo, and release the left mouse button. Holding the button only changes the preview. Releasing it submits one complete trajectory. Another drag is accepted after that trajectory finishes. An unreachable target or a colliding trajectory resets the marker to the current TCP.
+
+The gizmo controls the active tool's `tcp` link independently of `planning.pose_link`. For `patron`, TCP is at the chuck's working end, 55.2 mm along local Z from the `patron` link origin. Without a tool, it coincides with the `link_ee` flange. Regular Cartesian commands also use this point when `planning.pose_link: tcp`.
+
+The gizmo has the lowest motion priority. It is unavailable during regular motion. If a coordinate, joint, or file command arrives while a committed gizmo trajectory is running, that command waits for the trajectory to finish before proceeding.
+
+| `tcp_gizmo` parameter | Description |
+|---|---|
+| `enabled` | Set to `false` to disable the gizmo at the next launch; simulation only |
+| `publish_period` | State update period, seconds |
+| `command_timeout` | Maximum age of an incoming target, seconds; not a delay after dragging |
+| `state_timeout` | Maximum age of robot feedback, seconds |
+| `max_linear_speed` | TCP linear speed cap, m/s |
+| `max_angular_speed` | TCP angular speed cap, rad/s |
+
+Mouse release is detected directly, so no idle timer is needed. Joint limits, acceleration, and deceleration can reduce speed below the configured cap. The trajectory interpolates joint positions to reach the requested TCP pose; a straight Cartesian path is not guaranteed.
+
+See the [simulation guide](concepts/simulation.md) for the operating sequence.
+
+
+
 ---
 
 ## `tool` section — active tool
@@ -88,6 +159,8 @@ tool:
 
 Available tools are defined in `src/iiwa_config/config/tools.yaml`. To add a tool, describe it there and then set its name in `tool.active`.
 
+Apply tool selection through `cobot robot-setup`, which updates the tool description and SRDF. Rebuild the project and restart the simulation after changing tools.
+
 ---
 
 ## `planning` section — motion planning
@@ -169,7 +242,6 @@ The remaining parameters (`tls`, `topic_whitelist`, `min_qos_depth`, and others)
 | ✅ | `robot.fri_cycle_ms` | Select `10` (standard) or `5` (high frequency) |
 | ✅ | `tool.active` | Set the active tool |
 | ✅ | `web.enabled` | Set to `false` if the web interface is not needed |
-| ⚠️ | `robot.active_controller` | Change only when intentionally switching the control mode |
 | ⚠️ | `planning.*` | Change only when another planner or other parameters are required |
 | ❌ | `robot.description` | Do not change; this is the URDF path |
 | ❌ | `controller.moveit.*` | Do not change; these are package-internal MoveIt configuration paths |

doc/lwc-doc/docs/getting-started/configuration.md

@@ -28,9 +28,6 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   fri_cycle_ms: 10
-  joint_position_tau: 0.04
-  joint_velocity_tau: 0.01
-  active_controller: "jtc"
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 ```
 
@@ -40,11 +37,11 @@ robot:
 | `ip` | IP-адрес контроллера KUKA | **Изменить** на реальный IP вашего контроллера |
 | `port` | UDP-порт FRI | По умолчанию `30200` — менять только при конфликте портов |
 | `fri_cycle_ms` | Период цикла FRI: `5` мс = 200 Гц, `10` мс = 100 Гц | `10` для стабильной работы, `5` для высокоточных задач |
-| `joint_position_tau` | EMA-фильтр позиций [с] — сглаживает команды перед отправкой | Уменьшать для более быстрой реакции, увеличивать при вибрациях |
-| `joint_velocity_tau` | EMA-фильтр скорости [с] — убирает выбросы конечных разностей | Аналогично `joint_position_tau` |
-| `active_controller` | Режим управления: `jtc` (MoveIt / JointTrajectory) или `forward` (прямое управление) | `jtc` для большинства задач |
+| `joint_position_tau` | Постоянная времени EMA команд FRI, с | `0.04`; `0` отключает сглаживание |
 | `description` | Путь к URDF-описанию робота | Не менять |
 
+Управление движением всегда выполняет JointTrajectoryController. Перед отправкой в FRI позиционная команда сглаживается EMA с постоянной времени `robot.joint_position_tau`; фильтр скорости не применяется.
+
 ---
 
 ## Блок `digital_twin` — симулятор
@@ -53,6 +50,13 @@ robot:
 
 ```yaml
 digital_twin:
+  tcp_gizmo:
+    enabled: true
+    publish_period: 0.032
+    command_timeout: 0.3
+    state_timeout: 0.5
+    max_linear_speed: 0.50
+    max_angular_speed: 0.4
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     transform: "-0.25 0 0.79"
@@ -72,6 +76,72 @@ digital_twin:
 | `webots.cameras` | Список YAML-конфигов подключённых камер |
 | `rviz.config` | Путь к конфигурации RViz |
 
+### Камеры D455 в Webots
+
+Верхняя камера (`config/cameras/d455_top.yaml`) и камера инструмента `patron`
+используют **640×480, 30 кадров/с** для RGB и глубины. Горизонтальный угол глубины
+для VGA — 75°; 87° относится к HD. RGB использует приближение центральной обрезки
+сенсора 1280×800 с горизонтальным углом 90° до формата 4:3: около 79,61°.
+Точный угол RGB следует получить из калибровки конкретной камеры для 640×480.
+
+Глубина ограничена выбранным диапазоном симуляции 0,4–6 м, с шагом 1 мм.
+Это не гарантия точности и не точное значение минимальной дистанции D455 в VGA.
+Модель идеальная: без случайного шума, стереосопоставления, ИК-проектора,
+дисторсии и реального смещения между RGB и depth. Вертикальный угол Webots
+вычисляет из горизонтального угла и соотношения сторон; он не задаётся независимо.
+Для воспроизведения конкретного экземпляра нужны его intrinsics и extrinsics.
+
+Плагин `iiwa_utils.webots_camera.WebotsCamera` поддерживает среднюю частоту
+30 Гц по времени симуляции, не накапливая округление до шага Webots.
+При `basicTimeStep: 32` интервалы составляют 32 или 64 мс; равномерные интервалы
+33,33 мс на такой сетке невозможны. Частота по настенным часам зависит от скорости
+симуляции. Физический шаг мира менять не требуется.
+
+Имена топиков изображений сохранены:
+
+| Камера | RGB (`bgra8`) | Глубина (`32FC1`, метры) |
+|---|---|---|
+| Верхняя | `/d455_top/image_raw/image_color` | `/d455_top/depth/image_raw/image` |
+| Инструмент | `/camera/color/image_color` | `/camera/depth/image` |
+
+Рядом публикуется `camera_info` с матрицей виртуальной камеры и той же временной
+меткой. Для глубины доступен `point_cloud` при наличии подписчиков: XYZ в метрах
+в оптических осях (X вправо, Y вниз, Z вперёд), отсутствующие точки — NaN.
+Сохранены идентификаторы кадров `d455_top_link` и `camera_optical_frame`;
+плагин не публикует TF и не выполняет совмещение depth с RGB.
+
+Проверка после `cobot run local` → **Webots simulator**: подписаться на четыре
+топика, проверить размеры `Image`, совпадение временных меток с `CameraInfo`
+и среднюю частоту по меткам кадров за 10 секунд модельного времени.
+`ros2 topic hz /camera/depth/image` измеряет доставку по настенным часам,
+поэтому при медленной симуляции значение может быть ниже 30.
+
+Источники: [спецификация D400/D455](https://dev.realsenseai.com/download/42003/),
+[режимы RGB D455](https://support.realsenseai.com/hc/en-us/community/posts/13718098480019-How-to-resample-D455-RGB-images-from-1280-720-to-640-480),
+[Webots RangeFinder](https://cyberbotics.com/doc/reference/rangefinder).
+
+### Перемещение за TCP в Webots
+
+Порядок действий приведён в [руководстве по симуляции](concepts/simulation.md).
+
+Выберите маркер TCP, переместите или поверните его штатной гизмой Webots и отпустите левую кнопку мыши. Пока кнопка удерживается, меняется только маркер. После отпускания отправляется одна полная траектория. Следующее перетаскивание доступно после её завершения. Если цель недостижима или траектория проходит через столкновение, маркер возвращается к текущему TCP.
+
+Гизма использует звено `tcp` активного инструмента независимо от `planning.pose_link`. Для `patron` это рабочий торец патрона (55,2 мм от начала звена `patron` вдоль его локальной оси Z); без инструмента — фланец `link_ee`. Этот TCP также используется обычными декартовыми командами, когда `planning.pose_link: tcp`.
+
+У гизмы самый низкий приоритет: во время обычного движения она недоступна. Если команда координат, joints или файла приходит во время уже начатого движения гизмы, она ждёт завершения этой траектории.
+
+| Параметр `tcp_gizmo` | Описание |
+|---|---|
+| `enabled` | `false` полностью отключает гизму при следующем запуске; доступна только в симуляции |
+| `publish_period` | Период обновления состояния, с |
+| `command_timeout` | Максимальный возраст входящей цели, с; не задержка после перетаскивания |
+| `state_timeout` | Допустимый возраст обратной связи робота, с |
+| `max_linear_speed` | Верхний предел скорости TCP, м/с |
+| `max_angular_speed` | Верхний предел угловой скорости TCP, рад/с |
+
+Отпускание определяется по состоянию кнопки мыши, поэтому таймер ожидания не нужен. Скорость может быть ниже заданного предела из-за ограничений суставов, ускорения и торможения. Траектория интерполируется в суставах и достигает заданной позы TCP; прямолинейность пути TCP не гарантируется.
+
+
 ---
 
 ## Блок `tool` — активный инструмент
@@ -90,6 +160,8 @@ tool:
 
 Список доступных инструментов находится в файле `src/iiwa_config/config/tools.yaml`. Для добавления нового инструмента необходимо описать его там, после чего указать имя в `tool.active`.
 
+Применяйте выбор через `cobot robot-setup`: мастер обновляет описание инструмента и SRDF. После смены инструмента пересоберите проект и перезапустите симуляцию.
+
 ---
 
 ## Блок `planning` — планирование движений
@@ -171,7 +243,6 @@ foxglove:
 | ✅ | `robot.fri_cycle_ms` | Выбрать `10` (стандарт) или `5` (высокая частота) |
 | ✅ | `tool.active` | Указать активный инструмент |
 | ✅ | `web.enabled` | Выключить (`false`), если веб-интерфейс не нужен |
-| ⚠️ | `robot.active_controller` | Менять только при намеренном переключении режима управления |
 | ⚠️ | `planning.*` | Менять при необходимости другого планировщика или параметров |
 | ❌ | `robot.description` | Не трогать — путь к URDF |
 | ❌ | `controller.moveit.*` | Не трогать — пути к конфигам MoveIt внутри пакетов |

doc/lwc-doc/docs/realtime-fri.en.md

@@ -0,0 +1,84 @@
+# FIFO priority for FRI
+
+## Why it is needed
+
+FRI exchanges a position command on every cycle. With a 10 ms period, a new packet should be handled about every 10 ms. The regular Linux scheduler can delay the control thread behind other processes. The log then contains messages such as:
+
+```text
+Could not enable FIFO RT scheduling: Operation not permitted
+Overrun might occur ... Read time: 11500 us
+```
+
+`SCHED_FIFO` is Linux scheduling for threads with fixed priority. This project requests priority 50 for `controller_manager` and priority 80 for the FRI worker. The FRI worker receives the higher priority so it can receive a KUKA packet and send the reply on time.
+
+Configure this on the **ROS 2 computer** that runs `cobot run`. It does not require changes to the Sunrise Cabinet or `cobot-setting.yaml`.
+
+## Ubuntu 24.04 setup
+
+On the ROS 2 computer, run these commands as the user who starts `cobot`:
+
+```bash
+sudo groupadd -f realtime
+sudo usermod -aG realtime "$USER"
+sudo tee /etc/security/limits.d/99-ros2-realtime.conf >/dev/null <<'EOF'
+@realtime soft rtprio 99
+@realtime soft priority 99
+@realtime soft memlock unlimited
+@realtime hard rtprio 99
+@realtime hard priority 99
+@realtime hard memlock unlimited
+EOF
+```
+
+Log out of the desktop session completely and log in again, or reboot. An already open terminal does not receive the new group and limits.
+
+## Verification
+
+In a new terminal, verify the group and limits:
+
+```bash
+id -nG | tr ' ' '\n' | grep -x realtime
+ulimit -r
+ulimit -l
+```
+
+The expected output is `realtime`, then `99`, then `unlimited`.
+
+Start FRI without commanding robot motion and check the log for:
+
+```text
+FRI worker uses FIFO priority 80
+```
+
+You can also inspect the policies of all threads in the process:
+
+```bash
+PID=$(pgrep -n ros2_control_node)
+ps -L -p "$PID" -o pid,tid,cls,rtprio,pri,comm
+```
+
+Realtime threads have `FF` in the `CLS` column. The FRI worker has `RTPRIO` 80 and the `controller_manager` main loop has 50.
+
+!!! warning "Before the first motion"
+    First confirm that FRI connects and that the log contains `FRI worker uses FIFO priority 80`. Then make a short move at speed 0.1 with an operator supervising. FIFO reduces jitter on the computer; it does not replace network checks, robot limits, or a clear work area.
+
+## If setup does not work
+
+- If `realtime` is missing from `id`, log out and log in again.
+- If `ulimit -r` is less than 80, check `/etc/security/limits.d/99-ros2-realtime.conf` and that PAM loads `pam_limits.so`.
+- If the log still says `Operation not permitted`, make sure `cobot run` is started by the same user passed to `usermod`.
+- A Python virtualenv needs no extra setup: permissions are assigned to the user and inherited by Python.
+- Docker requires `--cap-add=sys_nice --ulimit rtprio=99 --ulimit memlock=-1`.
+
+## Rollback
+
+To remove the setup, run:
+
+```bash
+sudo rm /etc/security/limits.d/99-ros2-realtime.conf
+sudo gpasswd -d "$USER" realtime
+```
+
+After logging in again, processes use the regular Linux scheduler.
+
+See the official [ros2_control documentation](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism) for controller-manager priorities and limits.

doc/lwc-doc/docs/realtime-fri.md

@@ -0,0 +1,84 @@
+# FIFO-приоритет для FRI
+
+## Зачем это нужно
+
+FRI передаёт позиционную команду каждый цикл. При выбранном периоде 10 мс новый пакет должен обрабатываться примерно каждые 10 мс. Обычный планировщик Linux может задержать поток другим процессом. В журнале это видно по сообщениям:
+
+```text
+Could not enable FIFO RT scheduling: Operation not permitted
+Overrun might occur ... Read time: 11500 us
+```
+
+`SCHED_FIFO` — политика планировщика Linux для потоков с фиксированным приоритетом. В проекте `controller_manager` запрашивает приоритет 50, а FRI-поток — 80. Поток FRI получает более высокий приоритет, чтобы вовремя принять пакет KUKA и отправить ответ.
+
+Настройку выполняют **на компьютере с ROS 2**, с которого запускается `cobot run`. Sunrise Cabinet и `cobot-setting.yaml` менять для этого не нужно.
+
+## Настройка Ubuntu 24.04
+
+Выполните на компьютере с ROS 2 под пользователем, который запускает `cobot`:
+
+```bash
+sudo groupadd -f realtime
+sudo usermod -aG realtime "$USER"
+sudo tee /etc/security/limits.d/99-ros2-realtime.conf >/dev/null <<'EOF'
+@realtime soft rtprio 99
+@realtime soft priority 99
+@realtime soft memlock unlimited
+@realtime hard rtprio 99
+@realtime hard priority 99
+@realtime hard memlock unlimited
+EOF
+```
+
+Затем полностью выйдите из графического сеанса и войдите снова либо перезагрузите компьютер. Открытый терминал не получает новую группу и лимиты.
+
+## Проверка
+
+В новом терминале проверьте группу и лимиты:
+
+```bash
+id -nG | tr ' ' '\n' | grep -x realtime
+ulimit -r
+ulimit -l
+```
+
+Ожидаемый результат: первая команда выводит `realtime`, вторая — `99`, третья — `unlimited`.
+
+Запустите FRI без движения робота и убедитесь, что в журнале есть:
+
+```text
+FRI worker uses FIFO priority 80
+```
+
+После запуска можно дополнительно посмотреть политики всех потоков процесса:
+
+```bash
+PID=$(pgrep -n ros2_control_node)
+ps -L -p "$PID" -o pid,tid,cls,rtprio,pri,comm
+```
+
+Для потоков реального времени в столбце `CLS` будет `FF`; у FRI-потока `RTPRIO` равен 80, у основного цикла `controller_manager` — 50.
+
+!!! warning "Перед первым движением"
+    Сначала проверьте успешное подключение FRI и строку `FRI worker uses FIFO priority 80`. Затем выполните короткое перемещение на скорости 0.1 при наблюдении оператора. FIFO уменьшает jitter компьютера, но не отменяет контроль сети, ограничений робота и рабочей зоны.
+
+## Если настройка не сработала
+
+- Если `realtime` отсутствует в выводе `id`, выйдите из системы и войдите снова.
+- Если `ulimit -r` меньше 80, проверьте файл `/etc/security/limits.d/99-ros2-realtime.conf` и наличие `pam_limits.so` в конфигурации PAM.
+- Если лог всё ещё сообщает `Operation not permitted`, убедитесь, что `cobot run` запущен тем же пользователем, для которого выполнен `usermod`.
+- В Python virtualenv дополнительных действий не требуется: права задаются пользователю и наследуются процессом Python.
+- Для Docker нужны права контейнера: `--cap-add=sys_nice --ulimit rtprio=99 --ulimit memlock=-1`.
+
+## Откат
+
+Чтобы удалить настройку, выполните:
+
+```bash
+sudo rm /etc/security/limits.d/99-ros2-realtime.conf
+sudo gpasswd -d "$USER" realtime
+```
+
+После повторного входа процессы снова будут работать с обычным планировщиком Linux.
+
+Подробнее о лимитах и приоритете `controller_manager` — в [документации ros2_control](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism).

doc/lwc-doc/mkdocs.yml

@@ -141,5 +141,6 @@ nav:
         - Меню IO Group:       sunrise/kuka/features/io-group.md
 
   - Решение проблем:
+    - Реальное время FRI: realtime-fri.md
     - Ошибка конфигурации: troubleshooting/config-error.md
     - Ошибка SSL (rosdep/setup): troubleshooting/ssl-error.md

rosdep.yaml

@@ -18,3 +18,22 @@ python3-fastmcp:
     pip:
       packages: [fastmcp]
 
+# TCP gizmo / MoveIt Servo (ROS 2 Jazzy).
+moveit_servo:
+  ubuntu: [ros-jazzy-moveit-servo]
+rclcpp_action:
+  ubuntu: [ros-jazzy-rclcpp-action]
+control_msgs:
+  ubuntu: [ros-jazzy-control-msgs]
+trajectory_msgs:
+  ubuntu: [ros-jazzy-trajectory-msgs]
+action_msgs:
+  ubuntu: [ros-jazzy-action-msgs]
+rcl_interfaces:
+  ubuntu: [ros-jazzy-rcl-interfaces]
+tf2_eigen:
+  ubuntu: [ros-jazzy-tf2-eigen]
+python3-numpy:
+  ubuntu: [python3-numpy]
+python3-scipy:
+  ubuntu: [python3-scipy]

src/iiwa_bringup/launch/iiwa.launch.py

@@ -4,7 +4,7 @@ import sys
 sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
@@ -50,8 +50,9 @@ def _runtime_setup(context, *args, **kwargs):
             "initial_positions_file": settings.controller.moveit.initial_positions,
             "robot_ip": settings.robot.ip,
             "fri_port": str(settings.robot.port),
-            "simulate": "false",
+            "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
             "joint_position_tau": str(settings.robot.joint_position_tau),
+            "simulate": "false",
         }
 
     robot_description = converter.load_robot_description(
@@ -89,6 +90,7 @@ def _runtime_setup(context, *args, **kwargs):
                 "controller_timer": str(settings.digital_twin.webots.controller_timer),
                 "controller": str(settings.controller.controller_path),
                 "initial_positions_file": str(settings.controller.moveit.initial_positions),
+                "tcp_gizmo_enabled": str(settings.digital_twin.tcp_gizmo.enabled).lower(),
             }.items(),
         ))
 
@@ -118,9 +120,18 @@ def _runtime_setup(context, *args, **kwargs):
             "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,
+            "robot_ip": settings.robot.ip,
+            "fri_port": str(settings.robot.port),
         }
 
+    if not simulate:
+        setup.append(LogInfo(msg=(
+            "FRI configured from cobot-setting.yaml: "
+            f"period={settings.robot.fri_cycle_ms} ms, "
+            f"position_tau={settings.robot.joint_position_tau:.3f} s, "
+            f"endpoint={settings.robot.ip}:{settings.robot.port}"
+        )))
+
     setup.append(IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution([

src/iiwa_bringup/launch/supported/controllers.launch.py

@@ -1,10 +1,15 @@
+import math
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
 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 ament_index_python.packages import get_package_share_directory
+from pathlib import Path
+from xml.etree import ElementTree
 
 
 from iiwa_utils import converter
@@ -19,9 +24,12 @@ def _setup_controllers(context, *args, **kwargs):
     controller_timer = LaunchConfiguration("controller_timer").perform(context)
     controller_path = LaunchConfiguration("controller_path").perform(context)
     simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
-    controller = LaunchConfiguration("controller").perform(context)  # "jtc" | "forward"
     fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context))
-    joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context)
+    if not 1 <= fri_cycle_ms <= 100:
+        raise ValueError("fri_cycle_ms must be within the FRI 1.16 range 1..100 ms")
+    joint_position_tau = float(LaunchConfiguration("joint_position_tau").perform(context))
+    if not math.isfinite(joint_position_tau) or joint_position_tau < 0.0:
+        raise ValueError("joint_position_tau must be finite and non-negative")
     update_rate = 1000 // fri_cycle_ms
 
     xacro_args = {"initial_positions_file": initial_positions_file}
@@ -29,7 +37,10 @@ def _setup_controllers(context, *args, **kwargs):
     if simulate:
         xacro_args["simulate"] = "true"
     else:
-        xacro_args["joint_position_tau"] = joint_position_tau
+        xacro_args["robot_ip"] = LaunchConfiguration("robot_ip").perform(context)
+        xacro_args["fri_port"] = LaunchConfiguration("fri_port").perform(context)
+        xacro_args["fri_cycle_ms"] = str(fri_cycle_ms)
+        xacro_args["joint_position_tau"] = str(joint_position_tau)
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -75,6 +86,13 @@ def _setup_controllers(context, *args, **kwargs):
 
     # FRI
     else:
+        manifest = Path(get_package_share_directory("controller_manager")) / "package.xml"
+        version = ElementTree.parse(manifest).findtext("version", "0.0.0")
+        if tuple(int(part) for part in version.split(".")[:3]) < (4, 48, 0):
+            raise RuntimeError(
+                "FRI synchronization requires controller_manager >= 4.48.0 (ROS 2 Jazzy); "
+                f"installed: {version}"
+            )
         ros2_control_node = Node(
             package="controller_manager",
             executable="ros2_control_node",
@@ -83,6 +101,7 @@ def _setup_controllers(context, *args, **kwargs):
                 {"robot_description": robot_description},
                 controller_path,
                 {"update_rate": update_rate},
+                {"hardware_synchronization.expect_blocking_read_write": True},
             ],
         )
 
@@ -98,37 +117,23 @@ def _setup_controllers(context, *args, **kwargs):
 
         cm = ["--controller-manager", "/controller_manager"]
 
-        jtc_args = ["iiwa_arm_controller"] + cm
-        if controller == "forward":
-            jtc_args += ["--inactive"]
-
-        # ForwardCommandController: активен если controller=forward, иначе --inactive
-        forward_args = ["forward_position_controller"] + cm
-        if controller != "forward":
-            forward_args += ["--inactive"]
-
         jtc = Node(
             package="controller_manager",
             executable="spawner",
             output="screen",
-            arguments=jtc_args,
-        )
-
-        forward_controller = Node(
-            package="controller_manager",
-            executable="spawner",
-            output="screen",
-            arguments=forward_args,
+            arguments=["iiwa_arm_controller"] + cm,
         )
 
         jtc_after_jsb = RegisterEventHandler(
             OnProcessExit(
                 target_action=jsb,
-                on_exit=[jtc, forward_controller],
+                on_exit=[jtc],
             )
         )
 
         return [
+            LogInfo(msg=(f"FRI controller period={fri_cycle_ms} ms; Controller Manager rate={update_rate} Hz; "
+                         f"position smoothing tau={joint_position_tau:.3f} s")),
             ros2_control_node,
             jsb,
             jtc_after_jsb,
@@ -139,6 +144,7 @@ def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument("fri_cycle_ms", default_value="5"),
         DeclareLaunchArgument("joint_position_tau", default_value="0.04"),
-        DeclareLaunchArgument("controller", default_value="jtc"),
+        DeclareLaunchArgument("robot_ip", default_value="192.170.10.2"),
+        DeclareLaunchArgument("fri_port", default_value="30200"),
         OpaqueFunction(function=_setup_controllers),
     ])

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/simulation_nodes.py

@@ -1,4 +1,5 @@
 import json
+from dataclasses import asdict
 
 from launch_ros.actions import Node
 from webots_ros2_driver.webots_controller import WebotsController
@@ -7,12 +8,23 @@ from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf  # type
 
 
 def make_simulation_nodes(settings) -> list:
-    """Возвращает ноды камер для симуляции: [camera_spawner, *camera_controllers].
-
-    Если камеры не заданы в настройках — возвращает пустой список.
-    """
+    """Create the configured object spawner and camera nodes for Webots."""
+    nodes = []
+    webots = settings.digital_twin.webots
+    if webots.objects.enabled and webots.objects.count:
+        nodes.append(Node(
+            package="iiwa_utils",
+            executable="object_spawner",
+            name="object_spawner",
+            output="screen",
+            parameters=[{
+                "objects_config": json.dumps(asdict(webots.objects)),
+                "robot_translation": webots.transform,
+                "robot_rotation": webots.rotation,
+            }],
+        ))
     if not settings.digital_twin.webots.cameras:
-        return []
+        return nodes
 
     camera_spawner = Node(
         package="iiwa_utils",
@@ -37,4 +49,4 @@ def make_simulation_nodes(settings) -> list:
             respawn=True,
         ))
 
-    return [camera_spawner, *camera_controllers]
+    return [*nodes, camera_spawner, *camera_controllers]

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,
@@ -39,6 +40,13 @@ def _spawn_setup(context, *args, **kwargs):
     with open(controller, "r") as f:
         controller_cfg = yaml.safe_load(f) or {}
 
+    # FRI sets the hardware cycle dynamically in controllers.launch.py. Webots
+    # has no FRI period, so preserve its established 200 Hz controller cycle.
+    manager_params = controller_cfg.setdefault("controller_manager", {}).setdefault(
+        "ros__parameters", {}
+    )
+    manager_params["update_rate"] = 200
+
     combined = {
         "/**": {"ros__parameters": {
             "robot_description": robot_description,
@@ -57,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)

src/iiwa_bringup/package.xml

@@ -11,6 +11,7 @@
 
   <!-- Внутренние пакеты проекта -->
   <exec_depend>iiwa_utils</exec_depend>
+  <exec_depend>iiwa_planning</exec_depend>
   <exec_depend>iiwa_config</exec_depend>
   <exec_depend>iiwa_description</exec_depend>
   <exec_depend>foxglove_bridge</exec_depend>
@@ -26,7 +27,7 @@
   <exec_depend>rviz2</exec_depend>
 
   <!-- ros2_control -->
-  <exec_depend>controller_manager</exec_depend>
+  <exec_depend version_gte="4.48.0">controller_manager</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>forward_command_controller</exec_depend>

src/iiwa_config/config/cameras/d455_top.yaml

@@ -17,7 +17,9 @@ rotation: "0 1 0 1.5708"
 camera:
   width: 640
   height: 480
-  fieldOfView: 1.047        # ~60° в радианах
+  # VGA RGB: приближение центрального 4:3 crop сенсора 1280x800 с HFOV 90°.
+  # 2*atan(tan(90°/2)*(4/3)/(1280/800)) = 79.61°; уточнить по калибровке.
+  fieldOfView: 1.3894765524
   near: 0.1
   far: 0.0                  # 0 = без ограничений
   exposure: 1.0
@@ -40,13 +42,13 @@ camera:
 range_finder:
   width: 640
   height: 480
-  fieldOfView: 1.047
-  minRange: 0.1
-  maxRange: 5.0
-  resolution: -1.0          # -1 = без ограничений по дискретизации
+  fieldOfView: 1.308996939  # D455 VGA depth: HFOV 75°, не HD 87°
+  minRange: 0.4            # Выбранное ограничение симуляции, не точный Min-Z VGA
+  maxRange: 6.0
+  resolution: 0.001        # Шаг глубины 1 мм, не погрешность измерения
   noise: 0.0
 
   # Параметры публикации в ROS2
   ros:
     topic: /d455_top/depth/image_raw
-    update_rate: 15
+    update_rate: 30

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -1,6 +1,8 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200  # резервное значение; при запуске через launch: 1000 // fri_cycle_ms
+    # `update_rate` задаётся FRI launch-файлом как 1000 / robot.fri_cycle_ms.
+    # Не фиксировать его здесь: KLI Position всегда выдаёт 10 мс (100 Гц),
+    # а KONI может работать с 5 мс (200 Гц).
 
     joint_state_broadcaster:
       type: "joint_state_broadcaster/JointStateBroadcaster"
@@ -8,8 +10,6 @@ controller_manager:
     iiwa_arm_controller:
       type: "joint_trajectory_controller/JointTrajectoryController"
 
-    forward_position_controller:
-      type: "forward_command_controller/ForwardCommandController"
 
 joint_state_broadcaster:
   ros__parameters:
@@ -58,15 +58,3 @@ iiwa_arm_controller:
     #   joint5: { trajectory: 0.1, goal: 0.01 }
     #   joint6: { trajectory: 0.1, goal: 0.01 }
     #   joint7: { trajectory: 0.1, goal: 0.01 }
-
-forward_position_controller:
-  ros__parameters:
-    joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
-    interface_name: position

src/iiwa_config/config/setting.yaml

@@ -1,15 +1,20 @@
 robot:
   name: "iiwa7"
-  ip: "192.170.10.2"
+  ip: "192.168.21.147"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.04   # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI
-  joint_velocity_tau: 0.01   # EMA фильтр скорости [с]: убирает выбросы конечных разностей
-  active_controller: "jtc"  # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController
+  joint_position_tau: 0.04  # EMA позиций FRI [с]; 0 отключает сглаживание
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 
 digital_twin:
+  tcp_gizmo:
+    enabled: true             # TCP-гизма в Webots; только в режиме симуляции
+    publish_period: 0.032      # период управления [с]
+    command_timeout: 0.3       # максимальный возраст цели при отпускании мыши [с]
+    state_timeout: 0.5         # допустимый возраст состояния робота [с]
+    max_linear_speed: 0.50     # верхний предел скорости TCP [м/с]
+    max_angular_speed: 0.4     # скорость вращения TCP [рад/с]
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     # world: pkg://iiwa_description/worlds/simple_world.wbt
@@ -17,6 +22,26 @@ digital_twin:
     rotation: "0 0 1 0"
     controller_timer: "50"
 
+    objects:
+      enabled: true
+      count: 5                    # точное общее количество; модели могут повторяться
+      models:
+        - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer4.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer5.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+      area:                       # метры в системе основания робота, до падения
+        x: [0.3, 0.7]
+        y: [-0.3, 0.3]
+        z: [0.1, 0.1]             # высота нижней точки модели
+
+
     cameras:
       - pkg://iiwa_config/config/cameras/d455_top.yaml
 

src/iiwa_config/config/tools.yaml

@@ -15,7 +15,7 @@ tools:
     label: "Без захвата"
     xacro: null
     tip_link: "link_ee"
-    tcp_link: "link_ee"
+    tcp_link: "tcp"
     collisions: []
 
   patron:
@@ -61,4 +61,3 @@ tools:
       - [camera_hand, link2, Never]
       - [camera_hand, link1, Never]
       - [camera_hand, base_link, Never]
-

src/iiwa_controller/CMakeLists.txt

@@ -33,7 +33,7 @@ file(GLOB_RECURSE FRI_SOURCES
 
 add_library(fri_client_sdk STATIC ${FRI_SOURCES})
 
-target_include_directories(fri_client_sdk PUBLIC
+target_include_directories(fri_client_sdk SYSTEM PUBLIC
   ${FRI_SDK_DIR}/include
   ${FRI_SDK_DIR}/src/nanopb-0.2.8
   ${FRI_SDK_DIR}/src/protobuf
@@ -93,4 +93,17 @@ ament_export_targets(export_${PROJECT_NAME})
 ament_export_dependencies(
   controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs)
 
+if(BUILD_TESTING)
+  # Manual bounded loopback integration harness; no physical hardware access.
+  add_executable(fake_fri_server test/fake_fri_server.cpp)
+  target_link_libraries(fake_fri_server fri_client_sdk)
+  add_library(fri_loopback_bind SHARED test/loopback_bind.cpp)
+  target_link_libraries(fri_loopback_bind dl)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_fri_client test/test_fri_client.cpp)
+  target_link_libraries(test_fri_client ${PROJECT_NAME} fri_client_sdk)
+  ament_add_gtest(test_hardware test/test_hardware.cpp)
+  target_link_libraries(test_hardware ${PROJECT_NAME} fri_client_sdk hardware_interface::hardware_interface)
+endif()
+
 ament_package()

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -2,6 +2,7 @@
 
 #include <array>
 #include <atomic>
+#include <chrono>
 #include <mutex>
 
 #include "friClientApplication.h"
@@ -10,62 +11,70 @@
 
 namespace iiwa_controller
 {
-
-
-// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке
-// и так же за один lock читается из read() в потоке управления.
 struct IIWAStateSnapshot
 {
-  std::array<double, 7> measured_pos{}; // в Commanding = filtered_pos_ (open-loop)
-  std::array<double, 7> measured_tau{}; // измеренные моменты [Нм]
-  std::array<double, 7> external_tau{}; // внешние моменты без компенсации модели [Нм]
-  std::array<double, 7> ipo_pos{};      // позиция интерполятора [рад], только в Commanding
-  double sample_time{0.005};            // период цикла FRI [с]
+  std::array<double, 7> measured_pos{};  // encoder position [rad], never a command
+  std::array<double, 7> measured_tau{};  // measured torque [Nm]
+  std::array<double, 7> external_tau{};  // estimated external torque [Nm]
+  std::array<double, 7> ipo_pos{};       // interpolator position [rad]
+  std::array<double, 7> command_pos{};   // position sent in the current FRI reply [rad]
+  double sample_time{0.0};
+  KUKA::FRI::ESessionState session{KUKA::FRI::IDLE};
   KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
-  bool ipo_valid{false};                // в Monitor-режиме IPO недоступна
-  unsigned int time_stamp_sec{0};       // Unix-время пакета [с]
-  unsigned int time_stamp_nano_sec{0};  // наносекундная часть [нс]
+  KUKA::FRI::ESafetyState safety{KUKA::FRI::NORMAL_OPERATION};
+  KUKA::FRI::EDriveState drives{KUKA::FRI::OFF};
+  KUKA::FRI::EOperationMode operation{KUKA::FRI::TEST_MODE_1};
+  KUKA::FRI::EControlMode control{KUKA::FRI::NO_CONTROL};
+  KUKA::FRI::EClientCommandMode command_mode{KUKA::FRI::NO_COMMAND_MODE};
+  KUKA::FRI::EOverlayType overlay{KUKA::FRI::NO_OVERLAY};
+  double tracking_performance{0.0};
+  bool valid{false};
+  bool ipo_valid{false};
+  unsigned int time_stamp_sec{0};
+  unsigned int time_stamp_nano_sec{0};
+  std::chrono::steady_clock::time_point received_at{};
 };
 
 class FRIClient : public KUKA::FRI::LBRClient
 {
 public:
-  static constexpr size_t N_JOINTS = 7;
-
-  // joint_position_tau — постоянная времени экспоненциального фильтра позиций [с].
-  // Аналог joint_position_tau из lbr_fri_ros2_stack (по умолчанию 0.04 с = 40 мс).
-  // Сглаживает скачки команд перед отправкой роботу → убирает писк и стук суставов.
-  explicit FRIClient(double joint_position_tau = 0.04);
-  ~FRIClient() override = default;
-
-  // Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step()
+  static constexpr size_t N_JOINTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS;
+  using Joints = std::array<double, N_JOINTS>;
+  explicit FRIClient(double position_smoothing_tau = 0.04);
   void monitor() override;
   void waitForCommand() override;
   void command() override;
-  void onStateChange(
-    KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
+  void onStateChange(KUKA::FRI::ESessionState oldState,
+    KUKA::FRI::ESessionState newState) override;
 
-  // Потокобезопасное API для ros2_control, вызывается из read() и write()
-  void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
-  IIWAStateSnapshot getStateSnapshot() const;
+  // Configure before starting the FRI thread; limits come from the robot URDF.
+  void setLimits(const Joints & lower, const Joints & upper, const Joints & velocity);
+  void setExpectedSampleTime(double seconds) {expected_sample_time_ = seconds;}
+  void setTargetJointPositions(const Joints & q);
+  IIWAStateSnapshot getStateSnapshot() const;  // lifecycle/non-RT use
+  bool tryGetStateSnapshot(IIWAStateSnapshot & snapshot) const;
   bool isCommandingActive() const;
   KUKA::FRI::ESessionState getSessionState() const;
 
 private:
-  double joint_position_tau_;
   std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
-
-  mutable std::mutex data_mutex_;
-  std::array<double, N_JOINTS> target_pos_{};
-  // Сглаженная позиция, которую реально отправляем роботу.
-  // Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
-  std::array<double, N_JOINTS> filtered_pos_{};
-  IIWAStateSnapshot snapshot_{};
-
-  // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна)
-  void captureMonitoringData();
-  // Обновить snapshot_ вместе с ipo_pos (в Commanding-режиме)
-  void captureCommandingData();
+  mutable std::mutex state_mutex_;
+  std::mutex command_mutex_;
+  Joints target_pos_{};
+  Joints filtered_pos_{};
+  Joints requested_pos_{};
+  Joints sent_pos_{};
+  Joints lower_, upper_, max_velocity_;
+  bool initialized_{false};
+  bool requested_{false};
+  double position_smoothing_tau_{0.04};
+  double expected_sample_time_{0.0};
+  std::chrono::steady_clock::time_point requested_at_{};
+  std::chrono::steady_clock::time_point last_command_at_{};
+  IIWAStateSnapshot snapshot_{};  // published under state_mutex_
+  IIWAStateSnapshot current_{};   // owned exclusively by FRI thread
+  void captureData(bool commanding);
+  void publishState();
+  void validateCommanding() const;
 };
-
 }  // namespace iiwa_controller

src/iiwa_controller/include/iiwa_controller/FRICycleGate.hpp

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+
+namespace iiwa_controller
+{
+// One SDK step -> one read/update/write -> next SDK step. No queued commands,
+// no independent ROS timer and no overwrite of an unconsumed setpoint.
+class FRICycleGate
+{
+public:
+  void reset()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    stopped_ = available_ = acquired_ = completed_ = false;
+  }
+  void stop()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    stopped_ = true;
+    cv_.notify_all();
+  }
+  bool publishAndWait()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    if (stopped_) {return false;}
+    available_ = true;
+    completed_ = false;
+    cv_.notify_all();
+    if (!cv_.wait_for(lock, timeout_, [&] {return stopped_ || completed_;})) {
+      stopped_ = true;
+      cv_.notify_all();
+    }
+    return !stopped_;
+  }
+  bool waitReady(std::chrono::milliseconds timeout)
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return cv_.wait_for(lock, timeout, [&] {return stopped_ || available_;}) && !stopped_;
+  }
+  bool acquire()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    if (acquired_ || !cv_.wait_for(lock, timeout_, [&] {return stopped_ || available_;}) ||
+      stopped_) {return false;}
+    available_ = false;
+    acquired_ = true;
+    return true;
+  }
+  bool complete()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (stopped_ || !acquired_) {return false;}
+    acquired_ = false;
+    completed_ = true;
+    cv_.notify_all();
+    return true;
+  }
+private:
+  static constexpr std::chrono::milliseconds timeout_{100};
+  std::mutex mutex_;
+  std::condition_variable cv_;
+  bool stopped_{true}, available_{false}, acquired_{false}, completed_{false};
+};
+}  // namespace iiwa_controller

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -8,10 +8,7 @@
 #include <thread>
 #include <vector>
 
-// В Jazzy 4.44.0+ нельзя переопределять export_state_interfaces() и export_command_interfaces().
-// Устаревший конструктор не регистрирует introspection-callback pal_statistics,
-// из-за чего падает с segfault. Базовый класс сам создаёт интерфейсы из URDF.
-// Данные читаем и пишем через handle-API: set_state() и get_command().
+// Jazzy 4.48: framework-managed interfaces and handle API.
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_component_interface_params.hpp"
@@ -23,6 +20,7 @@
 #include "rclcpp_lifecycle/state.hpp"
 
 #include "iiwa_controller/FRIClient.h"
+#include "iiwa_controller/FRICycleGate.hpp"
 
 namespace iiwa_controller
 {
@@ -31,6 +29,7 @@ class IIWAHardwareInterface : public hardware_interface::SystemInterface
 {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface)
+  ~IIWAHardwareInterface() override;
 
   CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override;
@@ -39,13 +38,16 @@ public:
   std::vector<hardware_interface::InterfaceDescription>
   export_unlisted_state_interface_descriptions() override;
 
-  // Полный lifecycle: configure открывает сокет, activate запускает поток,
+  // Полный lifecycle: configure создаёт SDK, activate открывает сокет и запускает поток,
   // deactivate останавливает поток, cleanup освобождает FRI-объекты.
   CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
+
   hardware_interface::return_type read(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
@@ -58,44 +60,48 @@ private:
   // Параметры из секции <hardware><param> в URDF
   std::string robot_ip_;
   int fri_port_{30200};
-  bool simulate_{false};
+  int fri_cycle_ms_{5};
   double joint_position_tau_{0.04};
-  // EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей.
-  // joint_velocity_tau = 0 отключает фильтр (raw finite difference).
-  double joint_velocity_tau_{0.01};
-
+  int rt_prio_{80};
+  bool simulate_{false};
   // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;
   std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
   std::unique_ptr<KUKA::FRI::ClientApplication> app_;
 
   // FRI работает в отдельном потоке: step() блокируется в recvfrom().
-  // read() лишь читает готовый снимок — без блокировки RT-потока.
+  // read/write rendezvous once per SDK step; network I/O stays in this worker.
+  // Requires controller_manager hardware_synchronization mode (Jazzy 4.48+).
   std::thread fri_thread_;
+  FRICycleGate cycle_gate_;
   std::atomic<bool> fri_running_{false};
   void friThreadFunc();
+  void stopFRI();
+  void releaseFRI();
+  std::atomic<bool> fri_fault_{false};
+  bool active_{false};
+  IIWAStateSnapshot last_snapshot_{};
+  std::array<double, N_JOINTS> lower_, upper_, max_velocity_;
 
   // Хэндлы интерфейсов состояния, заполняются в on_activate
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_pos_;
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_vel_;
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_eff_;
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ext_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ipo_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_fri_cmd_;
 
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
 
-  // Вычисление скорости: конечные разности + EMA-фильтр
+  // Вычисление скорости по измеренной позиции и меткам времени FRI.
   std::array<double, N_JOINTS> prev_pos_{};
-  std::array<double, N_JOINTS> velocity_{};        // отфильтрованная скорость, публикуется в JTC
-  std::array<double, N_JOINTS> velocity_raw_{};    // сырая скорость до фильтра
+  std::array<double, N_JOINTS> velocity_{};        // измеренная скорость [рад/с]
   unsigned int last_ts_sec_{0};
   unsigned int last_ts_nsec_{0};
   bool velocity_initialized_{false};
   void compute_velocity_(const IIWAStateSnapshot & snap);
 
-  // Отслеживание сессии FRI для обнаружения потери управления
-  KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE};
 
-  rclcpp::Clock throttle_clock_{RCL_STEADY_TIME};
 
 };
 

src/iiwa_controller/package.xml

@@ -17,6 +17,7 @@
   <depend>realtime_tools</depend>
   <depend>std_msgs</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/iiwa_controller/src/FRIClient.cpp

@@ -1,155 +1,220 @@
 #include "iiwa_controller/FRIClient.h"
 
+#include <algorithm>
 #include <cmath>
-#include <cstring>
-
-#include <rclcpp/rclcpp.hpp>
+#include <limits>
+#include <stdexcept>
 
 namespace iiwa_controller
 {
+using namespace KUKA::FRI;
 
-static const char * friStateName(KUKA::FRI::ESessionState s)
+FRIClient::FRIClient(double position_smoothing_tau)
+: position_smoothing_tau_(position_smoothing_tau)
 {
-  switch (s) {
-    case KUKA::FRI::IDLE: return "IDLE";
-    case KUKA::FRI::MONITORING_WAIT: return "MONITORING_WAIT";
-    case KUKA::FRI::MONITORING_READY: return "MONITORING_READY";
-    case KUKA::FRI::COMMANDING_WAIT: return "COMMANDING_WAIT";
-    case KUKA::FRI::COMMANDING_ACTIVE: return "COMMANDING_ACTIVE";
-    default: return "UNKNOWN";
+  if (!std::isfinite(position_smoothing_tau_) || position_smoothing_tau_ < 0.0) {
+    throw std::invalid_argument("Position smoothing tau must be finite and non-negative");
   }
+  lower_.fill(-std::numeric_limits<double>::infinity());
+  upper_.fill(std::numeric_limits<double>::infinity());
+  max_velocity_.fill(std::numeric_limits<double>::infinity());
 }
 
-FRIClient::FRIClient(double joint_position_tau)
-: joint_position_tau_(joint_position_tau)
+void FRIClient::setLimits(const Joints & lower, const Joints & upper, const Joints & velocity)
 {
-  target_pos_.fill(0.0);
-  filtered_pos_.fill(0.0);
+  lower_ = lower;
+  upper_ = upper;
+  max_velocity_ = velocity;
 }
 
-// Вызывается только в Monitor-состояниях.
-// В Monitor-режиме getIpoJointPosition() бросает FRIException, поэтому здесь не зовём.
-void FRIClient::captureMonitoringData()
+void FRIClient::captureData(bool commanding)
 {
-  std::memcpy(
-    snapshot_.measured_pos.data(),
-    robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double));
-  std::memcpy(
-    snapshot_.measured_tau.data(),
-    robotState().getMeasuredTorque(), N_JOINTS * sizeof(double));
-  std::memcpy(
-    snapshot_.external_tau.data(),
-    robotState().getExternalTorque(), N_JOINTS * sizeof(double));
-  snapshot_.sample_time = robotState().getSampleTime();
-  snapshot_.quality = robotState().getConnectionQuality();
-  snapshot_.ipo_valid = false;
-  snapshot_.time_stamp_sec = robotState().getTimestampSec();
-  snapshot_.time_stamp_nano_sec = robotState().getTimestampNanoSec();
+  const auto & state = robotState();
+  std::copy_n(state.getMeasuredJointPosition(), N_JOINTS, current_.measured_pos.begin());
+  std::copy_n(state.getMeasuredTorque(), N_JOINTS, current_.measured_tau.begin());
+  std::copy_n(state.getExternalTorque(), N_JOINTS, current_.external_tau.begin());
+  if (!commanding) {
+    // No application command exists outside a commanding FRI state.
+    current_.command_pos = current_.measured_pos;
   }
-
-// Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE).
-// В отличие от Monitor, здесь getIpoJointPosition() доступна.
-void FRIClient::captureCommandingData()
+  current_.sample_time = state.getSampleTime();
+  current_.session = state.getSessionState();
+  current_.quality = state.getConnectionQuality();
+  current_.safety = state.getSafetyState();
+  current_.drives = state.getDriveState();
+  current_.operation = state.getOperationMode();
+  current_.control = state.getControlMode();
+  current_.command_mode = commanding ? state.getClientCommandMode() : NO_COMMAND_MODE;
+  current_.overlay = commanding ? state.getOverlayType() : NO_OVERLAY;
+  current_.tracking_performance = commanding ? state.getTrackingPerformance() : 0.0;
+  current_.ipo_valid = commanding;
+  if (commanding) {
+    std::copy_n(state.getIpoJointPosition(), N_JOINTS, current_.ipo_pos.begin());
+  }
+  const auto sec = state.getTimestampSec();
+  const auto nsec = state.getTimestampNanoSec();
+  if (current_.valid && (sec < current_.time_stamp_sec ||
+    (sec == current_.time_stamp_sec && nsec <= current_.time_stamp_nano_sec)))
   {
-  captureMonitoringData();
-  std::memcpy(
-    snapshot_.ipo_pos.data(),
-    robotState().getIpoJointPosition(), N_JOINTS * sizeof(double));
-  snapshot_.ipo_valid = true;
-  // Open-loop: JTC видит filtered_pos_ как «измеренную» позицию — как в lbr_fri_ros2_stack.
-  // Благодаря этому JTC не видит расхождения и не генерирует коррекций.
-  snapshot_.measured_pos = filtered_pos_;
+    throw std::runtime_error("FRI timestamp did not advance");
+  }
+  if (nsec >= 1000000000 || !std::isfinite(current_.sample_time) || current_.sample_time <= 0.0) {
+    throw std::runtime_error("Invalid FRI timestamp/sample period");
+  }
+  if (expected_sample_time_ > 0.0 &&
+    std::abs(current_.sample_time - expected_sample_time_) > 1e-9)
+  {
+    throw std::runtime_error(
+            "Sunrise FRI period is " + std::to_string(current_.sample_time * 1000.0) +
+            " ms, but robot.fri_cycle_ms is " +
+            std::to_string(expected_sample_time_ * 1000.0) + " ms");
+  }
+  if (current_.valid && current_.session == COMMANDING_ACTIVE) {
+    const double elapsed = static_cast<double>(sec - current_.time_stamp_sec) +
+      (static_cast<double>(nsec) - current_.time_stamp_nano_sec) * 1e-9;
+    if (std::abs(elapsed - current_.sample_time) > 1e-6) {
+      throw std::runtime_error("FRI commanding packet gap; refusing to catch up trajectory");
+    }
+  }
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    if (!std::isfinite(current_.measured_pos[i]) || !std::isfinite(current_.measured_tau[i]) ||
+      !std::isfinite(current_.external_tau[i]) ||
+      (commanding && !std::isfinite(current_.ipo_pos[i])))
+    {
+      throw std::runtime_error("Non-finite FRI telemetry");
+    }
+  }
+  current_.time_stamp_sec = sec;
+  current_.time_stamp_nano_sec = nsec;
+  current_.received_at = std::chrono::steady_clock::now();
+  current_.valid = true;
+}
+
+void FRIClient::publishState()
+{
+  std::unique_lock<std::mutex> lock(state_mutex_, std::try_to_lock);
+  if (lock.owns_lock()) {snapshot_ = current_;}
+}
+
+void FRIClient::validateCommanding() const
+{
+  if (current_.command_mode != POSITION || current_.overlay != JOINT ||
+    (current_.control != POSITION_CONTROL_MODE && current_.control != JOINT_IMP_CONTROL_MODE &&
+    current_.control != CART_IMP_CONTROL_MODE))
+  {
+    throw std::runtime_error("Expected FRI POSITION command mode with JOINT overlay");
+  }
+  if (current_.quality < GOOD || current_.safety != NORMAL_OPERATION ||
+    current_.drives != ACTIVE)
+  {
+    throw std::runtime_error("FRI commanding unavailable: quality, safety or drives");
+  }
 }
 
-// Вызывается в MONITORING_WAIT и MONITORING_READY
 void FRIClient::monitor()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
-  captureMonitoringData();
+  captureData(false);
+  LBRClient::monitor();
+  initialized_ = false;
+  publishState();
 }
 
-// Вызывается в COMMANDING_WAIT.
-// По документации FRI (п. 6.2.2) клиент должен отправлять команды в каждом цикле.
-// Переход в COMMANDING_ACTIVE происходит только когда разница между commanded_position
-// и IPO_position меньше 0.001 рад для всех суставов.
-// Важно эхировать именно IPO-позицию, не measured. Если взять measured,
-// статическое отклонение не даст выполниться этому условию.
 void FRIClient::waitForCommand()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
-  captureCommandingData();
-
-  // Инициализируем цель и фильтр IPO-позицией.
-  // Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE.
-  std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
-  std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
-
-  robotCommand().setJointPosition(filtered_pos_.data());
+  captureData(true);
+  validateCommanding();
+  LBRClient::waitForCommand();  // SDK 1.16 mirrors IPO, not measured position.
+  sent_pos_ = target_pos_ = filtered_pos_ = current_.ipo_pos;
+  current_.command_pos = sent_pos_;
+  initialized_ = true;
+  last_command_at_ = current_.received_at;
+  publishState();
 }
 
-// Вызывается в COMMANDING_ACTIVE, основной цикл управления
 void FRIClient::command()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
-
-  // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_
-  // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом).
-  // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA.
-  const double dt = robotState().getSampleTime();
-  const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0;
+  captureData(true);
+  validateCommanding();
+  if (!initialized_) {
+    sent_pos_ = target_pos_ = filtered_pos_ = current_.ipo_pos;
+    last_command_at_ = current_.received_at;
+    initialized_ = true;
+  }
+  {
+    std::lock_guard<std::mutex> lock(command_mutex_);
+    if (requested_ && requested_at_ > last_command_at_) {
+      target_pos_ = requested_pos_;
+      last_command_at_ = requested_at_;
+    }
+    requested_ = false;
+  }
+  // Local watchdog: a stalled ros2_control loop must not leave a moving target active.
+  if (current_.received_at - last_command_at_ > std::chrono::milliseconds(100)) {
+    throw std::runtime_error("FRI command watchdog expired");
+  }
+  const double dt = current_.sample_time;
+  const double alpha = position_smoothing_tau_ == 0.0 ? 1.0 :
+    dt / (position_smoothing_tau_ + dt);
   for (size_t i = 0; i < N_JOINTS; ++i) {
+    if (!std::isfinite(target_pos_[i]) || target_pos_[i] < lower_[i] ||
+      target_pos_[i] > upper_[i])
+    {
+      throw std::runtime_error("FRI position command outside configured limits");
+    }
     filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
+    const double step = filtered_pos_[i] - sent_pos_[i];
+    sent_pos_[i] += std::clamp(step, -max_velocity_[i] * dt, max_velocity_[i] * dt);
+  }
+  robotCommand().setJointPosition(sent_pos_.data());
+  current_.command_pos = sent_pos_;
+  publishState();
 }
 
-  robotCommand().setJointPosition(filtered_pos_.data());
-
-  // Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил.
-  captureCommandingData();
-}
-
-void FRIClient::onStateChange(
-  KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState)
+void FRIClient::onStateChange(ESessionState oldState, ESessionState newState)
 {
-  session_state_.store(newState, std::memory_order_relaxed);
+  session_state_.store(newState);
+  if (oldState == COMMANDING_ACTIVE && newState != COMMANDING_ACTIVE) {
+    throw std::runtime_error("FRI left COMMANDING_ACTIVE; explicit recovery required");
+  }
+}
 
-  RCLCPP_INFO(
-    rclcpp::get_logger("FRIClient"),
-    "FRI смена состояния: %s, теперь %s", friStateName(oldState), friStateName(newState));
-
-  if (newState == KUKA::FRI::IDLE ||
-    newState == KUKA::FRI::MONITORING_WAIT ||
-    newState == KUKA::FRI::MONITORING_READY)
+void FRIClient::setTargetJointPositions(const Joints & q)
 {
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    if (!std::isfinite(q[i]) || q[i] < lower_[i] || q[i] > upper_[i]) {
+      throw std::invalid_argument("Invalid FRI position command");
     }
   }
-
-void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
-{
-  // До первой команды контроллера интерфейс содержит NaN.
-  // Если отправить NaN роботу в COMMANDING_ACTIVE, получим CK_COMPOUND_RETURN_ERROR.
-  for (const auto & v : q) {
-    if (!std::isfinite(v)) {
-      return;
+  std::lock_guard<std::mutex> lock(command_mutex_);
+  if (requested_) {
+    throw std::logic_error("Previous FRI command has not been consumed");
   }
-  }
-  std::lock_guard<std::mutex> lock(data_mutex_);
-  target_pos_ = q;
+  requested_pos_ = q;
+  requested_at_ = std::chrono::steady_clock::now();
+  requested_ = true;
 }
 
 IIWAStateSnapshot FRIClient::getStateSnapshot() const
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
+  std::lock_guard<std::mutex> lock(state_mutex_);
   return snapshot_;
 }
 
+bool FRIClient::tryGetStateSnapshot(IIWAStateSnapshot & snapshot) const
+{
+  std::unique_lock<std::mutex> lock(state_mutex_, std::try_to_lock);
+  if (!lock.owns_lock()) {return false;}
+  snapshot = snapshot_;
+  return true;
+}
+
 bool FRIClient::isCommandingActive() const
 {
-  return session_state_.load(std::memory_order_relaxed) == KUKA::FRI::COMMANDING_ACTIVE;
+  return getSessionState() == COMMANDING_ACTIVE;
 }
 
-KUKA::FRI::ESessionState FRIClient::getSessionState() const
+ESessionState FRIClient::getSessionState() const
 {
-  return session_state_.load(std::memory_order_relaxed);
+  return session_state_.load();
 }
-
 }  // namespace iiwa_controller

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -1,5 +1,10 @@
 #include "iiwa_controller/IIWAHardwareInterface.hpp"
 
+#include <algorithm>
+#include <arpa/inet.h>
+#include <limits>
+#include <stdexcept>
+#include "friException.h"
 #include <chrono>
 #include <cmath>
 #include <cstdint>
@@ -10,6 +15,7 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_helpers.hpp"
 
 PLUGINLIB_EXPORT_CLASS(
   iiwa_controller::IIWAHardwareInterface,
@@ -21,6 +27,32 @@ namespace iiwa_controller
 using CallbackReturn =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
+// The SDK keeps a 100 ms receive bound after the first packet. During startup
+// only, allow 15 s for the operator to start Sunrise without retrying failed SDK steps.
+class StartupConnection : public KUKA::FRI::UdpConnection
+{
+public:
+  explicit StartupConnection(const std::atomic<bool> & running)
+  : KUKA::FRI::UdpConnection(100), running_(running) {}
+
+  int receive(char * buffer, int size) override
+  {
+    const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
+    do {
+      const int received = KUKA::FRI::UdpConnection::receive(buffer, size);
+      if (received >= 0) {
+        started_ = true;
+        return received;
+      }
+      if (started_) {return received;}
+    } while (running_.load() && std::chrono::steady_clock::now() < deadline);
+    return -1;
+  }
+private:
+  const std::atomic<bool> & running_;
+  bool started_{false};
+};
+
 static const char * LOG = "IIWAHardwareInterface";
 
 static std::string getParam(
@@ -43,18 +75,44 @@ CallbackReturn IIWAHardwareInterface::on_init(
 
   const auto & info = params.hardware_info;
 
+  try {
     robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
-  fri_port_            = std::stoi(getParam(info, "fri_port", "30200"));
-  simulate_            = (getParam(info, "simulate", "false") == "true");
-  joint_position_tau_  = std::stod(getParam(info, "joint_position_tau", "0.04"));
-  joint_velocity_tau_  = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
-
-  RCLCPP_INFO(rclcpp::get_logger(LOG),
-    "on_init: ip=%s port=%d simulate=%s pos_tau=%.3f vel_tau=%.3f",
-    robot_ip_.c_str(), fri_port_,
-    simulate_ ? "true" : "false",
-    joint_position_tau_,
-    joint_velocity_tau_);
+    size_t used = 0;
+    const auto port = getParam(info, "fri_port", "30200");
+    fri_port_ = std::stoi(port, &used);
+    if (used != port.size() || fri_port_ < 1 || fri_port_ > 65535) {
+      throw std::invalid_argument("Invalid fri_port");
+    }
+    const auto cycle = getParam(info, "fri_cycle_ms", "5");
+    fri_cycle_ms_ = std::stoi(cycle, &used);
+    if (used != cycle.size() || fri_cycle_ms_ < 1 || fri_cycle_ms_ > 100) {
+      throw std::invalid_argument("Invalid fri_cycle_ms (expected 1..100)");
+    }
+    const auto position_tau = getParam(info, "joint_position_tau", "0.04");
+    joint_position_tau_ = std::stod(position_tau, &used);
+    if (used != position_tau.size() || !std::isfinite(joint_position_tau_) ||
+      joint_position_tau_ < 0.0)
+    {
+      throw std::invalid_argument("Invalid joint_position_tau (expected >= 0)");
+    }
+    const auto rt_prio = getParam(info, "rt_prio", "80");
+    rt_prio_ = std::stoi(rt_prio, &used);
+    if (used != rt_prio.size() || rt_prio_ < 1 || rt_prio_ > 99) {
+      throw std::invalid_argument("Invalid rt_prio (expected 1..99)");
+    }
+    const auto simulation = getParam(info, "simulate", "false");
+    if (simulation != "true" && simulation != "false") {
+      throw std::invalid_argument("simulate must be true or false");
+    }
+    simulate_ = simulation == "true";
+    in_addr address{};
+    if (!simulate_ && inet_pton(AF_INET, robot_ip_.c_str(), &address) != 1) {
+      throw std::invalid_argument("robot_ip must be an IPv4 address");
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Invalid hardware parameters: %s", e.what());
+    return CallbackReturn::ERROR;
+  }
 
   if (info.joints.size() != N_JOINTS) {
     RCLCPP_FATAL(rclcpp::get_logger(LOG),
@@ -62,9 +120,41 @@ CallbackReturn IIWAHardwareInterface::on_init(
     return CallbackReturn::ERROR;
   }
 
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    const auto & joint = info.joints[i];
+    // SDK arrays are ordered A1..A7. Never silently permute axes.
+    if (joint.name != "joint" + std::to_string(i + 1) ||
+      joint.command_interfaces.size() != 1 || joint.command_interfaces[0].name != "position")
+    {
+      RCLCPP_ERROR(rclcpp::get_logger(LOG), "Expected joint1..joint7 with position commands");
+      return CallbackReturn::ERROR;
+    }
+    for (const auto * name : {"position", "velocity", "effort"}) {
+      if (std::none_of(joint.state_interfaces.begin(), joint.state_interfaces.end(),
+        [&](const auto & interface) {return interface.name == name;}))
+      {
+        return CallbackReturn::ERROR;
+      }
+    }
+    const auto limits = info.limits.find(joint.name);
+    if (limits == info.limits.end() || !limits->second.has_position_limits ||
+      !limits->second.has_velocity_limits)
+    {
+      RCLCPP_ERROR(rclcpp::get_logger(LOG), "Missing position/velocity limits for %s", joint.name.c_str());
+      return CallbackReturn::ERROR;
+    }
+    lower_[i] = limits->second.min_position;
+    upper_[i] = limits->second.max_position;
+    max_velocity_[i] = limits->second.max_velocity;
+    if (!std::isfinite(lower_[i]) || !std::isfinite(upper_[i]) || lower_[i] >= upper_[i] ||
+      !std::isfinite(max_velocity_[i]) || max_velocity_[i] <= 0.0)
+    {
+      return CallbackReturn::ERROR;
+    }
+  }
+
   prev_pos_.fill(0.0);
   velocity_.fill(0.0);
-  velocity_raw_.fill(0.0);
   return CallbackReturn::SUCCESS;
 }
 
@@ -74,21 +164,23 @@ std::vector<hardware_interface::InterfaceDescription>
 IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 {
   std::vector<hardware_interface::InterfaceDescription> descs;
-  descs.reserve(N_JOINTS);
+  descs.reserve(3 * N_JOINTS);
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
+    for (const auto * name : {"external_torque", "ipo_position", "fri_command_position"}) {
       hardware_interface::InterfaceInfo if_info;
-    if_info.name          = "external_torque";
+      if_info.name = name;
       if_info.data_type = "double";
       if_info.initial_value = "0.0";
       descs.emplace_back(info_.joints[i].name, if_info);
     }
+  }
 
   return descs;
 }
 
 // ── on_configure ───────────────────────────────────────────────────────────────
-// Открывает UDP-сокет и создаёт FRI-объекты. Не запускает поток.
+// Создаёт FRI-объекты. Сокет и поток запускаются при активации.
 
 CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State &)
 {
@@ -97,143 +189,171 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State
     return CallbackReturn::SUCCESS;
   }
 
+  releaseFRI();
   fri_client_ = std::make_unique<FRIClient>(joint_position_tau_);
-  // 100 мс таймаут recvfrom — поток корректно завершится после disconnect().
-  connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
+  fri_client_->setLimits(lower_, upper_, max_velocity_);
+  fri_client_->setExpectedSampleTime(fri_cycle_ms_ * 0.001);
+  connection_ = std::make_unique<StartupConnection>(fri_running_);
   app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
-
-  if (!app_->connect(fri_port_, robot_ip_.c_str())) {
-    RCLCPP_FATAL(rclcpp::get_logger(LOG),
-      "Не удалось открыть UDP-сокет на порту %d (робот: %s)", fri_port_, robot_ip_.c_str());
-    return CallbackReturn::ERROR;
-  }
-
-  RCLCPP_INFO(rclcpp::get_logger(LOG),
-    "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...",
-    fri_port_, robot_ip_.c_str());
-
   return CallbackReturn::SUCCESS;
 }
 
-// ── on_activate ────────────────────────────────────────────────────────────────
-// Получает хэндлы интерфейсов, запускает FRI-поток и ждёт установки сессии.
-
-CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
+CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State & state)
 {
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "Активация...");
-
+  stopFRI();
+  velocity_initialized_ = false;
+  last_snapshot_ = {};
+  try {
     for (size_t i = 0; i < N_JOINTS; ++i) {
-    const std::string & jn = info_.joints[i].name;
-
-    h_pos_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
-    h_vel_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_VELOCITY);
-    h_eff_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
-    h_ext_[i]     = get_state_interface_handle(jn + "/external_torque");
-    h_cmd_pos_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
-
-    if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || !h_cmd_pos_[i])
+      const auto & name = info_.joints[i].name;
+      h_pos_[i] = get_state_interface_handle(name + "/position");
+      h_vel_[i] = get_state_interface_handle(name + "/velocity");
+      h_eff_[i] = get_state_interface_handle(name + "/effort");
+      h_ext_[i] = get_state_interface_handle(name + "/external_torque");
+      h_ipo_[i] = get_state_interface_handle(name + "/ipo_position");
+      h_fri_cmd_[i] = get_state_interface_handle(name + "/fri_command_position");
+      h_cmd_pos_[i] = get_command_interface_handle(name + "/position");
+    }
+    if (!simulate_) {
+      // Recreate SDK sequence counters and buffers for every new session.
+      if (on_configure(state) != CallbackReturn::SUCCESS ||
+        !app_->connect(fri_port_, robot_ip_.c_str()))
       {
-      RCLCPP_FATAL(rclcpp::get_logger(LOG),
-        "Не удалось получить хэндл интерфейса для сустава '%s'. "
-        "Проверьте объявление <state_interface>/<command_interface> в URDF.",
-        jn.c_str());
+        releaseFRI();
+        return CallbackReturn::ERROR;
+      }
+      fri_fault_.store(false);
+      fri_running_.store(true);
+      cycle_gate_.reset();
+      fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
+      const bool ready = cycle_gate_.waitReady(std::chrono::seconds(15));
+      last_snapshot_ = fri_client_->getStateSnapshot();
+      if (!ready || fri_fault_.load() || !last_snapshot_.valid ||
+        last_snapshot_.quality < KUKA::FRI::GOOD ||
+        last_snapshot_.session < KUKA::FRI::MONITORING_READY ||
+        std::chrono::steady_clock::now() - last_snapshot_.received_at > std::chrono::milliseconds(100))
+      {
+        stopFRI();
+        RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI activation failed: no fresh ready session");
         return CallbackReturn::ERROR;
       }
     }
-
+    for (size_t i = 0; i < N_JOINTS; ++i) {
+      double position = 0.0;
       if (simulate_) {
-    return CallbackReturn::SUCCESS;
-  }
-
-  fri_running_.store(true, std::memory_order_relaxed);
-  fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
-
-  constexpr int kTimeoutMs = 15000;
-  constexpr int kPollMs    = 200;
-  for (int elapsed = 0;
-       fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs;
-       elapsed += kPollMs)
-  {
-    RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-      "Ожидание FRI-сессии... (%d мс)", elapsed);
-    std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs));
-  }
-
-  if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-      "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s",
-      kTimeoutMs / 1000, robot_ip_.c_str());
+        get_state(h_pos_[i], position, true);
+        if (!std::isfinite(position)) {position = 0.0;}
       } else {
-    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI сессия установлена!");
-    const auto snap = fri_client_->getStateSnapshot();
-    prev_pos_     = snap.measured_pos;
-    last_ts_sec_  = snap.time_stamp_sec;
-    last_ts_nsec_ = snap.time_stamp_nano_sec;
-    velocity_.fill(0.0);
-    velocity_initialized_ = true;
+        position = last_snapshot_.measured_pos[i];
       }
-
-  previous_session_state_ = fri_client_->getSessionState();
+      set_state(h_pos_[i], position, true);
+      set_state(h_vel_[i], 0.0, true);
+      set_state(h_eff_[i], simulate_ ? 0.0 : last_snapshot_.measured_tau[i], true);
+      set_state(h_ext_[i], simulate_ ? 0.0 : last_snapshot_.external_tau[i], true);
+      set_state(h_ipo_[i], position, true);
+      set_state(h_fri_cmd_[i], position, true);
+      set_command(h_cmd_pos_[i], position, true);
+    }
+  } catch (const std::exception & e) {
+    stopFRI();
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Activation failed: %s", e.what());
+    return CallbackReturn::ERROR;
+  }
+  active_ = true;
   return CallbackReturn::SUCCESS;
 }
 
-// ── on_deactivate ──────────────────────────────────────────────────────────────
-// Останавливает FRI-поток. FRI-объекты остаются — их очищает on_cleanup().
+void IIWAHardwareInterface::stopFRI()
+{
+  active_ = false;
+  fri_running_.store(false);
+  cycle_gate_.stop();
+  // Receive has a finite timeout. Never close a socket while step() uses it.
+  if (fri_thread_.joinable()) {fri_thread_.join();}
+  if (app_) {app_->disconnect();}
+}
+
+void IIWAHardwareInterface::releaseFRI()
+{
+  stopFRI();
+  // ClientApplication::~ClientApplication calls the connection by reference.
+  app_.reset();
+  connection_.reset();
+  fri_client_.reset();
+}
+
+IIWAHardwareInterface::~IIWAHardwareInterface() {releaseFRI();}
 
 CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
 {
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "Деактивация...");
-
-  if (!simulate_ && fri_running_.load()) {
-    fri_running_.store(false, std::memory_order_relaxed);
-    // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке.
-    // Только потом join(), иначе он зависнет навсегда.
-    if (app_) { app_->disconnect(); }
-    if (fri_thread_.joinable()) { fri_thread_.join(); }
-    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток остановлен");
-  }
-
-  for (size_t i = 0; i < N_JOINTS; ++i) {
-    h_pos_[i] = h_vel_[i] = h_eff_[i] = h_ext_[i] = nullptr;
-    h_cmd_pos_[i] = nullptr;
-  }
-
+  stopFRI();
   velocity_initialized_ = false;
   return CallbackReturn::SUCCESS;
 }
 
-// ── on_cleanup ─────────────────────────────────────────────────────────────────
-// Освобождает FRI-объекты. Вызывается после on_deactivate().
-
 CallbackReturn IIWAHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &)
 {
-  fri_client_.reset();
-  connection_.reset();
-  app_.reset();
+  releaseFRI();
   return CallbackReturn::SUCCESS;
 }
 
-// ── friThreadFunc ──────────────────────────────────────────────────────────────
+CallbackReturn IIWAHardwareInterface::on_shutdown(const rclcpp_lifecycle::State & state)
+{
+  return on_cleanup(state);
+}
+
+CallbackReturn IIWAHardwareInterface::on_error(const rclcpp_lifecycle::State & state)
+{
+  return on_cleanup(state);
+}
 
 void IIWAHardwareInterface::friThreadFunc()
 {
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен");
-
-  while (fri_running_.load(std::memory_order_relaxed)) {
+  try {
+    if (!realtime_tools::configure_sched_fifo(rt_prio_)) {
+      RCLCPP_WARN(rclcpp::get_logger(LOG),
+        "FRI worker could not enable FIFO priority %d; configure realtime permissions for this user",
+        rt_prio_);
+    } else {
+      RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI worker uses FIFO priority %d", rt_prio_);
+    }
+    bool synchronized = false;
+    const auto startup_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
+    while (fri_running_.load()) {
       if (!app_->step()) {
-      RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-        "FRI: step() вернул false, возможно потеряли соединение");
+        throw std::runtime_error("FRI step failed");
+      }
+      if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
+        throw std::runtime_error("FRI session ended");
+      }
+      const auto snapshot = fri_client_->getStateSnapshot();
+      synchronized = synchronized || (snapshot.quality >= KUKA::FRI::GOOD &&
+        snapshot.session >= KUKA::FRI::MONITORING_READY);
+      if (synchronized) {
+        if (!cycle_gate_.publishAndWait()) {
+          if (!fri_running_.load()) {break;}
+          throw std::runtime_error("FRI cycle was not completed by ros2_control within 100 ms");
+        }
+      } else if (std::chrono::steady_clock::now() >= startup_deadline) {
+        throw std::runtime_error("FRI did not reach MONITORING_READY within 15 s");
       }
     }
-
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток завершён");
+  } catch (const KUKA::FRI::FRIException & e) {
+    fri_fault_.store(true);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI SDK: %s", e.getErrorMessage());
+  } catch (const std::exception & e) {
+    fri_fault_.store(true);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI: %s", e.what());
+  } catch (...) {
+    fri_fault_.store(true);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Unknown FRI failure");
+  }
+  fri_running_.store(false);
+  cycle_gate_.stop();
 }
 
 // ── compute_velocity_ ──────────────────────────────────────────────────────────
-// Конечные разности + EMA-фильтр.
-// int64-вычитание timestamp'ов предотвращает потерю точности при больших Unix-значениях.
-// EMA-фильтр (joint_velocity_tau) убирает одиночные выбросы, которые видит JTC как
-// скачки состояния и компенсирует агрессивными командами → хруст двигателей.
+// Velocity is the finite difference of measured positions at robot timestamps.
 
 void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
 {
@@ -242,7 +362,6 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
     last_ts_sec_  = snap.time_stamp_sec;
     last_ts_nsec_ = snap.time_stamp_nano_sec;
     velocity_.fill(0.0);
-    velocity_raw_.fill(0.0);
     velocity_initialized_ = true;
     return;
   }
@@ -258,23 +377,10 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
     (static_cast<double>(snap.time_stamp_nano_sec) -
      static_cast<double>(last_ts_nsec_)) * 1e-9;
 
-  static constexpr std::array<double, N_JOINTS> kMaxVel =
-    {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
-  static constexpr double kVelDeadband = 1e-4;
 
   if (dt > 0.0) {
-    // EMA alpha для фильтра скорости: tau=0 → alpha=1 (без фильтра)
-    const double vel_alpha = (joint_velocity_tau_ > 0.0)
-      ? dt / (joint_velocity_tau_ + dt)
-      : 1.0;
-
     for (size_t i = 0; i < N_JOINTS; ++i) {
-      const double raw     = (snap.measured_pos[i] - prev_pos_[i]) / dt;
-      const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]);
-      velocity_raw_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
-
-      // EMA: velocity_[i] = alpha * raw + (1 - alpha) * prev_filtered
-      velocity_[i] = vel_alpha * velocity_raw_[i] + (1.0 - vel_alpha) * velocity_[i];
+      velocity_[i] = (snap.measured_pos[i] - prev_pos_[i]) / dt;
     }
   }
 
@@ -288,31 +394,40 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
 hardware_interface::return_type IIWAHardwareInterface::read(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
+  if (!active_) {return hardware_interface::return_type::OK;}
   if (simulate_) {
     for (size_t i = 0; i < N_JOINTS; ++i) {
       double pos = 0.0;
-      get_command(h_cmd_pos_[i], pos, false);
+      if (!get_command(h_cmd_pos_[i], pos, false) || !std::isfinite(pos)) {
+        return hardware_interface::return_type::ERROR;
+      }
       set_state(h_pos_[i], pos, false);
       set_state(h_vel_[i], 0.0, false);
       set_state(h_eff_[i], 0.0, false);
       set_state(h_ext_[i], 0.0, false);
+      set_state(h_ipo_[i], pos, false);
+      set_state(h_fri_cmd_[i], pos, false);
     }
     return hardware_interface::return_type::OK;
   }
 
-  const auto snap = fri_client_->getStateSnapshot();
-
-  // Обнаружение потери управления: неожиданный выход из COMMANDING_ACTIVE.
-  const auto current_state = fri_client_->getSessionState();
-  if (previous_session_state_ == KUKA::FRI::COMMANDING_ACTIVE &&
-      current_state != KUKA::FRI::COMMANDING_ACTIVE)
+  if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) {
+    return hardware_interface::return_type::ERROR;
+  }
+  if (!cycle_gate_.acquire()) {
+    fri_fault_.store(true);
+    fri_running_.store(false);
+    cycle_gate_.stop();
+    return hardware_interface::return_type::ERROR;
+  }
+  last_snapshot_ = fri_client_->getStateSnapshot();
+  const auto & snap = last_snapshot_;
+  if (!snap.valid || std::chrono::steady_clock::now() - snap.received_at >
+    std::chrono::milliseconds(100))
   {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-      "Робот вышел из COMMANDING_ACTIVE! Деактивируйте и повторно активируйте контроллер.");
+    fri_running_.store(false);
     return hardware_interface::return_type::ERROR;
   }
-  previous_session_state_ = current_state;
-
   compute_velocity_(snap);
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
@@ -320,6 +435,8 @@ hardware_interface::return_type IIWAHardwareInterface::read(
     set_state(h_vel_[i], velocity_[i],         false);
     set_state(h_eff_[i], snap.measured_tau[i], false);
     set_state(h_ext_[i], snap.external_tau[i], false);
+    set_state(h_ipo_[i], snap.ipo_valid ? snap.ipo_pos[i] : snap.measured_pos[i], false);
+    set_state(h_fri_cmd_[i], snap.command_pos[i], false);
   }
 
   return hardware_interface::return_type::OK;
@@ -330,22 +447,34 @@ hardware_interface::return_type IIWAHardwareInterface::read(
 hardware_interface::return_type IIWAHardwareInterface::write(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
-  if (simulate_) {
-    return hardware_interface::return_type::OK;
+  if (!active_ || simulate_) {return hardware_interface::return_type::OK;}
+  if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) {
+    return hardware_interface::return_type::ERROR;
   }
-
-  if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) {
-    return hardware_interface::return_type::OK;
+  if (!fri_client_->isCommandingActive()) {
+    return cycle_gate_.complete() ? hardware_interface::return_type::OK :
+           hardware_interface::return_type::ERROR;
   }
-
   std::array<double, N_JOINTS> pos_cmd{};
   for (size_t i = 0; i < N_JOINTS; ++i) {
-    get_command(h_cmd_pos_[i], pos_cmd[i], false);
+    if (!get_command(h_cmd_pos_[i], pos_cmd[i], false)) {
+      cycle_gate_.stop();
+      return hardware_interface::return_type::ERROR;
     }
-
+    if (!std::isfinite(pos_cmd[i]) || pos_cmd[i] < lower_[i] || pos_cmd[i] > upper_[i]) {
+      fri_fault_.store(true);
+      fri_running_.store(false);
+      return hardware_interface::return_type::ERROR;
+    }
+  }
+  try {
     fri_client_->setTargetJointPositions(pos_cmd);
-
-  return hardware_interface::return_type::OK;
+  } catch (const std::exception &) {
+    cycle_gate_.stop();
+    return hardware_interface::return_type::ERROR;
+  }
+  return cycle_gate_.complete() ? hardware_interface::return_type::OK :
+         hardware_interface::return_type::ERROR;
 }
 
 }  // namespace iiwa_controller

src/iiwa_controller/test/fake_fri_server.cpp

@@ -0,0 +1,112 @@
+// Offline protocol peer, not a dynamics or Sunrise safety simulator.
+// Both endpoints are hardcoded loopback addresses. Never accepts a robot IP.
+#include <arpa/inet.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <thread>
+#include "FRIMessages.pb.h"
+#include "pb_encode.h"
+#include "pb_decode.h"
+#include "pb_frimessages_callbacks.h"
+
+int main(int argc, char ** argv)
+{
+  if (argc != 2) {return 2;}
+  std::ofstream csv(argv[1]);
+  csv << "sequence,session,receive_seconds,position,delta\n";
+  csv.flush();
+  int fd = socket(AF_INET, SOCK_DGRAM, 0);
+  sockaddr_in local{}, remote{};
+  local.sin_family = remote.sin_family = AF_INET;
+  local.sin_port = remote.sin_port = htons(30283);
+  inet_pton(AF_INET, "127.0.0.2", &local.sin_addr);
+  inet_pton(AF_INET, "127.0.0.1", &remote.sin_addr);
+  if (fd < 0 || bind(fd, reinterpret_cast<sockaddr *>(&local), sizeof(local))) {
+    perror("fake FRI loopback bind"); return 2;
+  }
+  timeval timeout{0, 20000};
+  setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
+  std::array<double, 7> q{}, zero{}, received{};
+  std::array<int64_t, 7> drives{};
+  drives.fill(DriveState_ACTIVE);
+  tRepeatedDoubleArguments positions{7, 7, q.data()}, zeros{7, 7, zero.data()};
+  tRepeatedIntArguments drive_values{7, 7, drives.data()};
+  FRIMonitoringMessage m{};
+  m.header.messageIdentifier = 0x245142;  // Bundled SDK LBRMONITORMESSAGEID.
+  m.has_robotInfo = m.has_monitorData = m.has_connectionInfo = true;
+  m.robotInfo.has_numberOfJoints = m.robotInfo.has_safetyState = true;
+  m.robotInfo.has_operationMode = m.robotInfo.has_controlMode = true;
+  m.robotInfo.numberOfJoints = 7;
+  m.robotInfo.safetyState = SafetyState_NORMAL_OPERATION;
+  m.robotInfo.operationMode = OperationMode_AUTOMATIC_MODE;
+  m.robotInfo.controlMode = ControlMode_POSITION_CONTROLMODE;
+  m.robotInfo.driveState.funcs.encode = encode_repeatedInt;
+  m.robotInfo.driveState.arg = &drive_values;
+  m.connectionInfo.quality = FRIConnectionQuality_GOOD;
+  m.connectionInfo.has_sendPeriod = m.connectionInfo.has_receiveMultiplier = true;
+  m.connectionInfo.sendPeriod = 10;
+  m.connectionInfo.receiveMultiplier = 1;
+  auto & md = m.monitorData;
+  md.has_timestamp = md.has_measuredJointPosition = md.has_commandedJointPosition = true;
+  md.has_measuredTorque = md.has_commandedTorque = md.has_externalTorque = true;
+  for (auto * values : {&md.measuredJointPosition, &md.commandedJointPosition, &m.ipoData.jointPosition}) {
+    values->value.funcs.encode = encode_repeatedDouble;
+    values->value.arg = &positions;
+  }
+  for (auto * values : {&md.measuredTorque, &md.commandedTorque, &md.externalTorque}) {
+    values->value.funcs.encode = encode_repeatedDouble;
+    values->value.arg = &zeros;
+  }
+  m.ipoData.has_jointPosition = m.ipoData.has_clientCommandMode = true;
+  m.ipoData.has_overlayType = m.ipoData.has_trackingPerformance = true;
+  m.ipoData.clientCommandMode = ClientCommandMode_POSITION;
+  m.ipoData.overlayType = OverlayType_JOINT;
+  m.ipoData.trackingPerformance = 1.0;
+  using Clock = std::chrono::steady_clock;
+  const auto start = Clock::now();
+  auto next = start;
+  unsigned replies = 0;
+  for (unsigned seq = 0; Clock::now() - start < std::chrono::seconds(45); ++seq) {
+    m.header.sequenceCounter = seq;
+    m.connectionInfo.sessionState = replies < 20 ? FRISessionState_MONITORING_READY :
+      replies < 40 ? FRISessionState_COMMANDING_WAIT : FRISessionState_COMMANDING_ACTIVE;
+    m.has_ipoData = replies >= 20;
+    md.timestamp.sec = 1 + seq / 100;
+    md.timestamp.nanosec = (seq % 100) * 10000000;
+    uint8_t buffer[2048];
+    auto output = pb_ostream_from_buffer(buffer, sizeof(buffer));
+    if (!pb_encode(&output, FRIMonitoringMessage_fields, &m)) {return 3;}
+    sendto(fd, buffer, output.bytes_written, 0, reinterpret_cast<sockaddr *>(&remote), sizeof(remote));
+    auto count = recv(fd, buffer, sizeof(buffer), 0);
+    if (count > 0) {
+      FRICommandMessage command{};
+      tRepeatedDoubleArguments target{0, 7, received.data()};
+      command.commandData.jointPosition.value.funcs.decode = decode_repeatedDouble;
+      command.commandData.jointPosition.value.arg = &target;
+      auto input = pb_istream_from_buffer(buffer, count);
+      if (!pb_decode(&input, FRICommandMessage_fields, &command) ||
+        !command.has_commandData || !command.commandData.has_jointPosition) {return 4;}
+      for (double value : received) {if (!std::isfinite(value)) {return 5;}}
+      const double delta = received[0] - q[0];
+      csv << seq << ',' << m.connectionInfo.sessionState << ',' <<
+        std::chrono::duration<double>(Clock::now() - start).count() << ',' << received[0] << ',' << delta << '\n';
+      csv.flush();
+      q = received;
+      m.header.reflectedSequenceCounter = command.header.sequenceCounter;
+      ++replies;
+    } else if (replies) {
+      std::cerr << "Reply timeout after " << replies << " frames\n";
+      close(fd); return 6;
+    }
+    // Before connection, avoid catch-up bursts after receive timeouts.
+    next = replies ? next + std::chrono::milliseconds(10) : Clock::now();
+    std::this_thread::sleep_until(next);
+  }
+  close(fd);
+  return 0;
+}

src/iiwa_controller/test/loopback_bind.cpp

@@ -0,0 +1,20 @@
+// Test-only interposer: allow the unmodified SDK and fake robot to share one host.
+// The SDK otherwise binds all local addresses at the robot's source port.
+#include <arpa/inet.h>
+#include <dlfcn.h>
+#include <sys/socket.h>
+
+extern "C" int bind(int fd, const sockaddr * address, socklen_t length)
+{
+  using Bind = int (*)(int, const sockaddr *, socklen_t);
+  static auto real_bind = reinterpret_cast<Bind>(dlsym(RTLD_NEXT, "bind"));
+  if (address->sa_family == AF_INET && length == sizeof(sockaddr_in)) {
+    auto local = *reinterpret_cast<const sockaddr_in *>(address);
+    // Dedicated smoke-test port only; never alter DDS or other application binds.
+    if (local.sin_port == htons(30283) && local.sin_addr.s_addr == htonl(INADDR_ANY)) {
+      inet_pton(AF_INET, "127.0.0.1", &local.sin_addr);
+      return real_bind(fd, reinterpret_cast<const sockaddr *>(&local), sizeof(local));
+    }
+  }
+  return real_bind(fd, address, length);
+}

src/iiwa_controller/test/smoke_fri.py

@@ -0,0 +1,83 @@
+"""Manual bounded real-SDK/JTC integration check over loopback only.
+
+Run after building/sourcing iiwa_controller; never connects physical hardware.
+"""
+import csv
+import sys
+import time
+import os
+import signal
+import subprocess
+import tempfile
+from pathlib import Path
+import xacro
+import yaml
+
+root = Path(__file__).resolve().parents[3]
+work = Path(tempfile.mkdtemp(prefix='iiwa-fri-smoke-'))
+env = dict(os.environ, ROS_DOMAIN_ID='83', ROS_LOCALHOST_ONLY='1', ROS_LOG_DIR=str(work / 'logs'), ROS_HOME=str(work / 'ros'))
+env['CYCLONEDDS_URI'] = '<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="lo"/></Interfaces><AllowMulticast>false</AllowMulticast></General><Discovery><Peers><Peer Address="127.0.0.1"/></Peers></Discovery></Domain></CycloneDDS>'
+xml = xacro.process_file(str(root / 'src/iiwa_description/urdf/iiwa7.urdf.xacro'), mappings={'robot_ip': '127.0.0.2', 'fri_port': '30283', 'fri_cycle_ms': '10'}).toxml()
+assert '<param name="simulate">false</param>' in xml
+assert '<param name="robot_ip">127.0.0.2</param>' in xml
+assert '<param name="fri_port">30283</param>' in xml
+build = Path(sys.argv[1]) if len(sys.argv) > 1 else root / 'build/iiwa_controller'
+server = build / 'fake_fri_server'
+shim = build / 'libfri_loopback_bind.so'
+assert server.is_file() and shim.is_file(), 'Build the test harness first'
+trace = work / 'commands.csv'
+params = work / 'robot.yaml'
+params.write_text(yaml.safe_dump({'/**': {'ros__parameters': {'robot_description': xml}}}))
+processes = []
+handles = []
+try:
+    server_log = (work / 'fake_fri_server.log').open('w')
+    handles.append(server_log)
+    processes.append(subprocess.Popen([str(server), str(trace)], stdout=server_log, stderr=subprocess.STDOUT, start_new_session=True))
+    time.sleep(0.1)
+    assert processes[0].poll() is None, 'Fake peer failed to bind'
+    for package, executable, extra in [
+        ('robot_state_publisher', 'robot_state_publisher', []),
+        ('controller_manager', 'ros2_control_node', ['--params-file', str(root / 'src/iiwa_config/config/moveit/iiwa_controller.yaml'), '-p', 'update_rate:=100', '-p', 'hardware_synchronization.expect_blocking_read_write:=true']),
+    ]:
+        log = (work / (executable + '.log')).open('w')
+        handles.append(log)
+        processes.append(subprocess.Popen(['ros2', 'run', package, executable, '--ros-args', '--params-file', str(params)] + extra,
+                                          env=dict(env, **({'LD_PRELOAD': str(shim)} if executable == 'ros2_control_node' else {})), stdout=log, stderr=subprocess.STDOUT, start_new_session=True))
+    for name in ['joint_state_broadcaster', 'iiwa_arm_controller']:
+        result = subprocess.run(['ros2', 'run', 'controller_manager', 'spawner', name, '--controller-manager-timeout', '15'],
+                                env=env, text=True, capture_output=True, timeout=25)
+        print(result.stdout, result.stderr, flush=True)
+        assert result.returncode == 0, name
+    result = subprocess.run(['ros2', 'control', 'list_controllers'], env=env, text=True, capture_output=True, timeout=10)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'iiwa_arm_controller' in result.stdout and 'active' in result.stdout
+    initial = yaml.safe_load((root / 'src/iiwa_config/config/moveit/initial_positions.yaml').read_text())['initial_positions']
+    positions = [float(initial[f'joint{i}']) for i in range(1, 8)]
+    positions[0] += 0.02
+    goal = {'trajectory': {'joint_names': [f'joint{i}' for i in range(1, 8)],
+            'points': [{'positions': positions, 'time_from_start': {'sec': 2, 'nanosec': 0}}]}}
+    result = subprocess.run(['ros2', 'action', 'send_goal', '/iiwa_arm_controller/follow_joint_trajectory',
+                             'control_msgs/action/FollowJointTrajectory', yaml.safe_dump(goal)],
+                            env=env, text=True, capture_output=True, timeout=15)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'SUCCEEDED' in result.stdout
+    rows = list(csv.DictReader(trace.open()))
+    assert len(rows) > 100 and processes[0].poll() is None, 'FRI stream stopped'
+    active = [r for r in rows if int(r['session']) == 4]
+    assert active and abs(float(active[-1]['position']) - 0.02) < 0.001
+    deltas = [abs(float(r['delta'])) for r in active]
+    intervals = [float(b['receive_seconds']) - float(a['receive_seconds']) for a, b in zip(active, active[1:])]
+    assert max(deltas) <= 1.71 * 0.01 + 1e-6, 'Command exceeds per-frame velocity limit'
+    print(f'SMOKE PASS: real SDK loopback + actual CM/JTC action succeeded; {len(active)} active frames, max delta={max(deltas):.8f} rad; receive interval min/max={min(intervals):.6f}/{max(intervals):.6f} s.', flush=True)
+finally:
+    for process in processes:
+        if process.poll() is None:
+            os.killpg(process.pid, signal.SIGINT)
+    for process in processes:
+        try: process.wait(timeout=5)
+        except subprocess.TimeoutExpired: os.killpg(process.pid, signal.SIGKILL)
+    for handle in handles: handle.close()
+    for log in work.glob('*.log'):
+        print('\n' + log.name + '\n' + log.read_text()[-4500:])
+    print('Logs:', work)

src/iiwa_controller/test/smoke_jtc.py

@@ -0,0 +1,59 @@
+"""Manual local JTC integration check; never opens a FRI connection."""
+import os
+import signal
+import subprocess
+import tempfile
+from pathlib import Path
+import xacro
+import yaml
+
+root = Path(__file__).resolve().parents[3]
+work = Path(tempfile.mkdtemp(prefix='iiwa-controller-smoke-'))
+env = dict(os.environ, ROS_DOMAIN_ID='83', ROS_LOCALHOST_ONLY='1', ROS_LOG_DIR=str(work / 'logs'), ROS_HOME=str(work / 'ros'))
+env['CYCLONEDDS_URI'] = '<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="lo"/></Interfaces><AllowMulticast>false</AllowMulticast></General><Discovery><Peers><Peer Address="127.0.0.1"/></Peers></Discovery></Domain></CycloneDDS>'
+xml = xacro.process_file(str(root / 'src/iiwa_description/urdf/iiwa7.urdf.xacro')).toxml()
+assert '<param name="simulate">false</param>' in xml
+# Exercise our plugin's explicit local simulation path, with no FRI socket.
+xml = xml.replace('<param name="simulate">false</param>', '<param name="simulate">true</param>')
+params = work / 'robot.yaml'
+params.write_text(yaml.safe_dump({'/**': {'ros__parameters': {'robot_description': xml}}}))
+processes = []
+handles = []
+try:
+    for package, executable, extra in [
+        ('robot_state_publisher', 'robot_state_publisher', []),
+        ('controller_manager', 'ros2_control_node', ['--params-file', str(root / 'src/iiwa_config/config/moveit/iiwa_controller.yaml')]),
+    ]:
+        log = (work / (executable + '.log')).open('w')
+        handles.append(log)
+        processes.append(subprocess.Popen(['ros2', 'run', package, executable, '--ros-args', '--params-file', str(params)] + extra,
+                                          env=env, stdout=log, stderr=subprocess.STDOUT, start_new_session=True))
+    for name in ['joint_state_broadcaster', 'iiwa_arm_controller']:
+        result = subprocess.run(['ros2', 'run', 'controller_manager', 'spawner', name, '--controller-manager-timeout', '15'],
+                                env=env, text=True, capture_output=True, timeout=25)
+        print(result.stdout, result.stderr, flush=True)
+        assert result.returncode == 0, name
+    result = subprocess.run(['ros2', 'control', 'list_controllers'], env=env, text=True, capture_output=True, timeout=10)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'iiwa_arm_controller' in result.stdout and 'active' in result.stdout
+    initial = yaml.safe_load((root / 'src/iiwa_config/config/moveit/initial_positions.yaml').read_text())['initial_positions']
+    positions = [float(initial[f'joint{i}']) for i in range(1, 8)]
+    positions[0] += 0.02
+    goal = {'trajectory': {'joint_names': [f'joint{i}' for i in range(1, 8)],
+            'points': [{'positions': positions, 'time_from_start': {'sec': 2, 'nanosec': 0}}]}}
+    result = subprocess.run(['ros2', 'action', 'send_goal', '/iiwa_arm_controller/follow_joint_trajectory',
+                             'control_msgs/action/FollowJointTrajectory', yaml.safe_dump(goal)],
+                            env=env, text=True, capture_output=True, timeout=15)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'SUCCEEDED' in result.stdout
+    print('SMOKE PASS: actual URDF, iiwa hardware plugin, broadcaster and JTC active; no FRI connection.', flush=True)
+finally:
+    for process in processes:
+        os.killpg(process.pid, signal.SIGINT)
+    for process in processes:
+        try: process.wait(timeout=5)
+        except subprocess.TimeoutExpired: os.killpg(process.pid, signal.SIGKILL)
+    for handle in handles: handle.close()
+    for log in work.glob('*.log'):
+        print('\n' + log.name + '\n' + log.read_text()[-4500:])
+    print('Logs:', work)

src/iiwa_controller/test/test_fri_client.cpp

@@ -0,0 +1,279 @@
+#include <gtest/gtest.h>
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <thread>
+#include "iiwa_controller/FRIClient.h"
+#include "iiwa_controller/FRICycleGate.hpp"
+#include <atomic>
+#include <vector>
+#include "friClientData.h"
+#include "pb_frimessages_callbacks.h"
+
+using namespace KUKA::FRI;
+
+class TestApplication : public ClientApplication
+{
+public:
+  using ClientApplication::ClientApplication;
+  ClientData * data() {return _data;}
+};
+
+class ClientFixture : public iiwa_controller::FRIClient
+{
+public:
+  explicit ClientFixture(double position_smoothing_tau = 0.04)
+  : FRIClient(position_smoothing_tau), app(connection, *this), data(app.data())
+  {
+    auto & m = data->monitoringMsg;
+    m.connectionInfo.sendPeriod = 10;
+    m.connectionInfo.quality = FRIConnectionQuality_GOOD;
+    m.robotInfo.safetyState = SafetyState_NORMAL_OPERATION;
+    m.robotInfo.operationMode = OperationMode_AUTOMATIC_MODE;
+    m.robotInfo.controlMode = ControlMode_POSITION_CONTROLMODE;
+    auto * drives = static_cast<tRepeatedIntArguments *>(m.robotInfo.driveState.arg);
+    std::fill_n(drives->value, 7, DriveState_ACTIVE);
+    m.ipoData.clientCommandMode = ClientCommandMode_POSITION;
+    m.ipoData.overlayType = OverlayType_JOINT;
+    m.ipoData.has_trackingPerformance = false;
+    m.monitorData.timestamp.sec = 1;
+    m.monitorData.timestamp.nanosec = 0;
+    fill(m.monitorData.measuredJointPosition, 0.2);
+    fill(m.monitorData.commandedJointPosition, 0.3);
+    fill(m.monitorData.measuredTorque, 1.0);
+    fill(m.monitorData.externalTorque, 0.1);
+    fill(m.ipoData.jointPosition, 0.4);
+  }
+  static void fill(JointValues & values, double value)
+  {
+    auto * args = static_cast<tRepeatedDoubleArguments *>(values.value.arg);
+    std::fill_n(args->value, 7, value);
+    args->size = 7;
+  }
+  void cycle(ESessionState state)
+  {
+    data->resetCommandMessage();
+    data->monitoringMsg.ipoData.has_jointPosition = state >= COMMANDING_WAIT;
+    data->monitoringMsg.connectionInfo.sessionState = static_cast<FRISessionState>(state);
+    onStateChange(data->lastState, state);
+    data->lastState = state;
+    data->monitoringMsg.monitorData.timestamp.nanosec += 10000000;
+    if (state == COMMANDING_WAIT) waitForCommand();
+    else if (state == COMMANDING_ACTIVE) command();
+    else monitor();
+  }
+  double sentPosition()
+  {
+    return static_cast<tRepeatedDoubleArguments *>(
+      data->commandMsg.commandData.jointPosition.value.arg)->value[0];
+  }
+  UdpConnection connection;
+  TestApplication app;
+  ClientData * data;
+};
+
+TEST(FRIClient, ReportsMeasuredPositionInCommanding)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4);
+  EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2);
+}
+
+TEST(FRIClient, PreservesSdkMonitoringReplyWithoutIpo)
+{
+  ClientFixture client;
+  client.cycle(MONITORING_READY);
+  EXPECT_TRUE(client.data->commandMsg.commandData.has_jointPosition);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.3);
+  EXPECT_FALSE(client.getStateSnapshot().ipo_valid);
+}
+
+TEST(FRIClient, RejectsUnsupportedCommandModeBeforeSending)
+{
+  ClientFixture client;
+  client.data->monitoringMsg.ipoData.clientCommandMode = ClientCommandMode_TORQUE;
+  EXPECT_THROW(client.cycle(COMMANDING_WAIT), std::runtime_error);
+  EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
+}
+
+TEST(FRIClient, PositionCommandsAreSmoothedBeforeSending)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  iiwa_controller::FRIClient::Joints q;
+  q.fill(0.41);
+  client.setTargetJointPositions(q);
+  client.cycle(COMMANDING_ACTIVE);
+  // tau=40 ms, dt=10 ms: alpha=dt/(tau+dt)=0.2.
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.402);
+  EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2);
+}
+
+TEST(FRIClient, ZeroPositionSmoothingTauForwardsStepImmediately)
+{
+  ClientFixture client(0.0);
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  iiwa_controller::FRIClient::Joints q;
+  q.fill(0.41);
+  client.setTargetJointPositions(q);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.41);
+}
+
+TEST(FRIClient, RejectsInvalidCommandsAndBoundsCommandRate)
+{
+  ClientFixture client;
+  iiwa_controller::FRIClient::Joints lower, upper, velocity, q;
+  lower.fill(-1.0); upper.fill(1.0); velocity.fill(1.0);
+  client.setLimits(lower, upper, velocity);
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  q.fill(0.8);
+  client.setTargetJointPositions(q);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_NEAR(client.sentPosition(), 0.41, 1e-12);  // 1 rad/s * 10 ms
+  q[0] = 1.1;
+  EXPECT_THROW(client.setTargetJointPositions(q), std::invalid_argument);
+  q[0] = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_THROW(client.setTargetJointPositions(q), std::invalid_argument);
+}
+
+TEST(FRIClient, SafetyStopCannotSendApplicationCommand)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.data->monitoringMsg.robotInfo.safetyState = SafetyState_SAFETY_STOP_LEVEL_1;
+  EXPECT_THROW(client.cycle(COMMANDING_ACTIVE), std::runtime_error);
+  EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
+}
+
+TEST(FRIClient, DuplicateTimestampIsNotFreshTelemetry)
+{
+  ClientFixture client;
+  client.cycle(MONITORING_READY);
+  EXPECT_THROW(client.monitor(), std::runtime_error);
+}
+
+TEST(FRIClient, RejectsMismatchedPeriodAndMissingCommandingPackets)
+{
+  ClientFixture mismatch;
+  mismatch.setExpectedSampleTime(0.005);
+  EXPECT_THROW(mismatch.cycle(MONITORING_READY), std::runtime_error);
+  ClientFixture gap;
+  gap.cycle(COMMANDING_WAIT);
+  gap.data->monitoringMsg.monitorData.timestamp.nanosec += 10000000;
+  EXPECT_THROW(gap.cycle(COMMANDING_ACTIVE), std::runtime_error);
+  EXPECT_FALSE(gap.data->commandMsg.commandData.has_jointPosition);
+}
+
+TEST(FRIClient, LeavingCommandingRequiresExplicitRecovery)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_THROW(client.cycle(MONITORING_READY), std::runtime_error);
+}
+
+TEST(FRIClient, PositionOverlaySupportsSunriseImpedanceModes)
+{
+  for (auto mode : {ControlMode_JOINT_IMPEDANCE_CONTROLMODE,
+    ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE})
+  {
+    ClientFixture client;
+    client.data->monitoringMsg.robotInfo.controlMode = mode;
+    EXPECT_NO_THROW(client.cycle(COMMANDING_WAIT));
+    EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4);
+  }
+}
+
+TEST(FRIClient, StalledRosCommandStreamStopsBeforeSending)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  std::this_thread::sleep_for(std::chrono::milliseconds(110));
+  EXPECT_THROW(client.cycle(COMMANDING_ACTIVE), std::runtime_error);
+  EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
+}
+
+TEST(FRIClient, RejectsOverwritingAnUnconsumedPoint)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  iiwa_controller::FRIClient::Joints q;
+  q.fill(0.401);
+  client.setTargetJointPositions(q);
+  q.fill(0.402);
+  EXPECT_THROW(client.setTargetJointPositions(q), std::logic_error);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4002);
+}
+
+TEST(FRIClient, SynchronizedCyclesPreserveEveryPointDespiteSchedulingJitter)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  iiwa_controller::FRICycleGate gate;
+  gate.reset();
+  std::atomic<bool> worker_ok{true};
+  std::vector<double> sent;
+  std::thread worker([&] {
+    for (int frame = 0; frame <= 8; ++frame) {
+      client.cycle(COMMANDING_ACTIVE);
+      sent.push_back(client.sentPosition());
+      if (frame < 8 && !gate.publishAndWait()) {worker_ok = false; break;}
+    }
+  });
+  for (int frame = 1; frame <= 8; ++frame) {
+    if (!gate.acquire()) {worker_ok = false; break;}
+    // Variable controller execution time cannot cause a point to be overwritten.
+    std::this_thread::sleep_for(std::chrono::milliseconds(frame % 3));
+    iiwa_controller::FRIClient::Joints q;
+    q.fill(0.4 + frame * 0.001);
+    client.setTargetJointPositions(q);
+    if (!gate.complete()) {worker_ok = false; break;}
+  }
+  worker.join();
+  ASSERT_TRUE(worker_ok);
+  ASSERT_EQ(sent.size(), 9u);
+  double expected = 0.4;
+  for (size_t frame = 1; frame < sent.size(); ++frame) {
+    const double target = 0.4 + static_cast<double>(frame) * 0.001;
+    expected = 0.2 * target + 0.8 * expected;
+    EXPECT_NEAR(sent[frame], expected, 1e-12);
+  }
+}
+
+TEST(FRICycleGate, DuplicateReadOrWriteCannotAdvanceWorker)
+{
+  iiwa_controller::FRICycleGate gate;
+  gate.reset();
+  EXPECT_FALSE(gate.complete());
+  std::thread worker([&] {EXPECT_TRUE(gate.publishAndWait());});
+  EXPECT_TRUE(gate.acquire());
+  EXPECT_FALSE(gate.acquire());
+  EXPECT_TRUE(gate.complete());
+  EXPECT_FALSE(gate.complete());
+  worker.join();
+}
+
+TEST(FRICycleGate, StopWakesWorkerAndMissingWriteTimesOut)
+{
+  iiwa_controller::FRICycleGate gate;
+  gate.reset();
+  std::thread worker([&] {EXPECT_FALSE(gate.publishAndWait());});
+  EXPECT_TRUE(gate.acquire());
+  gate.stop();
+  worker.join();
+  gate.reset();
+  EXPECT_FALSE(gate.publishAndWait());
+  EXPECT_FALSE(gate.acquire());
+  EXPECT_FALSE(gate.complete());
+}

src/iiwa_controller/test/test_hardware.cpp

@@ -0,0 +1,109 @@
+#include <gtest/gtest.h>
+#include "iiwa_controller/IIWAHardwareInterface.hpp"
+
+using Hardware = iiwa_controller::IIWAHardwareInterface;
+using Return = hardware_interface::CallbackReturn;
+
+hardware_interface::HardwareComponentInterfaceParams parameters()
+{
+  hardware_interface::HardwareComponentInterfaceParams params;
+  auto & info = params.hardware_info;
+  info.name = "iiwa_test";
+  info.type = "system";
+  info.hardware_parameters["simulate"] = "true";
+  for (int i = 1; i <= 7; ++i) {
+    hardware_interface::ComponentInfo joint;
+    joint.name = "joint" + std::to_string(i);
+    for (const auto * name : {"position", "velocity", "effort"}) {
+      hardware_interface::InterfaceInfo interface;
+      interface.name = name;
+      interface.data_type = "double";
+      interface.initial_value = "0.2";
+      joint.state_interfaces.push_back(interface);
+    }
+    hardware_interface::InterfaceInfo command;
+    command.name = "position";
+    command.data_type = "double";
+    joint.command_interfaces.push_back(command);
+    joint_limits::JointLimits limits;
+    limits.has_position_limits = true;
+    limits.min_position = -2.0;
+    limits.max_position = 2.0;
+    limits.has_velocity_limits = true;
+    limits.max_velocity = 1.0;
+    info.limits[joint.name] = limits;
+    info.joints.push_back(joint);
+  }
+  return params;
+}
+
+TEST(Hardware, RejectsMalformedPortWithoutThrowing)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["fri_port"] = "30200oops";
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
+
+TEST(Hardware, RejectsInvalidRealtimePriority)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["rt_prio"] = "0";
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
+
+TEST(Hardware, RejectsInvalidPositionSmoothingTau)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["joint_position_tau"] = "-0.01";
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
+
+TEST(Hardware, RejectsWrongJointOrderAndMissingInterfaces)
+{
+  auto params = parameters();
+  std::swap(params.hardware_info.joints[0], params.hardware_info.joints[1]);
+  Hardware wrong_order;
+  EXPECT_EQ(wrong_order.on_init(params), Return::ERROR);
+  params = parameters();
+  params.hardware_info.joints[0].state_interfaces.pop_back();
+  Hardware missing_interface;
+  EXPECT_EQ(missing_interface.on_init(params), Return::ERROR);
+}
+
+TEST(Hardware, SimulationSeedsCommandsAndSupportsRepeatedLifecycle)
+{
+  Hardware hardware;
+  ASSERT_EQ(hardware.on_init(parameters()), Return::SUCCESS);
+  auto states = hardware.on_export_state_interfaces();
+  auto commands = hardware.on_export_command_interfaces();
+  const rclcpp_lifecycle::State state;
+  ASSERT_EQ(hardware.on_configure(state), Return::SUCCESS);
+  for (int cycle = 0; cycle < 2; ++cycle) {
+    ASSERT_EQ(hardware.on_activate(state), Return::SUCCESS);
+    const double command = hardware.get_command<double>("joint1/position");
+    EXPECT_DOUBLE_EQ(command, 0.2);
+    EXPECT_EQ(hardware.read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+      hardware_interface::return_type::OK);
+    EXPECT_EQ(hardware.on_deactivate(state), Return::SUCCESS);
+    EXPECT_EQ(hardware.read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+      hardware_interface::return_type::OK);
+  }
+  EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS);
+  EXPECT_EQ(hardware.on_shutdown(state), Return::SUCCESS);
+}
+
+TEST(Hardware, ConfiguredRealHardwareCleansUpInDependencyOrder)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["simulate"] = "false";
+  params.hardware_info.hardware_parameters["robot_ip"] = "127.0.0.1";
+  ASSERT_EQ(hardware.on_init(params), Return::SUCCESS);
+  const rclcpp_lifecycle::State state;
+  ASSERT_EQ(hardware.on_configure(state), Return::SUCCESS);
+  EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS);
+  EXPECT_EQ(hardware.on_error(state), Return::SUCCESS);
+}

src/iiwa_description/CMakeLists.txt

@@ -9,7 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY resource worlds urdf
+  DIRECTORY resource worlds urdf objects
   DESTINATION share/${PROJECT_NAME}
 )
 

src/iiwa_description/objects/hammer/hammer1.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 01_|_circular_ground_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_exposed_cut_wood
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_honey_hickory_longitudinal_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_satin_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png

src/iiwa_description/objects/hammer/hammer2.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 02_|_black_oxide_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_blue_glass_fibre_resin
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_graphite_moulded_rubber
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_machined_steel_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png

src/iiwa_description/objects/hammer/hammer3.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 03_|_ash_end_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_blue_tempered_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_bright_milled_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_pale_ash_straight_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png

src/iiwa_description/objects/hammer/hammer4.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 04_|_dark_walnut_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_polished_ball_and_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_walnut_end_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_warm_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png

src/iiwa_description/objects/hammer/hammer5.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 05_|_charcoal_anti_vibration_rubber
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_dark_textured_forged_coating
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_machined_impact_faces
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_orange_glass_fibre_resin
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -6,8 +6,8 @@
     <xacro:arg name="simulate" default="false"/>
     <xacro:arg name="robot_ip" default="192.170.10.2"/>
     <xacro:arg name="fri_port" default="30200"/>
+    <xacro:arg name="fri_cycle_ms" default="5"/>
     <xacro:arg name="joint_position_tau" default="0.04"/>
-    <xacro:arg name="joint_velocity_tau" default="0.01"/>
 
     <xacro:property name="initial_positions"
                     value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -17,6 +17,7 @@
     <xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
     <xacro:include filename="$(find iiwa_description)/urdf/joints.xacro"/>
 
+    <xacro:property name="tool_has_camera" value="false"/>
     <xacro:include filename="$(find iiwa_description)/urdf/tools/tool_active.xacro"/>
 
     <xacro:if value="$(arg simulate)">
@@ -26,6 +27,7 @@
 
             <device reference="camera_rgb" type="Camera">
                 <ros>
+                    <enabled>false</enabled>
                     <topicName>/camera/color</topicName>
                     <updateRate>30</updateRate>
                     <alwaysOn>True</alwaysOn>
@@ -35,6 +37,7 @@
 
             <device reference="camera_depth" type="RangeFinder">
                 <ros>
+                    <enabled>false</enabled>
                     <topicName>/camera/depth</topicName>
                     <updateRate>30</updateRate>
                     <alwaysOn>True</alwaysOn>
@@ -42,6 +45,23 @@
                 </ros>
             </device>
 
+            <!-- Keep the 30 Hz deadlines independent of Webots' 32 ms steps. -->
+            <xacro:if value="${tool_has_camera}">
+                <plugin type="iiwa_utils.webots_camera.WebotsCamera">
+                    <device>camera_rgb</device>
+                    <topicName>/camera/color</topicName>
+                    <updateRate>30</updateRate>
+                    <frameName>camera_optical_frame</frameName>
+                </plugin>
+                <plugin type="iiwa_utils.webots_camera.WebotsCamera">
+                    <device>camera_depth</device>
+                    <depth>true</depth>
+                    <topicName>/camera/depth</topicName>
+                    <updateRate>30</updateRate>
+                    <frameName>camera_optical_frame</frameName>
+                </plugin>
+            </xacro:if>
+
         </webots>
 
         <ros2_control name="iiwaGazeboControl" type="system">
@@ -145,9 +165,10 @@
                 <plugin>iiwa_controller/IIWAHardwareInterface</plugin>
                 <param name="robot_ip">$(arg robot_ip)</param>
                 <param name="fri_port">$(arg fri_port)</param>
-                <param name="simulate">false</param>
+                <param name="fri_cycle_ms">$(arg fri_cycle_ms)</param>
                 <param name="joint_position_tau">$(arg joint_position_tau)</param>
-                <param name="joint_velocity_tau">$(arg joint_velocity_tau)</param>
+                <param name="rt_prio">80</param>
+                <param name="simulate">false</param>
             </hardware>
 
             <joint name="joint1">

src/iiwa_description/urdf/tools/patron.xacro

@@ -1,6 +1,8 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_patron">
 
+    <xacro:property name="tool_has_camera" value="true"/>
+
     <link name="camera_holder">
         <inertial>
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
@@ -107,7 +109,8 @@
     </joint>
 
     <joint name="patron_tcp" type="fixed">
-        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5708"/>
+        <!-- Working end of patron.dae, 55.2 mm from the patron link origin. -->
+        <origin xyz="0.0 0.0 0.0552" rpy="0.0 0.0 -1.5708"/>
         <parent link="patron"/>
         <child link="tcp"/>
     </joint>
@@ -133,7 +136,8 @@
     <gazebo reference="camera_optical_frame">
         <sensor type="camera" name="camera_rgb">
             <camera>
-                <horizontal_fov>1.3089969</horizontal_fov>
+                <!-- VGA RGB central-crop approximation: 79.61 deg; calibrate per device. -->
+                <horizontal_fov>1.3894765524</horizontal_fov>
                 <image>
                     <width>640</width>
                     <height>480</height>
@@ -145,7 +149,7 @@
                 </clip>
                 <noise>
                     <type>gaussian</type>
-                    <stddev>0.005</stddev>
+                    <stddev>0.0</stddev>
                 </noise>
             </camera>
         </sensor>
@@ -158,18 +162,20 @@
                     <height>480</height>
                 </image>
                 <clip>
-                    <near>0.52</near>
+                    <near>0.01</near>
                     <far>6.0</far>
                 </clip>
             </camera>
             <range>
-                <min>0.52</min>
+                <!-- Conservative simulation cutoff, not calibrated VGA Min-Z. -->
+                <min>0.4</min>
                 <max>6.0</max>
                 <resolution>0.001</resolution>
             </range>
             <noise>
                 <type>gaussian</type>
-                <stddev>0.02</stddev>
+                <!-- No arbitrary Gaussian noise: this is an ideal depth model. -->
+                <stddev>0.0</stddev>
             </noise>
         </sensor>
     </gazebo> 

src/iiwa_msgs/CMakeLists.txt

@@ -7,6 +7,9 @@ find_package(geometry_msgs REQUIRED)
 find_package(action_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/GizmoCommand.msg"
+  "msg/GizmoState.msg"
+  "srv/MotionPriority.srv"
   "srv/MoveToPose.srv"
   "srv/MoveToNamedPose.srv"
   "action/MoveToPose.action"

src/iiwa_msgs/msg/GizmoCommand.msg

@@ -0,0 +1,3 @@
+# One committed TCP target on mouse release, with generation from GizmoState.
+geometry_msgs/PoseStamped target
+uint64 epoch

src/iiwa_msgs/msg/GizmoState.msg

@@ -0,0 +1,6 @@
+# Actual TCP pose; enabled is false while any higher-priority motion owns control.
+geometry_msgs/PoseStamped actual
+bool enabled
+# Advances on rejection, controller completion, or higher-priority ownership.
+uint64 epoch
+string reason

src/iiwa_msgs/srv/MotionPriority.srv

@@ -0,0 +1,7 @@
+# Acquire/renew or release a named high-priority lease. TTL uses wall time [s].
+string owner
+bool acquire
+float64 ttl
+---
+bool success
+string message

src/iiwa_planning/CMakeLists.txt

@@ -9,6 +9,8 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(control_msgs REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_core REQUIRED)
@@ -16,6 +18,16 @@ find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(iiwa_msgs REQUIRED)
+find_package(moveit_servo REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(tf2_eigen REQUIRED)
+
+add_executable(tcp_gizmo src/tcp_gizmo.cpp)
+target_compile_features(tcp_gizmo PUBLIC cxx_std_17)
+target_include_directories(tcp_gizmo PRIVATE include)
+ament_target_dependencies(tcp_gizmo rclcpp rclcpp_action control_msgs iiwa_msgs geometry_msgs trajectory_msgs
+  action_msgs moveit_servo moveit_ros_planning moveit_core tf2_eigen)
 
 add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
 target_include_directories(motion_planning_cpp PUBLIC include)
@@ -33,6 +45,7 @@ ament_target_dependencies(motion_planning_cpp
 ament_python_install_package(${PROJECT_NAME})
 
 install(TARGETS
+  tcp_gizmo
   motion_planning_cpp
   DESTINATION lib/${PROJECT_NAME}
 )
@@ -55,4 +68,14 @@ install(FILES
 )
 
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_gtest(test_tcp_gizmo_policy test/test_tcp_gizmo_policy.cpp)
+  target_compile_features(test_tcp_gizmo_policy PUBLIC cxx_std_17)
+  target_include_directories(test_tcp_gizmo_policy PRIVATE include)
+  ament_add_pytest_test(test_motion_priority test/test_motion_priority.py
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR})
+endif()
+
 ament_package()

src/iiwa_planning/iiwa_planning/motion_priority.py

@@ -0,0 +1,146 @@
+"""Wall-clock motion reservations and serialization, independent of ROS simulation time."""
+import threading
+import uuid
+
+
+class LeaseError(RuntimeError):
+    pass
+
+
+class MotionLease:
+    """Renew one owner until context exit; transport errors fail closed."""
+    def __init__(self, exchange=None, *, on_lost=lambda: None, ttl=5.0,
+                 renew_interval=1.0):
+        self.owner = uuid.uuid4().hex
+        self._exchange = exchange
+        self._on_lost = on_lost
+        self._ttl = ttl
+        self._interval = renew_interval
+        self._stop = threading.Event()
+        self._lost = threading.Event()
+        self._thread = None
+
+    def __enter__(self):
+        if self._exchange is not None:
+            self._exchange(self.owner, True, self._ttl)
+            self._thread = threading.Thread(target=self._renew, daemon=True)
+            self._thread.start()
+        return self
+
+    def _renew(self):
+        while not self._stop.wait(self._interval):
+            try:
+                self._exchange(self.owner, True, self._ttl)
+            except Exception:
+                self._lost.set()
+                try:
+                    self._on_lost()
+                finally:
+                    return
+
+    @property
+    def lost(self):
+        return self._lost.is_set()
+
+    def check(self):
+        if self.lost:
+            raise LeaseError('Motion priority reservation lost')
+
+    def __exit__(self, exc_type, exc, tb):
+        self._stop.set()
+        if self._thread is not None:
+            self._thread.join(timeout=3.0)
+            try:
+                self._exchange(self.owner, False, self._ttl)
+            except Exception:
+                # Expiry bounds cleanup even if the service disappeared.
+                pass
+        if exc_type is None:
+            self.check()
+        return False
+
+
+class MotionSerialiser:
+    """One PlanningComponent owner; stop invalidates both planning and queued work."""
+    def __init__(self):
+        self.lock = threading.Lock()
+        self._state_lock = threading.Lock()
+        self._generation = 0
+
+    def generation(self):
+        with self._state_lock:
+            return self._generation
+
+    def check(self, generation):
+        if generation != self.generation():
+            raise LeaseError('Motion interrupted by stop')
+
+    def stop(self, callback):
+        with self._state_lock:
+            self._generation += 1
+        callback()
+
+
+def wait_response(future, timeout=2.0, *, available=lambda: True):
+    """Wait only from worker threads; an independent executor services the future."""
+    done = threading.Event()
+    future.add_done_callback(lambda _: done.set())
+    if timeout is None:
+        # Acquisition is acknowledged only after a committed gizmo trajectory
+        # finishes. Its duration can exceed the normal service RPC timeout.
+        while not done.wait(.2):
+            if not available():
+                future.cancel()
+                raise LeaseError('Motion priority service unavailable while waiting')
+    elif not done.wait(timeout):
+        future.cancel()
+        raise LeaseError('Motion priority service response timed out')
+    result = future.result()
+    if result is None:
+        raise LeaseError('Motion priority service returned no response')
+    return result
+
+
+class MotionPriorityClient:
+    """Dedicated executor prevents lease starvation under concurrent motion requests."""
+    def __init__(self, *, context=None):
+        from rclpy.node import Node
+        from rclpy.callback_groups import ReentrantCallbackGroup
+        from rclpy.executors import MultiThreadedExecutor
+        from iiwa_msgs.srv import MotionPriority
+        self._type = MotionPriority
+        self._node = Node('motion_priority_' + uuid.uuid4().hex[:12], context=context,
+                          start_parameter_services=False, use_global_arguments=False)
+        self._client = self._node.create_client(
+            MotionPriority, '/cobot/tcp_gizmo/priority',
+            callback_group=ReentrantCallbackGroup())
+        self._executor = MultiThreadedExecutor(num_threads=2, context=context)
+        self._executor.add_node(self._node)
+        self._thread = threading.Thread(target=self._spin, daemon=True)
+        self._thread.start()
+
+    def _spin(self):
+        from rclpy.executors import ExternalShutdownException
+        try:
+            self._executor.spin()
+        except ExternalShutdownException:
+            pass
+
+    def create_stop_service(self, callback):
+        from std_srvs.srv import Trigger
+        return self._node.create_service(Trigger, "/cobot/stop", callback)
+
+    def exchange(self, owner, acquire, ttl):
+        if not self._client.wait_for_service(timeout_sec=0.5):
+            raise LeaseError('Motion priority service unavailable')
+        request = self._type.Request(owner=owner, acquire=acquire, ttl=ttl)
+        result = wait_response(
+            self._client.call_async(request), timeout=None if acquire else 2.0,
+            available=lambda: self._node.context.ok() and self._client.service_is_ready())
+        if not result.success:
+            raise LeaseError(result.message)
+
+    def close(self):
+        self._executor.shutdown(timeout_sec=2.0)
+        self._thread.join(timeout=2.0)
+        self._node.destroy_node()

src/iiwa_planning/include/iiwa_planning/tcp_gizmo_policy.hpp

@@ -0,0 +1,65 @@
+#pragma once
+#include <cmath>
+#include <cstdint>
+#include <map>
+#include <string>
+
+namespace iiwa_planning {
+inline bool requiresGizmoMotion(double translation_error, double angular_error) {
+  return translation_error >= 1e-5 || angular_error >= 1e-4;
+}
+
+// Pure arbitration state. The ROS node serializes all access. Steady time is
+// used for leases/commands; simulation time is used for trajectory deadlines.
+class GizmoPolicy {
+public:
+  std::uint64_t epoch() const { return epoch_; }
+  bool hasTarget() const { return target_; }
+  void invalidate() { target_ = false; ++epoch_; }
+  bool accept(std::uint64_t epoch, double now, double timeout) {
+    if (epoch != epoch_) return false;
+    target_ = true; target_deadline_ = now + timeout; return true;
+  }
+  bool expired(double now) const { return target_ && now >= target_deadline_; }
+  bool acquire(const std::string& owner, double ttl, double now) {
+    if (owner.empty() || !std::isfinite(ttl) || ttl <= 0. || ttl > 60.) return false;
+    const auto it = leases_.find(owner);
+    if (it == leases_.end() || it->second <= now) invalidate();
+    leases_[owner] = now + ttl; settling_ = true; return true;
+  }
+  void release(const std::string& owner) { leases_.erase(owner); }
+  void normal(double deadline) { invalidate(); normal_deadline_ = deadline; settling_ = true; }
+  void action(bool active) {
+    if (active && !action_active_) { invalidate(); settling_ = true; }
+    action_active_ = active;
+  }
+  bool allowed(double steady, double sim, bool settled) {
+    for (auto it = leases_.begin(); it != leases_.end();) {
+      if (it->second <= steady) it = leases_.erase(it); else ++it;
+    }
+    if (!leases_.empty() || action_active_ || sim < normal_deadline_) return false;
+    if (settling_ && !settled) return false;
+    settling_ = false; return true;
+  }
+private:
+  std::uint64_t epoch_{0};
+  bool target_{false}, action_active_{false}, settling_{false};
+  double target_deadline_{0.}, normal_deadline_{0.};
+  std::map<std::string, double> leases_;
+};
+// A controller action owns the low slot from submission through its terminal
+// result. Releasing the mouse or requesting high-priority motion must not
+// release this slot before the already dispatched bounded step finishes.
+class LowGoalBarrier {
+public:
+  bool idle() const { return !pending_; }
+  bool begin() {
+    if (pending_) return false;
+    pending_ = true; return true;
+  }
+  void accepted(bool value) { if (!value) terminal(); }
+  void terminal() { pending_ = false; }
+private:
+  bool pending_{false};
+};
+}  // namespace iiwa_planning

src/iiwa_planning/package.xml

@@ -11,6 +11,8 @@
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>control_msgs</depend>
   <depend>rclpy</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_core</depend>
@@ -25,9 +27,19 @@
   <depend>tf_transformations</depend>
   <depend>python3-scipy</depend>
   <depend>iiwa_msgs</depend>
+  <depend>moveit_servo</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>action_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>tf2_eigen</depend>
 
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>iiwa_utils</test_depend>
+  <test_depend>controller_manager_msgs</test_depend>
+  <test_depend>python3-numpy</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/iiwa_planning/scripts/motion_sequence_runner.py

@@ -28,6 +28,12 @@ import rosbag2_py
 from rosidl_runtime_py.utilities import get_message
 
 from iiwa_msgs.action import MoveToJoints, MoveToPose
+from rcl_interfaces.srv import GetParameters
+from rcl_interfaces.msg import ParameterType
+from std_srvs.srv import Trigger
+from iiwa_planning.motion_priority import (
+    MotionLease, MotionPriorityClient, LeaseError, wait_response,
+)
 
 
 def _is_joints_waypoint(wp: dict) -> bool:
@@ -70,6 +76,15 @@ class MotionSequenceRunner(Node):
         )
         self.get_logger().info(f'joints_action={joints_action}  pose_action={pose_action}')
 
+        self._parameters_client = self.create_client(
+            GetParameters, '/iiwa_motion_server/get_parameters', callback_group=self._cb_group)
+        self._stop_client = self.create_client(
+            Trigger, '/cobot/stop', callback_group=self._cb_group)
+        self._priority = None
+        self._lease_active = None
+        self._aborted = threading.Event()
+        self._active_goal = None
+        self.exit_code = 1
         self._writer: rosbag2_py.SequentialWriter | None = None
         self._registered_topics: set[str] = set()
         self._subs = []
@@ -180,6 +195,9 @@ class MotionSequenceRunner(Node):
 
         def _on_goal(future):
             gh = future.result()
+            self._active_goal = gh
+            if self._aborted.is_set() and gh.accepted:
+                gh.cancel_goal_async()
             if not gh.accepted:
                 self.get_logger().error('MoveToJoints goal rejected')
                 done.set()
@@ -187,8 +205,15 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
-        done.wait()
-        return result_holder[0]
+        deadline = time.monotonic() + 300.0
+        while not done.wait(0.1):
+            if self._aborted.is_set() or time.monotonic() >= deadline or not rclpy.ok():
+                self._abort_sequence()
+                return False
+            if self._lease_active:
+                self._lease_active.check()
+        self._active_goal = None
+        return result_holder[0] and not self._aborted.is_set()
 
     def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool:
         goal = MoveToPose.Goal()
@@ -215,6 +240,9 @@ class MotionSequenceRunner(Node):
 
         def _on_goal(future):
             gh = future.result()
+            self._active_goal = gh
+            if self._aborted.is_set() and gh.accepted:
+                gh.cancel_goal_async()
             if not gh.accepted:
                 self.get_logger().error('MoveToPose goal rejected')
                 done.set()
@@ -222,8 +250,15 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
-        done.wait()
-        return result_holder[0]
+        deadline = time.monotonic() + 300.0
+        while not done.wait(0.1):
+            if self._aborted.is_set() or time.monotonic() >= deadline or not rclpy.ok():
+                self._abort_sequence()
+                return False
+            if self._lease_active:
+                self._lease_active.check()
+        self._active_goal = None
+        return result_holder[0] and not self._aborted.is_set()
 
     def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool:
         if _is_joints_waypoint(wp):
@@ -231,21 +266,49 @@ class MotionSequenceRunner(Node):
         return self._send_pose_goal(wp, idx=idx)
 
 
+    def _abort_sequence(self):
+        self._aborted.set()
+        if self._active_goal is not None and self._active_goal.accepted:
+            self._active_goal.cancel_goal_async()
+        # Asynchronous so renewal never waits on the motion server's callback pool.
+        if self._stop_client.service_is_ready():
+            self._stop_client.call_async(Trigger.Request())
+
+    def _discover_priority(self):
+        if not self._parameters_client.wait_for_service(timeout_sec=5.0):
+            raise LeaseError('Motion server parameter service unavailable')
+        response = wait_response(self._parameters_client.call_async(
+            GetParameters.Request(names=['tcp_gizmo_enabled'])))
+        if len(response.values) != 1 or response.values[0].type != ParameterType.PARAMETER_BOOL:
+            raise LeaseError('Motion server tcp_gizmo_enabled parameter unavailable')
+        if response.values[0].bool_value:
+            self._priority = MotionPriorityClient(context=self.context)
+
     def run(self, done_event: threading.Event):
+        self.exit_code = 1
         try:
+            self._discover_priority()
+            with MotionLease(self._priority.exchange if self._priority else None,
+                             on_lost=self._abort_sequence) as lease:
+                self._lease_active = lease
                 for i in range(self._n_iter):
                     self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
-
-                self._send_waypoint(self._home)
-
-                for idx, wp in enumerate(self._waypoints):
-                    self._send_waypoint(wp, idx=idx)
-
-                self._send_waypoint(self._home)
-                time.sleep(self._delay)
-
+                    for idx, wp in enumerate([self._home, *self._waypoints, self._home]):
+                        lease.check()
+                        if self._aborted.is_set() or not self._send_waypoint(wp, idx=idx):
+                            raise LeaseError(f'Sequence interrupted at waypoint {idx}')
+                    if self._aborted.wait(max(0.0, self._delay)):
+                        raise LeaseError('Sequence reservation lost during pause')
+                    lease.check()
             self.get_logger().info('======= Sequence complete =======')
+            self.exit_code = 0
+        except Exception as exc:
+            self._abort_sequence()
+            self.get_logger().error(f'Sequence failed: {exc}')
         finally:
+            self._lease_active = None
+            if self._priority:
+                self._priority.close()
             self.close_bag()
             done_event.set()
 
@@ -264,12 +327,16 @@ def main():
         while not done_event.is_set():
             executor.spin_once(timeout_sec=0.1)
     except KeyboardInterrupt:
-        pass
+        node._abort_sequence()
+        deadline = time.monotonic() + 5.0
+        while not done_event.is_set() and time.monotonic() < deadline:
+            executor.spin_once(timeout_sec=0.1)
     finally:
         node.close_bag()
         node.destroy_node()
         rclpy.shutdown()
+    return node.exit_code
 
 
 if __name__ == '__main__':
-    main()
+    raise SystemExit(main())

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -17,6 +17,9 @@ from moveit.core.robot_state import RobotState
 
 from iiwa_msgs.action import MoveToPose, MoveToJoints
 from iiwa_msgs.srv import MoveToNamedPose
+from iiwa_planning.motion_priority import (
+    MotionLease, MotionPriorityClient, MotionSerialiser, LeaseError,
+)
 
 
 # Таблица планировщиков
@@ -48,6 +51,10 @@ class IiwaMotionServer(Node):
         super().__init__("iiwa_motion_server")
         self._setup_parameters()
         self._setup_moveit()
+        self._motion_serial = MotionSerialiser()
+        self._operation = threading.local()
+        self._priority = MotionPriorityClient(context=self.context)
+        self._gizmo_enabled = self.get_parameter("tcp_gizmo_enabled").value
         self._setup_servers()
 
         self.get_logger().info("Сервер движения iiwa запущен")
@@ -59,6 +66,7 @@ class IiwaMotionServer(Node):
         self.declare_parameter("default_frame", "base_link")
         self.declare_parameter("default_planner", "ompl")
         self.declare_parameter("planning_attempts", 3)
+        self.declare_parameter("tcp_gizmo_enabled", False)
 
         self._pose_link = self.get_parameter("pose_link").value
         self._planning_group = self.get_parameter("planning_group").value
@@ -67,7 +75,7 @@ class IiwaMotionServer(Node):
         self._planning_attempts = self.get_parameter("planning_attempts").value
 
     def _setup_moveit(self):
-        self._moveit = MoveItPy(node_name="iiwa_motion_server")
+        self._moveit = MoveItPy(node_name="iiwa_motion_moveit")
         self._arm: PlanningComponent = self._moveit.get_planning_component(self._planning_group)
         self._robot_model = self._moveit.get_robot_model()
 
@@ -75,19 +83,67 @@ class IiwaMotionServer(Node):
         cb = ReentrantCallbackGroup()
 
         ActionServer(
-            self, MoveToPose, "cobot/move_to_pose", self._execute_pose,
+            self, MoveToPose, "cobot/move_to_pose", lambda gh: self._guard_action(gh, MoveToPose.Result, self._execute_pose),
             callback_group=cb,
             goal_callback=lambda _: GoalResponse.ACCEPT,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
         ActionServer(
-            self, MoveToJoints, "cobot/move_to_joints", self._execute_joints,
+            self, MoveToJoints, "cobot/move_to_joints", lambda gh: self._guard_action(gh, MoveToJoints.Result, self._execute_joints),
             callback_group=cb,
             goal_callback=lambda _: GoalResponse.ACCEPT,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
-        self.create_service(MoveToNamedPose, "cobot/move_to_named", self._handle_named, callback_group=cb)
-        self.create_service(Trigger, "cobot/stop", self._handle_stop, callback_group=cb)
+        self.create_service(MoveToNamedPose, "cobot/move_to_named", self._guard_named, callback_group=cb)
+        self._priority.create_stop_service(self._handle_stop)
+
+    def _stop_execution(self):
+        self._motion_serial.stop(
+            lambda: self._moveit.get_trajectory_execution_manager().stop_execution())
+
+    def _lease(self):
+        return MotionLease(self._priority.exchange if self._gizmo_enabled else None,
+                           on_lost=self._stop_execution)
+
+    def _guarded(self, callback):
+        generation = self._motion_serial.generation()
+        # Reserve before waiting so queued requests also exclude the gizmo.
+        with self._lease() as lease:
+            with self._motion_serial.lock:
+                self._motion_serial.check(generation)
+                lease.check()
+                self._operation.generation = generation
+                self._operation.lease = lease
+                return callback()
+
+    def _guard_action(self, goal_handle, result_type, callback):
+        try:
+            result, ok, message = self._guarded(lambda: callback(goal_handle))
+            return self._finish_action(goal_handle, result, ok, message)
+        except Exception as exc:
+            result = result_type()
+            result.success = False
+            result.message = str(exc)
+            if goal_handle.is_active:
+                goal_handle.abort()
+            return result
+
+    def _guard_named(self, request, response):
+        try:
+            return self._guarded(lambda: self._handle_named(request, response))
+        except Exception as exc:
+            response.success = False
+            response.message = str(exc)
+            return response
+
+    def _check_operation(self):
+        self._operation.lease.check()
+        self._motion_serial.check(self._operation.generation)
+
+    def destroy_node(self):
+        if self._priority:
+            self._priority.close()
+        return super().destroy_node()
 
     def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float, accel_scale: float | None = None) -> PlanRequestParameters:
         params = PlanRequestParameters(self._moveit, self._planning_group)
@@ -105,19 +161,29 @@ class IiwaMotionServer(Node):
         Возвращает (True, msg) при успехе, (False, msg) при ошибке,
         (None, 'canceled') если цель была отменена.
         """
+        self._check_operation()
         plan_result = self._arm.plan(single_plan_parameters=plan_params)
         if not plan_result:
             return False, "Планирование не удалось: поза недостижима или в столкновении"
 
+        self._check_operation()
         if goal_handle.is_cancel_requested:
             return None, "canceled"
 
         done = threading.Event()
         failed = threading.Event()
 
+        generation = self._operation.generation
+        lease = self._operation.lease
+
         def do_execute():
             try:
-                self._moveit.execute(plan_result.trajectory, controllers=[])
+                lease.check()
+                self._motion_serial.check(generation)
+                execution = self._moveit.execute(plan_result.trajectory, controllers=[])
+                # Jazzy ExecutionStatus.__bool__ is true only for SUCCEEDED.
+                if not bool(execution):
+                    failed.set()
             except Exception as exc:
                 self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
                 failed.set()
@@ -126,14 +192,21 @@ class IiwaMotionServer(Node):
 
         threading.Thread(target=do_execute, daemon=True).start()
 
+        interrupted = False
         while not done.wait(timeout=0.05):
-            if goal_handle.is_cancel_requested:
+            if (goal_handle.is_cancel_requested or generation != self._motion_serial.generation()
+                    or lease.lost):
+                interrupted = True
+                # Repeat to cover stop racing the blocking execute() submission.
                 try:
                     self._moveit.get_trajectory_execution_manager().stop_execution()
-                except Exception:
-                    pass
-                done.wait()
+                except Exception as exc:
+                    self.get_logger().error(f"Stop retry failed: {exc}")
+        if goal_handle.is_cancel_requested:
             return None, "canceled"
+        self._check_operation()
+        if interrupted:
+            return False, "Выполнение траектории остановлено"
 
         if failed.is_set():
             return False, "Выполнение траектории завершилось ошибкой"
@@ -165,8 +238,7 @@ class IiwaMotionServer(Node):
             result.success = False
             result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
             self.get_logger().error(result.message)
-            goal_handle.abort()
-            return result
+            return result, False, result.message
 
         pipeline, planner_id, plan_time = PLANNERS[planner_key]
 
@@ -198,7 +270,7 @@ class IiwaMotionServer(Node):
 
         plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
         ok, msg = self._plan_and_execute(plan_params, goal_handle)
-        return self._finish_action(goal_handle, result, ok, msg)
+        return result, ok, msg
 
     def _execute_joints(self, goal_handle):
         req = goal_handle.request
@@ -230,7 +302,7 @@ class IiwaMotionServer(Node):
             "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
         )
         ok, msg = self._plan_and_execute(plan_params, goal_handle)
-        return self._finish_action(goal_handle, result, ok, msg)
+        return result, ok, msg
 
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
         name = request.name.strip()
@@ -254,30 +326,26 @@ class IiwaMotionServer(Node):
         plan_params = self._make_plan_params(
             "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
         )
-        plan_result = self._arm.plan(single_plan_parameters=plan_params)
-        if not plan_result:
-            response.success = False
-            response.message = f"Не удалось построить траекторию для '{name}'"
-            self.get_logger().error(response.message)
-            return response
+        class NamedGoal:
+            is_cancel_requested = False
 
-        exec_result = self._moveit.execute(plan_result.trajectory, controllers=[])
-        if exec_result:
-            response.success = True
-            response.message = f"Переместился в '{name}'"
-            self.get_logger().info(response.message)
-        else:
-            response.success = False
-            response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)"
-            self.get_logger().error(response.message)
+        ok, message = self._plan_and_execute(plan_params, NamedGoal())
+        response.success = bool(ok)
+        response.message = message
         return response
 
     def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
         try:
-            self._moveit.get_trajectory_execution_manager().stop_execution()
+            with self._lease():
+                self._stop_execution()
             response.success = True
             response.message = "Выполнение траектории остановлено"
         except Exception as exc:
+            # Failure to reserve must never prevent the requested high-priority stop.
+            try:
+                self._stop_execution()
+            except Exception as stop_exc:
+                self.get_logger().error(f"Stop failed: {stop_exc}")
             response.success = False
             response.message = str(exc)
         self.get_logger().info(f"[stop] {response.message}")
@@ -294,6 +362,9 @@ def main(args=None):
     except (KeyboardInterrupt, ExternalShutdownException):
         pass
     finally:
+        if "node" in locals():
+            node.destroy_node()
+        if rclpy.ok():
             rclpy.shutdown()
 
 

src/iiwa_planning/src/tcp_gizmo.cpp

@@ -0,0 +1,471 @@
+#include <iiwa_planning/tcp_gizmo_policy.hpp>
+#include <iiwa_msgs/msg/gizmo_command.hpp>
+#include <iiwa_msgs/msg/gizmo_state.hpp>
+#include <iiwa_msgs/srv/motion_priority.hpp>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <moveit_servo/servo.hpp>
+#include <moveit_servo/utils/common.hpp>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <Eigen/Geometry>
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <deque>
+#include <functional>
+#include <map>
+#include <stdexcept>
+
+namespace iiwa_planning {
+class TcpGizmo {
+  using Trajectory = trajectory_msgs::msg::JointTrajectory;
+  using Priority = iiwa_msgs::srv::MotionPriority;
+  using Follow = control_msgs::action::FollowJointTrajectory;
+  using ClientGoal = rclcpp_action::ClientGoalHandle<Follow>;
+  using ServerGoal = rclcpp_action::ServerGoalHandle<Follow>;
+  struct ProxyGoal {
+    std::shared_ptr<ServerGoal> upstream;
+    ClientGoal::SharedPtr downstream;
+    bool cancel_requested{false};
+    bool cancel_sent{false};
+    bool finished{false};
+  };
+public:
+  explicit TcpGizmo(const rclcpp::Node::SharedPtr& node) : node_(node) {
+    if (!param<bool>("enabled", false) || !param<bool>("simulation", false))
+      throw std::runtime_error("TCP gizmo requires enabled=true and simulation=true");
+    group_name_ = param<std::string>("planning_group", "iiwa_arm");
+    tcp_ = param<std::string>("pose_link", "tcp");
+    frame_ = param<std::string>("default_frame", "base_link");
+    period_ = positive("publish_period", .032);
+    command_timeout_ = positive("command_timeout", .3);
+    state_timeout_ = positive("state_timeout", .5);
+    linear_speed_ = positive("max_linear_speed", .10);
+    angular_speed_ = positive("max_angular_speed", .4);
+    listener_ = std::make_shared<servo::ParamListener>(node_, "moveit_servo");
+    const auto params = listener_->get_params();
+    if (params.move_group_name != group_name_ || std::abs(params.publish_period - period_) > 1e-9)
+      throw std::runtime_error("Servo group/period must match TCP gizmo parameters");
+    scene_ = moveit_servo::createPlanningSceneMonitor(node_, params);
+    if (!scene_ || !scene_->getRobotModel()) throw std::runtime_error("Missing planning scene");
+    group_ = scene_->getRobotModel()->getJointModelGroup(group_name_);
+    if (!group_ || !group_->getSolverInstance()) throw std::runtime_error("Missing IK solver");
+    tip_ = group_->getSolverInstance()->getTipFrame();
+    const auto model = scene_->getRobotModel();
+    if (!model->hasLinkModel(tcp_) || !model->hasLinkModel(frame_) || !model->hasLinkModel(tip_))
+      throw std::runtime_error("Unknown TCP, frame or solver tip");
+    if (moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(model->getLinkModel(tcp_)) !=
+        moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(model->getLinkModel(tip_)))
+      throw std::runtime_error("TCP must be rigidly attached to the IK solver tip");
+    output_ = node_->create_publisher<Trajectory>("/cobot/tcp_gizmo/controller_command", 10);
+    state_pub_ = node_->create_publisher<iiwa_msgs::msg::GizmoState>("/cobot/tcp_gizmo/state", 1);
+    command_sub_ = node_->create_subscription<iiwa_msgs::msg::GizmoCommand>(
+      "/cobot/tcp_gizmo/command", 1, [this](const iiwa_msgs::msg::GizmoCommand& msg) {
+        try { command(msg); }
+        catch (const std::exception& e) { reject(std::string("target error: ") + e.what()); }
+      });
+    action_client_ = rclcpp_action::create_client<Follow>(node_, "/cobot/tcp_gizmo/follow_joint_trajectory");
+    action_server_ = rclcpp_action::create_server<Follow>(node_, "/iiwa_arm_controller/follow_joint_trajectory",
+      [this](const rclcpp_action::GoalUUID&, std::shared_ptr<const Follow::Goal>) {
+        if (!action_client_->action_server_is_ready()) return rclcpp_action::GoalResponse::REJECT;
+        ++high_goals_count_;
+        gate_.action(true);  // Block low before accepting a public high goal.
+        reason_ = "trajectory action";
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+      },
+      [this](const std::shared_ptr<ServerGoal> goal) {
+        const auto it = high_goals_.find(goal->get_goal_id());
+        if (it != high_goals_.end()) {
+          it->second->cancel_requested = true;
+          cancelHigh(it->second);
+        }
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [this](const std::shared_ptr<ServerGoal> goal) {
+        auto proxy = std::make_shared<ProxyGoal>();
+        proxy->upstream = goal;
+        high_goals_[goal->get_goal_id()] = proxy;
+        after_low_.push_back([this, proxy]() { sendHigh(proxy); });
+        flushHigh();
+      });
+    normal_sub_ = node_->create_subscription<Trajectory>("/iiwa_arm_controller/joint_trajectory", 10,
+      [this](const Trajectory& msg) {
+        // The low action must have a terminal controller result before ANY high
+        // output. DDS publication/cancel acknowledgements alone are insufficient.
+        gate_.normal(trajectoryDeadline(msg));
+        reason_ = "ordinary trajectory";
+        after_low_.push_back([this, msg]() {
+          gate_.normal(trajectoryDeadline(msg));
+          output_->publish(msg);
+        });
+        flushHigh();
+      });
+    priority_ = node_->create_service<Priority>("/cobot/tcp_gizmo/priority",
+      [this](const std::shared_ptr<rmw_request_id_t> header, const Priority::Request::SharedPtr request) {
+        const auto token = ++lease_requests_[request->owner];
+        if (!request->acquire) {
+          gate_.release(request->owner);
+          replyPriority(header, true, "priority released");
+          return;
+        }
+        if (!gate_.acquire(request->owner, request->ttl, steady())) {
+          replyPriority(header, false, "owner and finite TTL in (0, 60] required");
+          return;
+        }
+        reason_ = "waiting for low action to stop";
+        after_low_.push_back([this, header, request, token]() {
+          if (lease_requests_[request->owner] != token) {
+            replyPriority(header, false, "priority request superseded"); return;
+          }
+          // TTL starts at acknowledgement, even if controller completion was
+          // delayed. The pending callback itself prevents any new low command.
+          gate_.acquire(request->owner, request->ttl, steady());
+          replyPriority(header, true, "controller low action stopped; priority granted");
+          reason_ = "motion priority lease";
+        });
+        flushHigh();
+      });
+    timer_ = node_->create_wall_timer(std::chrono::duration<double>(period_), [this]() { tick(); });
+  }
+
+private:
+  template<class T> T param(const std::string& name, const T& value) {
+    if (!node_->has_parameter(name)) return node_->declare_parameter<T>(name, value);
+    return node_->get_parameter(name).get_value<T>();
+  }
+  double positive(const std::string& name, double value) {
+    value = param<double>(name, value);
+    if (!std::isfinite(value) || value <= 0.) throw std::runtime_error("Invalid " + name);
+    return value;
+  }
+  static double steady() {
+    return std::chrono::duration<double>(std::chrono::steady_clock::now().time_since_epoch()).count();
+  }
+  moveit::core::RobotStatePtr current() {
+    auto monitor = scene_->getStateMonitor();
+    if (!monitor || !monitor->haveCompleteState(rclcpp::Duration::from_seconds(state_timeout_))) return {};
+    auto pair = monitor->getCurrentStateAndTime();
+    const double age = (node_->now() - pair.second).seconds();
+    if (age < -.01 || age > state_timeout_) return {};
+    const auto stamp = pair.second.nanoseconds();
+    if (stamp != feedback_stamp_) { feedback_stamp_ = stamp; feedback_received_ = steady(); }
+    if (steady() - feedback_received_ > state_timeout_) return {};
+    pair.first->update();
+    std::vector<double> positions;
+    pair.first->copyJointGroupPositions(group_, positions);
+    if (!std::all_of(positions.begin(), positions.end(), [](double x) { return std::isfinite(x); })) return {};
+    return pair.first;
+  }
+  bool settled(const moveit::core::RobotState& state) {
+    std::vector<double> positions;
+    state.copyJointGroupPositions(group_, positions);
+    const double now = node_->now().seconds();
+    if (previous_.size() != positions.size() || now < previous_time_) {
+      previous_ = positions; previous_time_ = now; stable_since_ = now; return false;
+    }
+    if (now > previous_time_) {
+      double delta = 0.;
+      for (std::size_t i = 0; i < positions.size(); ++i) delta = std::max(delta, std::abs(positions[i] - previous_[i]));
+      if (delta / (now - previous_time_) > .01) stable_since_ = now;
+      previous_ = positions; previous_time_ = now;
+    }
+    return now - stable_since_ >= .15;
+  }
+  double trajectoryDeadline(const Trajectory& msg) const {
+    double start = node_->now().seconds();
+    if (rclcpp::Time(msg.header.stamp).nanoseconds() != 0)
+      start = std::max(start, rclcpp::Time(msg.header.stamp).seconds());
+    double duration = 0.;
+    for (const auto& point : msg.points)
+      duration = std::max(duration, rclcpp::Duration(point.time_from_start).seconds());
+    return start + duration;
+  }
+  void replyPriority(const std::shared_ptr<rmw_request_id_t>& header, bool success, const std::string& message) {
+    Priority::Response response;
+    response.success = success; response.message = message;
+    priority_->send_response(*header, response);
+  }
+  void flushHigh() {
+    if (!low_barrier_.idle()) return;
+    while (!after_low_.empty()) {
+      auto callback = std::move(after_low_.front());
+      after_low_.pop_front();
+      callback();
+    }
+  }
+  void sendLow(const Trajectory& trajectory) {
+    if (!action_client_->action_server_is_ready()) { reject("controller action unavailable"); return; }
+    if (!low_barrier_.begin()) return;
+    Follow::Goal goal;
+    goal.trajectory = trajectory;
+    // One released marker pose is one complete trajectory. The user chose
+    // to finish this trajectory before dispatching a queued ordinary command.
+    // No low CancelGoal request is needed (including the JTC cancel/result race).
+    goal.goal_time_tolerance = rclcpp::Duration::from_seconds(state_timeout_);
+    rclcpp_action::Client<Follow>::SendGoalOptions options;
+    options.goal_response_callback = [this](const ClientGoal::SharedPtr handle) {
+      low_goal_ = handle;
+      low_barrier_.accepted(static_cast<bool>(handle));
+      if (!handle) {
+        gate_.invalidate(); reason_ = "controller rejected low trajectory"; flushHigh();
+      }
+    };
+    options.result_callback = [this](const ClientGoal::WrappedResult& result) {
+      low_goal_.reset(); low_barrier_.terminal();
+      // A new epoch acknowledges completion to the marker and permits a new
+      // gesture. Never redispatch the same goal after a successful result.
+      gate_.invalidate();
+      reason_ = result.code == rclcpp_action::ResultCode::SUCCEEDED ?
+        "target reached" : "controller did not complete TCP trajectory";
+      flushHigh();
+    };
+    try { action_client_->async_send_goal(goal, options); }
+    catch (const std::exception& error) {
+      // An exception leaves delivery uncertain. Fail closed: do not forward a
+      // high command without a terminal controller result for this send.
+      gate_.invalidate(); reason_ = std::string("low action send uncertain: ") + error.what();
+    }
+  }
+  void cancelHigh(const std::shared_ptr<ProxyGoal>& proxy) {
+    if (proxy->downstream && proxy->cancel_requested && !proxy->cancel_sent && !proxy->finished) {
+      proxy->cancel_sent = true;
+      action_client_->async_cancel_goal(proxy->downstream);
+    }
+  }
+  void finishHigh(const std::shared_ptr<ProxyGoal>& proxy, rclcpp_action::ResultCode code,
+                  const std::shared_ptr<Follow::Result>& result) {
+    if (proxy->finished) return;
+    proxy->finished = true;
+    if (proxy->upstream->is_canceling()) proxy->upstream->canceled(result);
+    else if (code == rclcpp_action::ResultCode::SUCCEEDED) proxy->upstream->succeed(result);
+    else proxy->upstream->abort(result);
+    high_goals_.erase(proxy->upstream->get_goal_id());
+    if (high_goals_count_ > 0) --high_goals_count_;
+    gate_.action(high_goals_count_ > 0);
+  }
+  void sendHigh(const std::shared_ptr<ProxyGoal>& proxy) {
+    if (proxy->cancel_requested || proxy->upstream->is_canceling()) {
+      auto result = std::make_shared<Follow::Result>();
+      result->error_string = "canceled before controller dispatch";
+      finishHigh(proxy, rclcpp_action::ResultCode::CANCELED, result); return;
+    }
+    if (!action_client_->action_server_is_ready()) {
+      auto result = std::make_shared<Follow::Result>();
+      result->error_code = Follow::Result::INVALID_GOAL;
+      result->error_string = "controller action unavailable";
+      finishHigh(proxy, rclcpp_action::ResultCode::ABORTED, result); return;
+    }
+    rclcpp_action::Client<Follow>::SendGoalOptions options;
+    options.goal_response_callback = [this, proxy](const ClientGoal::SharedPtr handle) {
+      proxy->downstream = handle;
+      if (!handle) {
+        auto result = std::make_shared<Follow::Result>();
+        result->error_code = Follow::Result::INVALID_GOAL;
+        result->error_string = "controller rejected trajectory";
+        finishHigh(proxy, rclcpp_action::ResultCode::ABORTED, result);
+      } else cancelHigh(proxy);
+    };
+    options.feedback_callback = [proxy](ClientGoal::SharedPtr, const std::shared_ptr<const Follow::Feedback> feedback) {
+      if (!proxy->finished && proxy->upstream->is_active()) proxy->upstream->publish_feedback(std::make_shared<Follow::Feedback>(*feedback));
+    };
+    options.result_callback = [this, proxy](const ClientGoal::WrappedResult& result) {
+      finishHigh(proxy, result.code, result.result);
+    };
+    try { action_client_->async_send_goal(*proxy->upstream->get_goal(), options); }
+    catch (const std::exception& error) {
+      // Keep its high reservation on ambiguous delivery; exposing low motion
+      // here could overwrite a goal that the controller actually received.
+      reason_ = std::string("high action send uncertain: ") + error.what();
+    }
+  }
+  void reject(const std::string& reason) {
+    RCLCPP_WARN(node_->get_logger(), "TCP gizmo rejected target: %s", reason.c_str());
+    gate_.invalidate(); reason_ = reason;
+  }
+  bool safe(moveit::core::RobotState& state, const planning_scene::PlanningSceneConstPtr& scene) {
+    state.update();
+    return state.satisfiesBounds(group_) && !scene->isStateColliding(state, group_name_);
+  }
+  void command(const iiwa_msgs::msg::GizmoCommand& msg) {
+    auto state = current();
+    if (!state) { reject("joint state unavailable or stale"); return; }
+    if (!after_low_.empty() || !gate_.allowed(steady(), node_->now().seconds(), settled(*state))) return;
+    if (msg.epoch != gate_.epoch()) return;
+    if (!low_barrier_.idle()) return;
+    const auto& p = msg.target.pose;
+    const double age = (node_->now() - rclcpp::Time(msg.target.header.stamp)).seconds();
+    Eigen::Quaterniond q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
+    Eigen::Vector3d t(p.position.x, p.position.y, p.position.z);
+    if (msg.target.header.frame_id != frame_ || !q.coeffs().allFinite() || !t.allFinite() ||
+        std::abs(q.norm() - 1.) > .01 || age < -.01 || age > command_timeout_) {
+      reject("invalid or stale target"); return;
+    }
+    Eigen::Isometry3d requested = Eigen::Isometry3d::Identity();
+    requested.linear() = q.normalized().toRotationMatrix(); requested.translation() = t;
+    const auto actual = state->getGlobalLinkTransform(frame_).inverse() * state->getGlobalLinkTransform(tcp_);
+    if (!requiresGizmoMotion((requested.translation() - actual.translation()).norm(),
+                            Eigen::Quaterniond(requested.linear()).angularDistance(Eigen::Quaterniond(actual.linear())))) {
+      gate_.invalidate();
+      reason_ = "target reached"; return;
+    }
+    const auto model_target = state->getGlobalLinkTransform(frame_) * requested;
+    // Solve once on release. setFromIK handles the tool's fixed TCP offset.
+    moveit::core::RobotState candidate(*state);
+    planning_scene_monitor::LockedPlanningSceneRO scene(scene_);
+    const bool valid = candidate.setFromIK(group_, model_target, tcp_, .01,
+      [this, &scene](moveit::core::RobotState* s, const moveit::core::JointModelGroup* group, const double* values) {
+        s->setJointGroupPositions(group, values); return safe(*s, scene);
+      });
+    if (!valid || !safe(candidate, scene)) { reject("target unreachable, outside limits or colliding"); return; }
+    // Do not accept approximate IK solutions as reachable targets.
+    const auto reached = candidate.getGlobalLinkTransform(tcp_);
+    if ((reached.translation() - model_target.translation()).norm() > .002 ||
+        Eigen::AngleAxisd(reached.linear().transpose() * model_target.linear()).angle() > .01) {
+      reject("IK solution does not reach target"); return;
+    }
+    candidate.copyJointGroupPositions(group_, ik_target_);
+    gate_.accept(msg.epoch, steady(), command_timeout_);
+    reason_ = "tracking";
+  }
+  void tick() {
+    flushHigh();
+    auto state = current();
+    if (!state) {
+      if (gate_.hasTarget()) reject("joint state unavailable or stale");
+      publishState({}, false, "joint state unavailable or stale"); return;
+    }
+    const bool available = after_low_.empty() && gate_.allowed(steady(), node_->now().seconds(), settled(*state));
+    if (!available) {
+      publishState(state, false, reason_); return;
+    }
+    // command_timeout validates the age at receipt. A released goal is a
+    // committed action and needs no stream of keepalive mouse commands.
+    if (gate_.hasTarget() && low_barrier_.idle()) {
+      try { step(state); }
+      catch (const std::exception& e) { reject(std::string("TCP trajectory error: ") + e.what()); }
+    }
+    publishState(state, true, reason_);
+  }
+  void step(const moveit::core::RobotStatePtr& state) {
+    const auto& names = group_->getVariableNames();
+    if (ik_target_.size() != names.size()) { reject("missing IK target"); return; }
+    std::vector<double> actual;
+    state->copyJointGroupPositions(group_, actual);
+    Eigen::VectorXd delta(names.size());
+    double duration = 2. * period_;
+    double max_travel = 0.;
+    for (std::size_t i = 0; i < names.size(); ++i) {
+      delta[i] = ik_target_[i] - actual[i];
+      const double travel = std::abs(delta[i]);
+      max_travel = std::max(max_travel, travel);
+      const auto& limit = state->getRobotModel()->getVariableBounds(names[i]);
+      // q(u) = q0 + (3u^2 - 2u^3) * delta, with zero endpoint velocities.
+      if (limit.velocity_bounded_)
+        duration = std::max(duration, 1.5 * travel / limit.max_velocity_);
+      if (limit.acceleration_bounded_)
+        duration = std::max(duration, std::sqrt(6. * travel / limit.max_acceleration_));
+    }
+    if (!delta.allFinite() || !std::isfinite(duration)) { reject("invalid trajectory limits"); return; }
+
+    // Validate the complete path, including the actual tool tip. Use forward
+    // Jacobians only to measure Cartesian speed; no inverse/singularity gate.
+    // Densely sample the cubic path and increase duration to obey BOTH TCP
+    // translation and rotation limits throughout it, not just at its endpoints.
+    const Eigen::Vector3d tcp_offset = (state->getGlobalLinkTransform(tip_).inverse() *
+                                        state->getGlobalLinkTransform(tcp_)).translation();
+    const auto* tip_link = state->getRobotModel()->getLinkModel(tip_);
+    const int samples = std::max(100, static_cast<int>(std::ceil(max_travel / .01)));
+    planning_scene_monitor::LockedPlanningSceneRO scene(scene_);
+    moveit::core::RobotState sample(*state);
+    Eigen::MatrixXd jacobian;
+    std::vector<double> positions(actual.size());
+    for (int k = 0; k <= samples; ++k) {
+      const double u = static_cast<double>(k) / samples;
+      const double blend = u * u * (3. - 2. * u);
+      for (std::size_t i = 0; i < positions.size(); ++i) positions[i] = actual[i] + blend * delta[i];
+      sample.setJointGroupPositions(group_, positions);
+      if (!safe(sample, scene)) { reject("TCP trajectory is outside limits or colliding"); return; }
+      if (!sample.getJacobian(group_, tip_link, tcp_offset, jacobian)) {
+        reject("cannot compute tool TCP speed"); return;
+      }
+      const Eigen::VectorXd twist = jacobian * delta * (6. * u * (1. - u));
+      duration = std::max(duration, 1.01 * twist.head<3>().norm() / linear_speed_);
+      duration = std::max(duration, 1.01 * twist.tail<3>().norm() / angular_speed_);
+    }
+    if (!std::isfinite(duration)) { reject("invalid TCP trajectory duration"); return; }
+    Trajectory message;
+    message.joint_names = names;
+    trajectory_msgs::msg::JointTrajectoryPoint start, end;
+    start.positions = actual;
+    start.velocities.assign(actual.size(), 0.);
+    start.time_from_start = rclcpp::Duration::from_seconds(.001);
+    end.positions = ik_target_;
+    end.velocities.assign(actual.size(), 0.);
+    end.time_from_start = rclcpp::Duration::from_seconds(duration + .001);
+    message.points = {start, end};
+    reason_ = "executing released TCP target";
+    sendLow(message);
+  }
+  void publishState(const moveit::core::RobotStatePtr& state, bool enabled, const std::string& reason) {
+    iiwa_msgs::msg::GizmoState message;
+    message.actual.header.frame_id = (state || have_actual_) ? frame_ : "";
+    message.actual.header.stamp = node_->now();
+    message.actual.pose = last_actual_;
+    if (state) {
+      const auto pose = state->getGlobalLinkTransform(frame_).inverse() * state->getGlobalLinkTransform(tcp_);
+      message.actual.pose.position.x = pose.translation().x(); message.actual.pose.position.y = pose.translation().y();
+      message.actual.pose.position.z = pose.translation().z();
+      const Eigen::Quaterniond q(pose.linear());
+      message.actual.pose.orientation.x = q.x(); message.actual.pose.orientation.y = q.y();
+      message.actual.pose.orientation.z = q.z(); message.actual.pose.orientation.w = q.w();
+      last_actual_ = message.actual.pose; have_actual_ = true;
+    }
+    message.enabled = enabled; message.epoch = gate_.epoch(); message.reason = reason;
+    state_pub_->publish(message);
+  }
+  rclcpp::Node::SharedPtr node_;
+  planning_scene_monitor::PlanningSceneMonitorPtr scene_;
+  std::shared_ptr<servo::ParamListener> listener_;
+  const moveit::core::JointModelGroup* group_{};
+  std::string group_name_, tcp_, tip_, frame_, reason_{"ready"};
+  double period_{}, command_timeout_{}, state_timeout_{}, linear_speed_{}, angular_speed_{};
+  GizmoPolicy gate_;
+  std::vector<double> ik_target_;
+  bool have_actual_{false};
+  LowGoalBarrier low_barrier_;
+  ClientGoal::SharedPtr low_goal_;
+  std::deque<std::function<void()>> after_low_;
+  std::map<std::string, std::uint64_t> lease_requests_;
+  std::map<rclcpp_action::GoalUUID, std::shared_ptr<ProxyGoal>> high_goals_;
+  std::size_t high_goals_count_{0};
+  rclcpp_action::Client<Follow>::SharedPtr action_client_;
+  rclcpp_action::Server<Follow>::SharedPtr action_server_;
+  std::vector<double> previous_;
+  double previous_time_{0.}, stable_since_{0.}, feedback_received_{0.};
+  std::int64_t feedback_stamp_{-1};
+  geometry_msgs::msg::Pose last_actual_{};
+  rclcpp::Publisher<Trajectory>::SharedPtr output_;
+  rclcpp::Publisher<iiwa_msgs::msg::GizmoState>::SharedPtr state_pub_;
+  rclcpp::Subscription<iiwa_msgs::msg::GizmoCommand>::SharedPtr command_sub_;
+  rclcpp::Subscription<Trajectory>::SharedPtr normal_sub_;
+  rclcpp::Service<Priority>::SharedPtr priority_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace iiwa_planning
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>("tcp_gizmo", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
+  try {
+    iiwa_planning::TcpGizmo gizmo(node);
+    // All motion, priority, target and timer callbacks are serialized.
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node); executor.spin();
+  } catch (const std::exception& error) {
+    RCLCPP_FATAL(node->get_logger(), "%s", error.what()); rclcpp::shutdown(); return 1;
+  }
+  rclcpp::shutdown(); return 0;
+}

src/iiwa_planning/test/gizmo_webots_regression.py

@@ -0,0 +1,232 @@
+"""Manual regression against Webots: release commits exactly one TCP action.
+
+Exercises the marker model with real ROS/controller feedback. Physical mouse
+input is covered separately by the Webots plugin adapter tests.
+"""
+import copy
+import threading
+import time
+import xml.etree.ElementTree as ET
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.parameter_client import AsyncParameterClient
+from rclpy.qos import QoSProfile, DurabilityPolicy
+from action_msgs.msg import GoalStatusArray
+from control_msgs.action import FollowJointTrajectory
+from controller_manager_msgs.srv import ListControllers
+from trajectory_msgs.msg import JointTrajectoryPoint
+from iiwa_msgs.msg import GizmoCommand, GizmoState
+from iiwa_utils.tcp_gizmo import pose_matrix
+from iiwa_utils.tcp_gizmo_model import DragTarget
+from iiwa_planning.motion_priority import MotionPriorityClient
+
+
+def main():
+    rclpy.init()
+    node = rclpy.create_node('gizmo_webots_regression')
+    state = {}
+    drag = DragTarget()
+
+    def capture(msg):
+        state['gizmo'] = msg
+        if msg.actual.header.frame_id:
+            drag.update_feedback(pose_matrix(msg.actual.pose), msg.enabled, msg.epoch)
+
+    node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', capture, 10)
+    node.create_subscription(
+        GoalStatusArray, '/cobot/tcp_gizmo/follow_joint_trajectory/_action/status',
+        lambda msg: state.update(status=msg),
+        QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
+    publisher = node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
+
+    def pump(duration):
+        end = time.monotonic() + duration
+        while time.monotonic() < end:
+            rclpy.spin_once(node, timeout_sec=.01)
+
+    def wait(predicate, timeout=30.):
+        end = time.monotonic() + timeout
+        while not predicate():
+            assert time.monotonic() < end, f'Timeout: {state.get("gizmo")}'
+            rclpy.spin_once(node, timeout_sec=.01)
+
+    def ids():
+        return {bytes(g.goal_info.goal_id.uuid) for g in state['status'].status_list} if 'status' in state else set()
+
+    def error(goal):
+        diff = np.linalg.inv(pose_matrix(goal.pose)) @ pose_matrix(state['gizmo'].actual.pose)
+        return np.linalg.norm(diff[:3, 3]), Rotation.from_matrix(diff[:3, :3]).magnitude()
+
+    def release(goal):
+        initial = copy.deepcopy(state['gizmo'].actual)
+        epoch = state['gizmo'].epoch
+        before = ids()
+        drag.step(pose_matrix(initial.pose), False)
+        for _ in range(8):
+            shown, command = drag.step(pose_matrix(goal.pose), True)
+            assert command is None, 'Mouse hold must only edit the preview'
+            pump(.02)
+        assert ids() == before, 'Controller received an action before release'
+        assert error(initial)[0] < .001, 'Robot moved while mouse was held'
+        shown, command = drag.step(shown, False)
+        assert command is not None
+        msg = GizmoCommand(target=copy.deepcopy(goal), epoch=epoch)
+        msg.target.header.stamp = state['gizmo'].actual.header.stamp
+        publisher.publish(msg)
+        return epoch, before
+
+    def finish(goal, epoch, before):
+        # No command keepalive: one release must complete even beyond timeout.
+        wait(lambda: state['gizmo'].epoch != epoch)
+        assert state['gizmo'].reason == 'target reached', state['gizmo'].reason
+        wait(lambda: len(ids() - before) == 1, 2.)
+        wait(lambda: error(goal)[0] < .002 and error(goal)[1] < .01, 2.)
+        pump(.15)
+        assert len(ids() - before) == 1, 'A release generated repeated actions'
+        assert not any(g.status in (1, 2, 3) for g in state['status'].status_list)
+        shown, command = drag.step(pose_matrix(goal.pose), False)
+        assert command is None
+        np.testing.assert_allclose(shown, pose_matrix(state['gizmo'].actual.pose), atol=1e-8)
+
+    try:
+        params = AsyncParameterClient(node, '/tcp_gizmo')
+        assert params.wait_for_services(timeout_sec=30.)
+        future = params.get_parameters(['simulation', 'use_sim_time', 'pose_link', 'robot_description_semantic',
+                                        'max_linear_speed'])
+        wait(future.done)
+        values = future.result().values
+        assert values[0].bool_value and values[1].bool_value, 'Simulation only'
+        assert values[2].string_value == 'tcp', 'Gizmo must use active tool TCP'
+        srdf = ET.fromstring(values[3].string_value)
+        controllers = node.create_client(ListControllers, '/controller_manager/list_controllers')
+        assert controllers.wait_for_service(timeout_sec=30.)
+        deadline = time.monotonic() + 30.
+        while True:
+            listing = controllers.call_async(ListControllers.Request())
+            wait(listing.done)
+            if any(c.name == 'iiwa_arm_controller' and c.state == 'active' for c in listing.result().controller):
+                break
+            assert time.monotonic() < deadline
+            pump(.1)
+        wait(lambda: 'gizmo' in state and state['gizmo'].enabled)
+        action = ActionClient(node, FollowJointTrajectory, '/iiwa_arm_controller/follow_joint_trajectory')
+        assert action.wait_for_server(timeout_sec=5.)
+
+        def ordinary(name):
+            joints = srdf.find(f"group_state[@name='{name}']")
+            goal = FollowJointTrajectory.Goal()
+            goal.trajectory.joint_names = [j.attrib['name'] for j in joints]
+            point = JointTrajectoryPoint(positions=[float(j.attrib['value']) for j in joints])
+            point.time_from_start.sec = 3
+            goal.trajectory.points = [point]
+            future = action.send_goal_async(goal)
+            wait(future.done)
+            assert future.result().accepted
+            return future.result().get_result_async()
+
+        done = ordinary('home')
+        wait(done.done)
+        assert done.result().result.error_code == 0
+        wait(lambda: state['gizmo'].enabled)
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.z -= .02
+        finish(goal, *release(goal))
+        print('PASS: straight posture -> 2 cm descent; no action during drag, exactly one on release', flush=True)
+
+        done = ordinary('work')
+        wait(done.done)
+        assert done.result().result.error_code == 0
+        wait(lambda: state['gizmo'].enabled)
+        for kind in ('translation', 'rotation'):
+            for axis in range(3):
+                for sign in (1., -1.):
+                    goal = copy.deepcopy(state['gizmo'].actual)
+                    if kind == 'translation':
+                        key = ('x', 'y', 'z')[axis]
+                        setattr(goal.pose.position, key, getattr(goal.pose.position, key) + sign * .01)
+                    else:
+                        rotvec = np.zeros(3)
+                        rotvec[axis] = sign * .03
+                        q = (Rotation.from_rotvec(rotvec) * Rotation.from_matrix(pose_matrix(goal.pose)[:3, :3])).as_quat()
+                        goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w = q.tolist()
+                    finish(goal, *release(goal))
+                    print(f'PASS: {kind} axis={axis} sign={sign:+.0f}; exactly one action', flush=True)
+
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x = 10.
+        epoch, before = release(goal)
+        wait(lambda: state['gizmo'].epoch != epoch)
+        assert 'unreachable' in state['gizmo'].reason
+        assert ids() == before, 'Invalid target dispatched an action'
+        shown, command = drag.step(pose_matrix(goal.pose), False)
+        np.testing.assert_allclose(shown, pose_matrix(state['gizmo'].actual.pose), atol=1e-8)
+        assert command is None
+        print('PASS: unreachable released target resets marker; no controller action', flush=True)
+
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x -= .1
+        epoch, before = release(goal)
+        wait(lambda: len(ids() - before) == 1)
+        low_id = next(iter(ids() - before))
+        assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 2 for g in state['status'].status_list)
+        done = ordinary('work')
+        # The high request may be accepted upstream, but must not be dispatched
+        # to JTC until the complete gizmo action has succeeded.
+        deadline = time.monotonic() + 20.
+        while not done.done():
+            assert time.monotonic() < deadline, 'Queued ordinary command did not complete'
+            pump(.01)
+            if len(ids() - before) > 1:
+                assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 4
+                           for g in state['status'].status_list), 'Ordinary motion interrupted gizmo'
+        assert done.result().result.error_code == 0
+        wait(lambda: state['gizmo'].enabled)
+        assert len(ids() - before) == 2
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x -= .01
+        finish(goal, *release(goal))
+        print('PASS: ordinary action waits for completed gizmo action; next release works', flush=True)
+
+        # Pose/joints/file planners first request a lease, before planning from
+        # the robot's current state. Exercise their actual client as well.
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x -= .1
+        epoch, before = release(goal)
+        wait(lambda: len(ids() - before) == 1)
+        low_id = next(iter(ids() - before))
+        priority = MotionPriorityClient()
+        completed = threading.Event()
+        failures = []
+        def acquire():
+            try:
+                priority.exchange('release-regression', True, 5.)
+            except Exception as error:
+                failures.append(error)
+            finally:
+                completed.set()
+        started = time.monotonic()
+        worker = threading.Thread(target=acquire, daemon=True)
+        worker.start()
+        try:
+            wait(completed.is_set)
+            assert not failures, failures
+            elapsed = time.monotonic() - started
+            assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 4
+                       for g in state['status'].status_list), 'Lease granted before gizmo completed'
+            if values[4].double_value <= .03:
+                assert elapsed > 2., 'Slow trajectory did not exercise the old 2 s RPC timeout'
+            priority.exchange('release-regression', False, 5.)
+            wait(lambda: state['gizmo'].enabled)
+            print(f'PASS: planner lease waits for full gizmo action ({elapsed:.2f} s)', flush=True)
+        finally:
+            priority.close()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/iiwa_planning/test/gizmo_webots_speed.py

@@ -0,0 +1,112 @@
+"""Manual Webots speed regression: work -> 10 cm inward TCP translation.
+
+Run against iiwa.launch.py simulate:=true. Checks actual feedback, both peak
+TCP speed and convergence, and prints elapsed simulation/wall time.
+"""
+import copy
+import math
+import time
+import xml.etree.ElementTree as ET
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.parameter_client import AsyncParameterClient
+from control_msgs.action import FollowJointTrajectory
+from controller_manager_msgs.srv import ListControllers
+from trajectory_msgs.msg import JointTrajectoryPoint
+from iiwa_msgs.msg import GizmoCommand, GizmoState
+
+
+def main():
+    rclpy.init()
+    node = rclpy.create_node('gizmo_webots_speed')
+    states = []
+    node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', states.append, 10)
+    publisher = node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
+
+    def wait(predicate, timeout=30.):
+        deadline = time.monotonic() + timeout
+        while not predicate():
+            assert time.monotonic() < deadline, 'Timed out waiting for simulation'
+            rclpy.spin_once(node, timeout_sec=.01)
+
+    def xyz(state):
+        p = state.actual.pose.position
+        return p.x, p.y, p.z
+
+    def stamp(state):
+        t = state.actual.header.stamp
+        return t.sec + t.nanosec * 1e-9
+
+    try:
+        params = AsyncParameterClient(node, '/tcp_gizmo')
+        assert params.wait_for_services(timeout_sec=30.)
+        future = params.get_parameters([
+            'simulation', 'use_sim_time', 'max_linear_speed', 'robot_description_semantic'])
+        wait(future.done)
+        values = future.result().values
+        assert values[0].bool_value and values[1].bool_value, 'Webots simulation only'
+        speed_limit = values[2].double_value
+        work = ET.fromstring(values[3].string_value).find("group_state[@name='work']")
+        assert work is not None
+        controllers = node.create_client(ListControllers, '/controller_manager/list_controllers')
+        assert controllers.wait_for_service(timeout_sec=30.)
+        deadline = time.monotonic() + 30.
+        while True:
+            listing = controllers.call_async(ListControllers.Request())
+            wait(listing.done)
+            if any(c.name == 'iiwa_arm_controller' and c.state == 'active'
+                   for c in listing.result().controller):
+                break
+            assert time.monotonic() < deadline, 'Controller did not activate'
+            rclpy.spin_once(node, timeout_sec=.1)
+        wait(lambda: states and states[-1].enabled)
+        action = ActionClient(node, FollowJointTrajectory, '/iiwa_arm_controller/follow_joint_trajectory')
+        assert action.wait_for_server(timeout_sec=5.)
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory.joint_names = [joint.attrib['name'] for joint in work]
+        point = JointTrajectoryPoint(positions=[float(joint.attrib['value']) for joint in work])
+        point.time_from_start.sec = 3
+        goal.trajectory.points = [point]
+        future = action.send_goal_async(goal)
+        wait(future.done)
+        assert future.result().accepted
+        done = future.result().get_result_async()
+        wait(done.done)
+        assert done.result().result.error_code == 0
+        wait(lambda: states[-1].enabled)
+        initial = states[-1]
+        target = copy.deepcopy(initial.actual)
+        target.pose.position.x -= .1
+        start = len(states) - 1
+        epoch = initial.epoch
+        wall_start = time.monotonic()
+        publisher.publish(GizmoCommand(target=target, epoch=epoch))
+        while states[-1].epoch == epoch:
+            assert time.monotonic() - wall_start < 30., 'TCP did not reach 10 cm target'
+            end = time.monotonic() + .025
+            while time.monotonic() < end:
+                rclpy.spin_once(node, timeout_sec=.005)
+        assert states[-1].reason == 'target reached', states[-1].reason
+        wait(lambda: initial.actual.pose.position.x - states[-1].actual.pose.position.x >= .099, 2.)
+        samples = states[start:]
+        speeds = [math.dist(xyz(a), xyz(b)) / (stamp(b) - stamp(a))
+                  for a, b in zip(samples, samples[1:]) if stamp(b) > stamp(a)]
+        elapsed = stamp(samples[-1]) - stamp(initial)
+        peak = max(speeds)
+        distance = math.dist(xyz(initial), xyz(samples[-1]))
+        print(f'limit={speed_limit:.3f} m/s; distance={distance:.6f} m; '
+              f'simulation_time={elapsed:.3f} s; wall_time={time.monotonic() - wall_start:.3f} s; '
+              f'mean={distance / elapsed:.4f} m/s; peak={peak:.4f} m/s', flush=True)
+        assert peak <= speed_limit * 1.05 + .002, 'Measured TCP speed exceeds configured cap'
+        # At 0.50 m/s, a 10 cm work-pose move must not consist of hundreds of
+        # tiny acceleration-limited 64 ms steps. Keep margin for simulator load.
+        if speed_limit >= .5:
+            assert elapsed < 3., f'Artificial short-step speed bottleneck: {elapsed:.3f} s'
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/iiwa_planning/test/test_motion_priority.py

@@ -0,0 +1,185 @@
+import threading
+import pytest
+from iiwa_planning.motion_priority import MotionLease, LeaseError, MotionSerialiser
+from iiwa_planning.motion_priority import wait_response
+
+
+def test_priority_acquisition_waits_for_full_gizmo_trajectory():
+    from concurrent.futures import Future
+    future = Future()
+    timer = threading.Timer(2.1, lambda: future.set_result('granted'))
+    timer.start()
+    try:
+        assert wait_response(future, timeout=None, available=lambda: True) == 'granted'
+    finally:
+        timer.join()
+
+
+def test_waiting_priority_fails_if_server_disappears():
+    from concurrent.futures import Future
+    future = Future()
+    with pytest.raises(LeaseError, match='unavailable'):
+        wait_response(future, timeout=None, available=lambda: False)
+    assert future.cancelled()
+
+
+def test_overlapping_operations_have_independent_owners_and_release():
+    owners = set()
+    def exchange(owner, acquire, ttl):
+        owners.add(owner) if acquire else owners.discard(owner)
+    with MotionLease(exchange) as first:
+        with MotionLease(exchange) as second:
+            assert first.owner != second.owner
+            assert len(owners) == 2
+        assert owners == {first.owner}
+    assert not owners
+
+
+def test_acquisition_fails_closed():
+    def unavailable(*args):
+        raise LeaseError('unavailable')
+    with pytest.raises(LeaseError), MotionLease(unavailable):
+        pytest.fail('motion entered without reservation')
+
+
+def test_renewal_failure_stops_operation():
+    stopped = threading.Event()
+    calls = []
+    def exchange(owner, acquire, ttl):
+        calls.append(acquire)
+        if acquire and len(calls) > 1:
+            raise LeaseError('lost')
+    with pytest.raises(LeaseError):
+        with MotionLease(exchange, on_lost=stopped.set, renew_interval=0.01) as lease:
+            assert stopped.wait(1)
+            lease.check()
+    assert calls[-1] is False
+
+
+def test_release_does_not_mask_original_error():
+    def exchange(owner, acquire, ttl):
+        if not acquire:
+            raise LeaseError('release failed')
+    with pytest.raises(ValueError, match='original'):
+        with MotionLease(exchange):
+            raise ValueError('original')
+
+
+def test_stop_invalidates_plans_and_queued_requests():
+    serial = MotionSerialiser()
+    generation = serial.generation()
+    serial.stop(lambda: None)
+    with pytest.raises(LeaseError):
+        serial.check(generation)
+    serial.check(serial.generation())
+
+
+def test_motion_lock_serializes_workers():
+    serial = MotionSerialiser()
+    entered = threading.Event()
+    finished = threading.Event()
+    with serial.lock:
+        def worker():
+            entered.set()
+            with serial.lock:
+                finished.set()
+        thread = threading.Thread(target=worker)
+        thread.start()
+        assert entered.wait(1)
+        assert not finished.is_set()
+    thread.join(1)
+    assert finished.is_set()
+
+
+def _server_methods():
+    """Exercise real server orchestration without requiring MoveIt or ROS libraries."""
+    import ast
+    from pathlib import Path
+    path = Path(__file__).parents[1] / 'scripts' / 'move_to_pose_server.py'
+    source = ast.parse(path.read_text())
+    cls = next(n for n in source.body if isinstance(n, ast.ClassDef))
+    names = {'_plan_and_execute', '_check_operation', '_guarded', '_lease'}
+    methods = [n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name in names]
+    namespace = {'threading': threading, 'MotionLease': MotionLease,
+                 'PlanRequestParameters': object}
+    exec(compile(ast.Module(body=methods, type_ignores=[]), str(path), 'exec'), namespace)
+    return type('Server', (), {n: namespace[n] for n in names})
+
+
+def _fake_server(plan, execute):
+    from types import SimpleNamespace
+    server = _server_methods()()
+    server._motion_serial = MotionSerialiser()
+    server._operation = threading.local()
+    server._operation.generation = server._motion_serial.generation()
+    server._operation.lease = MotionLease()
+    server._arm = SimpleNamespace(plan=plan)
+    server._moveit = SimpleNamespace(execute=execute)
+    server.get_logger = lambda: SimpleNamespace(error=lambda _: None)
+    return server
+
+
+def test_stop_during_planning_prevents_execution():
+    from types import SimpleNamespace
+    executions = []
+    def plan(**kwargs):
+        server._motion_serial.stop(lambda: None)
+        return SimpleNamespace(trajectory=object())
+    server = _fake_server(plan, lambda *a, **kw: executions.append(a))
+    with pytest.raises(LeaseError, match='stop'):
+        server._plan_and_execute(None, SimpleNamespace(is_cancel_requested=False))
+    assert not executions
+
+
+def test_failed_execution_is_reported_as_failure():
+    from types import SimpleNamespace
+    server = _fake_server(lambda **kw: SimpleNamespace(trajectory=object()),
+                          lambda *a, **kw: False)
+    ok, _ = server._plan_and_execute(None, SimpleNamespace(is_cancel_requested=False))
+    assert ok is False
+
+
+def test_queued_motion_reserves_before_waiting_for_planning_lock():
+    server = _server_methods()()
+    from types import SimpleNamespace
+    reserved = threading.Event()
+    completed = threading.Event()
+    server._motion_serial = MotionSerialiser()
+    server._operation = threading.local()
+    server._gizmo_enabled = True
+    server._stop_execution = lambda: None
+    server._priority = SimpleNamespace(exchange=lambda owner, acquire, ttl:
+                                       reserved.set() if acquire else None)
+    with server._motion_serial.lock:
+        thread = threading.Thread(target=lambda: server._guarded(completed.set))
+        thread.start()
+        assert reserved.wait(1)
+        assert not completed.is_set()
+    thread.join(1)
+    assert completed.is_set()
+
+
+def test_disabled_lease_needs_no_ros_service():
+    with MotionLease() as lease:
+        lease.check()
+
+
+def test_failed_sequence_reports_nonzero_exit_and_releases_resources():
+    import ast
+    from pathlib import Path
+    from types import SimpleNamespace
+    path = Path(__file__).parents[1] / 'scripts' / 'motion_sequence_runner.py'
+    tree = ast.parse(path.read_text())
+    cls = next(n for n in tree.body if isinstance(n, ast.ClassDef))
+    method = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == 'run')
+    namespace = {'threading': threading, 'MotionLease': MotionLease, 'LeaseError': LeaseError}
+    exec(compile(ast.Module(body=[method], type_ignores=[]), str(path), 'exec'), namespace)
+    stopped, closed, done = threading.Event(), threading.Event(), threading.Event()
+    def unavailable():
+        raise LeaseError('priority unavailable')
+    runner = SimpleNamespace(_discover_priority=unavailable, _priority=None,
+                             _abort_sequence=stopped.set, close_bag=closed.set,
+                             get_logger=lambda: SimpleNamespace(error=lambda _: None))
+    namespace['run'](runner, done)
+    assert stopped.is_set() and closed.is_set() and done.is_set()
+    assert runner.exit_code != 0

src/iiwa_planning/test/test_tcp_gizmo_policy.cpp

@@ -0,0 +1,81 @@
+#include <gtest/gtest.h>
+#include <iiwa_planning/tcp_gizmo_policy.hpp>
+using iiwa_planning::GizmoPolicy;
+TEST(GizmoPolicy, LeasesAreBarriersAndExpiredCommandsNeverReplay) {
+  GizmoPolicy gate;
+  EXPECT_TRUE(gate.accept(0, 1., .3));
+  EXPECT_TRUE(gate.acquire("planning", 2., 1.));
+  EXPECT_EQ(gate.epoch(), 1u);
+  EXPECT_FALSE(gate.allowed(1.5, 0., true));
+  EXPECT_FALSE(gate.hasTarget());
+  EXPECT_TRUE(gate.allowed(3., 0., true));
+  EXPECT_FALSE(gate.accept(0, 3., .3));
+  EXPECT_TRUE(gate.accept(1, 3., .3));
+  EXPECT_TRUE(gate.expired(3.31));
+}
+TEST(GizmoPolicy, RenewalAndMultipleOwnersHoldThroughPauses) {
+  GizmoPolicy gate;
+  gate.acquire("a", 2., 0.); gate.acquire("b", 5., 0.);
+  gate.acquire("a", 4., 1.);
+  EXPECT_EQ(gate.epoch(), 2u);
+  gate.release("b");
+  EXPECT_FALSE(gate.allowed(4., 100., true));
+  EXPECT_TRUE(gate.allowed(5.1, 100., true));
+}
+TEST(GizmoPolicy, NormalTrajectoryWaitsForSimulationDeadlineAndSettling) {
+  GizmoPolicy gate;
+  gate.normal(12.);
+  EXPECT_FALSE(gate.allowed(100., 11., true));
+  EXPECT_FALSE(gate.allowed(100., 12.1, false));
+  EXPECT_TRUE(gate.allowed(100., 12.1, true));
+  EXPECT_FALSE(gate.hasTarget());
+}
+TEST(GizmoPolicy, RejectsInvalidLeaseAndBlocksActions) {
+  GizmoPolicy gate;
+  EXPECT_FALSE(gate.acquire("", 1., 0.));
+  EXPECT_FALSE(gate.acquire("x", -1., 0.));
+  gate.action(true);
+  EXPECT_FALSE(gate.allowed(0., 0., true));
+  gate.action(false);
+  EXPECT_FALSE(gate.allowed(0., 0., false));
+  EXPECT_TRUE(gate.allowed(0., 0., true));
+}
+
+TEST(GizmoPolicy, LeaseReleaseRequiresRobotToSettle) {
+  GizmoPolicy gate;
+  gate.acquire("planning", 2., 0.);
+  gate.release("planning");
+  EXPECT_FALSE(gate.allowed(1., 1., false));
+  EXPECT_TRUE(gate.allowed(1., 1., true));
+}
+
+TEST(LowGoalBarrier, DelayedAcceptanceCannotPassHighBarrier) {
+  iiwa_planning::LowGoalBarrier barrier;
+  EXPECT_TRUE(barrier.begin());
+  EXPECT_FALSE(barrier.idle());
+  barrier.accepted(true);
+  EXPECT_FALSE(barrier.idle());
+  // A high-priority request must drain the already accepted low step.
+  EXPECT_FALSE(barrier.begin());
+  barrier.terminal();
+  EXPECT_TRUE(barrier.idle());
+}
+TEST(LowGoalBarrier, RejectedGoalReleasesBarrierWithoutCancellation) {
+  iiwa_planning::LowGoalBarrier barrier;
+  EXPECT_TRUE(barrier.begin());
+  EXPECT_FALSE(barrier.begin());
+  barrier.accepted(false);
+  EXPECT_TRUE(barrier.idle());
+  EXPECT_TRUE(barrier.begin());
+  barrier.accepted(true);
+  EXPECT_FALSE(barrier.idle());
+  barrier.terminal();
+  EXPECT_TRUE(barrier.idle());
+}
+
+TEST(GizmoPolicy, ReachedTargetDoesNotRequestServoMotion) {
+  EXPECT_FALSE(iiwa_planning::requiresGizmoMotion(0., 0.));
+  EXPECT_FALSE(iiwa_planning::requiresGizmoMotion(1e-6, 1e-5));
+  EXPECT_TRUE(iiwa_planning::requiresGizmoMotion(1e-3, 0.));
+  EXPECT_TRUE(iiwa_planning::requiresGizmoMotion(0., .01));
+}

src/iiwa_utils/iiwa_utils/camera_spawner.py

@@ -178,12 +178,19 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
         devices.append(
             f'    <device reference="{cfg.name}" type="Camera">\n'
             f'      <ros>\n'
+            f'        <enabled>false</enabled>\n'
             f'        <topicName>{ros.topic}</topicName>\n'
             f'        <updateRate>{ros.update_rate}</updateRate>\n'
             f'        <alwaysOn>True</alwaysOn>\n'
             f'        <frameName>{cfg.name}_link</frameName>\n'
             f'      </ros>\n'
-            f'    </device>'
+            f'    </device>\n'
+            f'    <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
+            f'      <device>{cfg.name}</device>\n'
+            f'      <topicName>{ros.topic}</topicName>\n'
+            f'      <updateRate>{ros.update_rate}</updateRate>\n'
+            f'      <frameName>{cfg.name}_link</frameName>\n'
+            f'    </plugin>'
         )
 
     if cfg.range_finder is not None:
@@ -191,12 +198,20 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
         devices.append(
             f'    <device reference="{cfg.name}_depth" type="RangeFinder">\n'
             f'      <ros>\n'
+            f'        <enabled>false</enabled>\n'
             f'        <topicName>{ros.topic}</topicName>\n'
             f'        <updateRate>{ros.update_rate}</updateRate>\n'
             f'        <alwaysOn>True</alwaysOn>\n'
             f'        <frameName>{cfg.name}_link</frameName>\n'
             f'      </ros>\n'
-            f'    </device>'
+            f'    </device>\n'
+            f'    <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
+            f'      <device>{cfg.name}_depth</device>\n'
+            f'      <depth>true</depth>\n'
+            f'      <topicName>{ros.topic}</topicName>\n'
+            f'      <updateRate>{ros.update_rate}</updateRate>\n'
+            f'      <frameName>{cfg.name}_link</frameName>\n'
+            f'    </plugin>'
         )
 
     devices_str = "\n".join(devices)

src/iiwa_utils/iiwa_utils/object_scene.py

@@ -0,0 +1,196 @@
+"""Validated object configuration and Webots scene generation (metres/radians)."""
+import json
+import math
+import random
+import shlex
+from dataclasses import dataclass, field
+from pathlib import Path
+
+from scipy.spatial.transform import Rotation
+
+
+@dataclass(frozen=True)
+class ObjectModelCfg:
+    path: str
+    rotation_deg: tuple[float, float, float] = (0.0, 0.0, 0.0)
+
+
+@dataclass(frozen=True)
+class ObjectsCfg:
+    enabled: bool = False
+    count: int = 0
+    models: list[ObjectModelCfg] = field(default_factory=list)
+    area: dict[str, list[float]] = field(default_factory=dict)
+
+
+def parse_objects(raw, resolve_path) -> ObjectsCfg:
+    if raw is None:
+        return ObjectsCfg()
+    if not isinstance(raw, dict):
+        raise ValueError('digital_twin.webots.objects must be a mapping')
+    enabled = raw.get('enabled', False)
+    count = raw.get('count', 0)
+    models = raw.get('models', [])
+    area = raw.get('area', {})
+    if type(enabled) is not bool:
+        raise ValueError('objects.enabled must be true or false')
+    if type(count) is not int or count < 0:
+        raise ValueError('objects.count must be an integer >= 0')
+    if not isinstance(models, list):
+        raise ValueError('objects.models must be a list of paths or model mappings')
+    if enabled and count and not models:
+        raise ValueError('objects.models must not be empty when count > 0')
+    if not isinstance(area, dict):
+        raise ValueError('objects.area must be a mapping')
+    parsed_area = {}
+    if area or (enabled and count):
+        for axis in 'xyz':
+            bounds = area.get(axis)
+            if (not isinstance(bounds, (list, tuple)) or len(bounds) != 2
+                    or any(type(v) not in (int, float) or not math.isfinite(v) for v in bounds)
+                    or bounds[0] > bounds[1]):
+                raise ValueError(f'objects.area.{axis} must be [min, max], finite metres, min <= max')
+            parsed_area[axis] = list(map(float, bounds))
+    resolved = []
+    for index, entry in enumerate(models):
+        if isinstance(entry, str):
+            entry = {'path': entry}
+        if not isinstance(entry, dict) or entry.keys() - {'path', 'rotation_deg'}:
+            raise ValueError(f'objects.models[{index}] requires path and optional rotation_deg')
+        path = entry.get('path')
+        angles = entry.get('rotation_deg', (0, 0, 0))
+        if not isinstance(path, str) or not path.strip():
+            raise ValueError(f'objects.models[{index}].path must be a nonempty path')
+        if (not isinstance(angles, (list, tuple)) or len(angles) != 3
+                or any(type(v) not in (int, float) or not math.isfinite(v) for v in angles)):
+            raise ValueError(f'objects.models[{index}].rotation_deg must be finite [X, Y, Z] degrees')
+        resolved.append(ObjectModelCfg(resolve_path(path), tuple(map(float, angles))))
+    if any(Path(model.path).suffix.lower() != '.obj' for model in resolved):
+        raise ValueError('objects.models currently supports Wavefront .obj files')
+    return ObjectsCfg(enabled, count, resolved, parsed_area)
+
+
+def robot_pose(translation, rotation):
+    try:
+        position = tuple(map(float, translation.split()))
+        orient = tuple(map(float, rotation.split()))
+    except (ValueError, AttributeError) as exc:
+        raise ValueError('Robot transform/rotation must be numeric strings') from exc
+    if len(position) != 3 or len(orient) != 4 or not all(map(math.isfinite, position + orient)):
+        raise ValueError('Robot transform needs 3 and rotation 4 finite numbers')
+    length = math.sqrt(sum(v*v for v in orient[:3]))
+    if length == 0:
+        raise ValueError('Robot rotation axis must not be zero')
+    return position, tuple(v / length for v in orient[:3]) + (orient[3],)
+
+
+def world_position(local, translation, rotation):
+    position, orient = robot_pose(translation, rotation)
+    axis, angle = orient[:3], orient[3]
+    c, s = math.cos(angle), math.sin(angle)
+    dot = sum(a*b for a, b in zip(axis, local))
+    cross = (axis[1]*local[2] - axis[2]*local[1],
+             axis[2]*local[0] - axis[0]*local[2],
+             axis[0]*local[1] - axis[1]*local[0])
+    return tuple(position[i] + local[i]*c + cross[i]*s + axis[i]*dot*(1-c) for i in range(3))
+
+
+@dataclass(frozen=True)
+class ObjAsset:
+    path: Path
+    offset: tuple
+    size: tuple
+    textures: tuple
+
+
+def _require_file(path):
+    if not path.is_file():
+        raise ValueError(f'Object asset file does not exist: {path}')
+
+
+def load_obj(path) -> ObjAsset:
+    """Keep original OBJ/MTL/UV data; derive a centred, bottom-aligned origin."""
+    path = Path(path).resolve()
+    _require_file(path)
+    low, high = [math.inf]*3, [-math.inf]*3
+    materials, textures = [], []
+    for line in path.read_text(encoding='utf-8').splitlines():
+        parts = line.split()
+        if not parts:
+            continue
+        if parts[0] == 'v':
+            vertex = tuple(map(float, parts[1:4]))
+            if len(vertex) != 3 or not all(map(math.isfinite, vertex)):
+                raise ValueError(f'Invalid vertex in {path}')
+            low = [min(a, b) for a, b in zip(low, vertex)]
+            high = [max(a, b) for a, b in zip(high, vertex)]
+        elif parts[0] == 'mtllib':
+            materials.extend(path.parent / p for p in shlex.split(line)[1:])
+    size = tuple(b-a for a, b in zip(low, high))
+    if any(not math.isfinite(v) or v <= 0 for v in size):
+        raise ValueError(f'OBJ must have finite, nonzero 3D bounds: {path}')
+    for material in materials:
+        _require_file(material)
+        for line in material.read_text(encoding='utf-8').splitlines():
+            parts = shlex.split(line, comments=True)
+            if parts and (parts[0].lower().startswith('map_') or parts[0].lower() in ('bump', 'disp', 'decal', 'norm')):
+                # Wavefront map options precede the final texture filename.
+                texture = material.parent / parts[-1]
+                _require_file(texture)
+                textures.append(texture)
+    offset = (-(low[0]+high[0])/2, -(low[1]+high[1])/2, -low[2])
+    return ObjAsset(path, offset, size, tuple(textures))
+
+
+def _vector(values):
+    return ' '.join(format(v, '.12g') for v in values)
+
+
+def model_rotation(rotation_deg):
+    """Fixed-axis XYZ degrees: Rz(z) @ Ry(y) @ Rx(x), relative to robot base."""
+    rotvec = Rotation.from_euler('xyz', rotation_deg, degrees=True).as_rotvec()
+    angle = math.sqrt(sum(v*v for v in rotvec))
+    if angle < 1e-12:
+        return (0.0, 0.0, 1.0, 0.0)
+    return tuple(float(v / angle) for v in rotvec) + (angle,)
+
+
+def build_objects(config, translation, rotation, rng=None):
+    if not config.enabled or config.count == 0:
+        return []
+    _, orient = robot_pose(translation, rotation)
+    # Validate every allowed asset before any service request is sent.
+    assets = [(load_obj(model.path), model.rotation_deg) for model in config.models]
+    rng = rng or random.Random()
+    nodes = []
+    for index in range(config.count):
+        asset, angles = rng.choice(assets)
+        model_orient = model_rotation(angles)
+        matrix = Rotation.from_euler('xyz', angles, degrees=True).as_matrix()
+        # Lift the rotated collision box above the spawn anchor. Keep the
+        # original oriented box, rather than inflating it into a world AABB.
+        half_height = sum(abs(matrix[2, i]) * asset.size[i] / 2 for i in range(3))
+        center = (-asset.offset[0], -asset.offset[1],
+                  -asset.offset[2] + asset.size[2] / 2)
+        rotated_center = matrix @ center
+        visual_offset = tuple(-rotated_center[i] + (half_height if i == 2 else 0)
+                              for i in range(3))
+        local = tuple(rng.uniform(*config.area[axis]) for axis in 'xyz')
+        world = world_position(local, translation, rotation)
+        nodes.append(f'''Solid {{
+  name "spawned_object_{index:04d}"
+  translation {_vector(world)}
+  rotation {_vector(orient)}
+  children [ Pose {{
+    translation {_vector(visual_offset)}
+    rotation {_vector(model_orient)}
+    children [ CadShape {{ url [ {json.dumps(str(asset.path))} ] }} ]
+  }} ]
+  boundingObject Pose {{
+    translation 0 0 {half_height:.12g}
+    rotation {_vector(model_orient)}
+    children [ Box {{ size {_vector(asset.size)} }} ]
+  }}
+  physics Physics {{ density -1 mass 0.5 }}
+}}''')
+    return nodes

src/iiwa_utils/iiwa_utils/object_spawner.py

@@ -1,88 +1,50 @@
-# /Ros2Supervisor/spawn_node_from_string / webots_ros2_msgs/srv/SpawnNodeFromString
-# /Ros2Supervisor/spawn_urdf_robot
-
-
-from random import randint
+"""Spawn the configured batch once through the Webots supervisor service."""
+import json
 
 import rclpy
 from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
-from rclpy.task import Future
 from webots_ros2_msgs.srv import SpawnNodeFromString
 
+from iiwa_utils.object_scene import build_objects, parse_objects
 
-class ObjectSpawner(Node):
-    def __init__(self):
-        super().__init__("object_spawner")
 
-        # параметры передаваемые
-        self.declare_parameter('objects_path', '')
-
-        # переменные
-        self._object_count: int = randint(1, 5)
-        self._spawned_count: int = 0
-        self._call_in_progress: bool = False
-
-        # Реализация
-        self.cli = self.create_client(
-            SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
-        )
-
-        while not self.cli.wait_for_service(timeout_sec=10):
-            self.get_logger().warning(
-                "service /Ros2Supervisor/spawn_node_from_string  not available, waiting again..."
-            )
-
-        self._timer = self.create_timer(0.1, self.timer_callback)
-
-    # TODO: переделать процесс спавна, должен сформировать все proto файлы, после чего заспавнить все объекты
-    def timer_callback(self):
-        # TODO: здесь должен получать объекты
-        if self._spawned_count >= self._object_count:
-            self.get_logger().info(f"All {self._object_count} objects spawned")
-            self._timer.cancel()
+def spawn_objects(node):
+    raw = json.loads(node.declare_parameter('objects_config', '{}').value)
+    translation = node.declare_parameter('robot_translation', '0 0 0').value
+    rotation = node.declare_parameter('robot_rotation', '0 0 1 0').value
+    config = parse_objects(raw, lambda path: path)
+    objects = build_objects(config, translation, rotation)
+    if not objects:
+        node.get_logger().info('Object spawning disabled or count is zero')
         return
-
-        if self._call_in_progress:
-            return
-
-        data = 'Solid { name "test_box2" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }'
-        data = self._create_group(children=[data])
-
-        req = SpawnNodeFromString.Request(data=data, check_fields=True)
-        self.get_logger().info(
-            f"Calling spawn service #{self._spawned_count + 1}/{self._object_count}"
-        )
-        self._call_in_progress = True
-        future = self.cli.call_async(req)
-        future.add_done_callback(self._response_callback)
-
-    def _response_callback(self, future: Future):
-        try:
+    client = node.create_client(SpawnNodeFromString, '/Ros2Supervisor/spawn_node_from_string')
+    if not client.wait_for_service(timeout_sec=60.0):
+        raise RuntimeError('Webots supervisor unavailable; no objects spawned')
+    for index, data in enumerate(objects):
+        future = client.call_async(SpawnNodeFromString.Request(data=data))
+        rclpy.spin_until_future_complete(node, future, timeout_sec=30.0)
+        if not future.done():
+            # A timeout does not prove rejection: retrying could create duplicates.
+            raise RuntimeError(f'Object {index + 1}: spawn timed out; result unknown, no retry')
         response = future.result()
-            self.get_logger().info(f"Spawn service response: {response}")
-        except Exception as e:
-            self.get_logger().error(f"Service call field: {e}")
-        finally:
-            self._call_in_progress = False
-            self._spawned_count += 1
-
-    # Вспомогательные методы формирования кода для спавна объектов
-    def _create_group(self, children: list[str]) -> str:
-        return f"DEF OBJECTS Group {{children [{" ".join(children)}]}}"
+        if response is None or not response.success:
+            raise RuntimeError(f'Object {index + 1} rejected; {index}/{len(objects)} spawned')
+    node.get_logger().info(f'All {len(objects)} objects spawned')
 
 
 def main(args=None):
-    try:
     rclpy.init(args=args)
-        node = ObjectSpawner()
-        rclpy.spin(node=node)
+    node = Node('object_spawner')
+    try:
+        spawn_objects(node)
     except (KeyboardInterrupt, ExternalShutdownException):
         pass
     finally:
         node.destroy_node()
+        if rclpy.ok():
             rclpy.shutdown()
 
 
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -1,10 +1,13 @@
+import math
 import os
-from dataclasses import dataclass, fields, is_dataclass
+from dataclasses import dataclass, field, fields, is_dataclass
 from typing import Any, Dict, List, Optional, Type, TypeVar
 
 import yaml
 from ament_index_python.packages import get_package_share_directory
 
+from iiwa_utils.object_scene import ObjectsCfg, load_obj, parse_objects, robot_pose
+
 T = TypeVar("T")
 
 
@@ -16,7 +19,6 @@ class RobotCfg:
     description: str
     fri_cycle_ms: int
     joint_position_tau: float
-    active_controller: str  # "jtc" | "forward"
 
 
 @dataclass(frozen=True)
@@ -26,6 +28,7 @@ class WebotsCfg:
     rotation: str
     controller_timer: str
     cameras: List[str]
+    objects: ObjectsCfg = field(default_factory=ObjectsCfg)
 
 
 @dataclass(frozen=True)
@@ -33,10 +36,21 @@ class RvizCfg:
     config: str
 
 
+@dataclass(frozen=True)
+class TcpGizmoCfg:
+    enabled: bool = False
+    publish_period: float = 0.032
+    command_timeout: float = 0.3
+    state_timeout: float = 0.5
+    max_linear_speed: float = 0.10
+    max_angular_speed: float = 0.4
+
+
 @dataclass(frozen=True)
 class DigitalTwinCfg:
     webots: WebotsCfg
     rviz: RvizCfg
+    tcp_gizmo: TcpGizmoCfg = TcpGizmoCfg()
 
 
 @dataclass(frozen=True)
@@ -107,6 +121,24 @@ class SettingsError(RuntimeError):
     pass
 
 
+def parse_tcp_gizmo(raw) -> TcpGizmoCfg:
+    if raw is None:
+        return TcpGizmoCfg()
+    if not isinstance(raw, dict):
+        raise SettingsError('digital_twin.tcp_gizmo must be a mapping')
+    values = {f.name: raw.get(f.name, f.default) for f in fields(TcpGizmoCfg)}
+    if type(values['enabled']) is not bool:
+        raise SettingsError('digital_twin.tcp_gizmo.enabled must be true or false')
+    for key in values.keys() - {'enabled'}:
+        value = values[key]
+        if isinstance(value, bool) or not isinstance(value, (float, int)) or not math.isfinite(value) or value <= 0:
+            raise SettingsError(f'digital_twin.tcp_gizmo.{key} must be finite and positive')
+        values[key] = float(value)
+    if values['command_timeout'] < 2 * values['publish_period']:
+        raise SettingsError('tcp_gizmo.command_timeout must allow at least two publish periods')
+    return TcpGizmoCfg(**values)
+
+
 @dataclass(frozen=True)
 class Settings:
     robot: RobotCfg
@@ -284,14 +316,16 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
 
     # robot
     robot_raw = require(raw, "robot")
+    joint_position_tau = float(robot_raw.get("joint_position_tau", 0.04))
+    if not math.isfinite(joint_position_tau) or joint_position_tau < 0.0:
+        raise ValueError("robot.joint_position_tau must be a finite value >= 0")
     robot = RobotCfg(
         name=str(require(robot_raw, "name")),
         ip=str(require(robot_raw, "ip")),
         port=int(require(robot_raw, "port")),
         description=resolve_path(str(require(robot_raw, "description")), settings_dir),
         fri_cycle_ms=int(robot_raw.get("fri_cycle_ms", 5)),
-        joint_position_tau=float(robot_raw.get("joint_position_tau", 0.04)),
-        active_controller=str(robot_raw.get("active_controller", "jtc")),
+        joint_position_tau=joint_position_tau,
     )
 
     # digital_twin
@@ -308,11 +342,19 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         rotation=str(require(webots_raw, "rotation")),
         controller_timer=str(int(require(webots_raw, "controller_timer"))),
         cameras=cameras,
+        objects=parse_objects(webots_raw.get("objects"),
+                              lambda p: resolve_path(p, settings_dir)),
     )
+    if webots.objects.enabled and webots.objects.count:
+        robot_pose(webots.transform, webots.rotation)
+        if check_files:
+            for model in webots.objects.models:
+                load_obj(model.path)
     rviz = RvizCfg(
         config=resolve_path(str(require(rviz_raw, "config")), settings_dir)
     )
-    digital_twin = DigitalTwinCfg(webots=webots, rviz=rviz)
+    digital_twin = DigitalTwinCfg(webots=webots, rviz=rviz,
+                                 tcp_gizmo=parse_tcp_gizmo(dt_raw.get('tcp_gizmo')))
 
     # controller + moveit
     ctrl_raw = require(raw, "controller")

src/iiwa_utils/iiwa_utils/tcp_gizmo.py

@@ -0,0 +1,91 @@
+"""Native Webots TCP marker. Loaded as a webots_ros2_driver Python plugin."""
+import time
+
+import numpy as np
+import rclpy
+from scipy.spatial.transform import Rotation
+
+from iiwa_msgs.msg import GizmoCommand, GizmoState
+from iiwa_utils.tcp_gizmo_model import DragTarget, to_webots, webots_pose
+
+
+def pose_matrix(pose):
+    result = np.eye(4)
+    result[:3, 3] = [pose.position.x, pose.position.y, pose.position.z]
+    result[:3, :3] = Rotation.from_quat([
+        pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
+    ]).as_matrix()
+    if not np.isfinite(result).all():
+        raise ValueError('Non-finite TCP pose')
+    return result
+
+
+def set_pose(message, matrix):
+    message.position.x, message.position.y, message.position.z = matrix[:3, 3].tolist()
+    q = Rotation.from_matrix(matrix[:3, :3]).as_quat().tolist()
+    message.orientation.x, message.orientation.y, message.orientation.z, message.orientation.w = q
+
+
+class WebotsTcpGizmo:
+    def init(self, webots_node, properties):
+        self._robot = webots_node.robot
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        self._node = rclpy.create_node('tcp_gizmo_webots')
+        self._frame = properties.get('frame', 'base_link')
+        self._mount = webots_pose(
+            [float(x) for x in properties['translation'].split()],
+            [float(x) for x in properties['rotation'].split()],
+        )
+        self._inverse_mount = np.linalg.inv(self._mount)
+        self._state_timeout = float(properties.get('state_timeout', '0.5'))
+        self._drag = DragTarget()
+        self._last_state = 0.0
+        self._last_stamp = None
+        marker = self._robot.getSelf()
+        self._translation = marker.getField('translation')
+        self._rotation = marker.getField('rotation')
+        self._mouse = self._robot.getMouse()
+        self._mouse.enable(int(self._robot.getBasicTimeStep()))
+        self._publisher = self._node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
+        self._node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', self._feedback, 1)
+        self._node.get_logger().info('TCP gizmo: position/rotate the marker, then release the mouse to move')
+
+    def _feedback(self, message):
+        # The launch validates the frame: the Webots mount maps base_link to world.
+        if message.actual.header.frame_id != self._frame:
+            self._drag.invalidate()
+            return
+        try:
+            actual = self._mount @ pose_matrix(message.actual.pose)
+        except ValueError:
+            self._drag.invalidate()
+            return
+        self._last_state = time.monotonic()
+        self._last_stamp = message.actual.header.stamp
+        self._drag.update_feedback(actual, message.enabled, message.epoch)
+
+    def step(self):
+        rclpy.spin_once(self._node, timeout_sec=0)
+        if time.monotonic() - self._last_state > self._state_timeout:
+            self._drag.invalidate()
+        try:
+            observed = webots_pose(self._translation.getSFVec3f(), self._rotation.getSFRotation())
+        except ValueError:
+            self._drag.invalidate()
+            return
+        # Webots may not expose a 3-D handle drag through getSelected().  A
+        # command still requires the marker fields themselves to change, so a
+        # mouse press elsewhere cannot start TCP motion.
+        dragging = self._mouse.getState().left
+        shown, target = self._drag.step(observed, dragging)
+        xyz, aa = to_webots(shown)
+        self._translation.setSFVec3f(xyz)
+        self._rotation.setSFRotation(aa)
+        if target is not None:
+            command = GizmoCommand()
+            command.epoch = self._drag.epoch
+            command.target.header.frame_id = self._frame
+            command.target.header.stamp = self._last_stamp
+            set_pose(command.target.pose, self._inverse_mount @ target)
+            self._publisher.publish(command)

src/iiwa_utils/iiwa_utils/tcp_gizmo_model.py

@@ -0,0 +1,94 @@
+"""Webots marker geometry and drag lifecycle, independent of ROS/Webots APIs."""
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def webots_pose(translation, axis_angle):
+    xyz = np.asarray(translation, dtype=float)
+    aa = np.asarray(axis_angle, dtype=float)
+    if xyz.shape != (3,) or aa.shape != (4,) or not np.isfinite(xyz).all() or not np.isfinite(aa).all():
+        raise ValueError('Pose must contain finite translation and axis-angle')
+    norm = np.linalg.norm(aa[:3])
+    if norm < 1e-12 and abs(aa[3]) > 1e-12:
+        raise ValueError('Rotation axis must be nonzero')
+    result = np.eye(4)
+    result[:3, :3] = Rotation.from_rotvec(aa[:3] * aa[3] / max(norm, 1e-12)).as_matrix()
+    result[:3, 3] = xyz
+    return result
+
+
+def to_webots(matrix):
+    vector = Rotation.from_matrix(matrix[:3, :3]).as_rotvec()
+    angle = np.linalg.norm(vector)
+    axis = vector / angle if angle > 1e-12 else np.array([0., 0., 1.])
+    return matrix[:3, 3].tolist(), [*axis.tolist(), float(angle)]
+
+
+def close_pose(left, right, linear=0.001, angular=0.01):
+    delta = left[:3, :3].T @ right[:3, :3]
+    return (np.linalg.norm(left[:3, 3] - right[:3, 3]) < linear
+            and Rotation.from_matrix(delta).magnitude() < angular)
+
+
+class DragTarget:
+    """Separate mouse edits from marker writes and never replay rejected targets."""
+
+    def __init__(self):
+        self.actual = None
+        self.epoch = None
+        self.enabled = False
+        self.shown = None
+        self.target = None
+        self._reset = True
+        self._wait_release = False
+        self._mouse_down = False
+        self._submitted = False
+
+    def update_feedback(self, actual, enabled, epoch):
+        if epoch != self.epoch or not enabled:
+            self._reset = True
+            self.target = None
+        self.actual = actual.copy()
+        self.enabled = enabled
+        self.epoch = epoch
+
+    def invalidate(self):
+        self.enabled = False
+        self.target = None
+        self._reset = True
+
+    def step(self, observed, mouse_down):
+        was_down = self._mouse_down
+        self._mouse_down = bool(mouse_down)
+        if self.actual is None:
+            self.shown = observed.copy()
+            return self.shown, None
+        if self._reset or not self.enabled:
+            self.target = None
+            self._submitted = False
+            self._wait_release = mouse_down
+            self._reset = False
+            self.shown = self.actual.copy()
+            return self.shown, None
+        if self._wait_release:
+            self._wait_release = mouse_down
+            self.shown = self.actual.copy()
+            return self.shown, None
+        if self._submitted:
+            # A committed goal owns the marker until controller completion (a
+            # new feedback epoch). Do not queue another mouse gesture behind it.
+            self.shown = self.target.copy()
+            return self.shown, None
+        # Supervisor field writes/readbacks are not mouse commands. In
+        # particular, startup synchronisation can read an older marker pose.
+        if mouse_down and self.shown is not None and not close_pose(observed, self.shown, 1e-6, 1e-5):
+            self.target = observed.copy()
+        command = None
+        if was_down and not mouse_down and self.target is not None:
+            if close_pose(self.target, self.actual):
+                self.target = None
+            else:
+                self._submitted = True
+                command = self.target.copy()
+        self.shown = (self.target if self.target is not None else self.actual).copy()
+        return self.shown, command

src/iiwa_utils/iiwa_utils/tcp_gizmo_spawner.py

@@ -0,0 +1,46 @@
+"""Spawn the nonphysical external supervisor marker without editing the world."""
+import rclpy
+from rclpy.node import Node
+from webots_ros2_msgs.srv import SpawnNodeFromString
+
+
+MARKER = '''DEF TCP_GIZMO Robot {
+  name "tcp_gizmo"
+  controller "<extern>"
+  supervisor TRUE
+  children [
+    Shape {
+      appearance PBRAppearance { baseColor 0.1 0.9 0.65 roughness 0.6 metalness 0 }
+      geometry Sphere { radius 0.018 subdivision 2 }
+    }
+    Shape {
+      appearance Appearance { material Material { emissiveColor 1 0.15 0.15 } }
+      geometry IndexedLineSet { coord Coordinate { point [0 0 0, 0.08 0 0] } coordIndex [0 1 -1] }
+    }
+    Shape {
+      appearance Appearance { material Material { emissiveColor 0.15 1 0.15 } }
+      geometry IndexedLineSet { coord Coordinate { point [0 0 0, 0 0.08 0] } coordIndex [0 1 -1] }
+    }
+    Shape {
+      appearance Appearance { material Material { emissiveColor 0.15 0.35 1 } }
+      geometry IndexedLineSet { coord Coordinate { point [0 0 0, 0 0 0.08] } coordIndex [0 1 -1] }
+    }
+  ]
+}'''
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = Node('tcp_gizmo_spawner')
+    try:
+        client = node.create_client(SpawnNodeFromString, '/Ros2Supervisor/spawn_node_from_string')
+        if not client.wait_for_service(timeout_sec=60.0):
+            raise RuntimeError('Webots supervisor unavailable; TCP marker was not spawned')
+        future = client.call_async(SpawnNodeFromString.Request(data=MARKER, check_fields=True))
+        rclpy.spin_until_future_complete(node, future, timeout_sec=30.0)
+        if not future.done() or not future.result().success:
+            raise RuntimeError('Webots could not spawn TCP marker')
+        node.get_logger().info('TCP marker spawned')
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

src/iiwa_utils/iiwa_utils/webots_camera.py

@@ -0,0 +1,114 @@
+"""Publish Webots camera frames without accumulating timestep rounding drift."""
+import array
+import math
+import sys
+
+import numpy as np
+import rclpy
+from rclpy.qos import QoSProfile, ReliabilityPolicy
+from builtin_interfaces.msg import Time
+from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField
+
+
+class FrameSchedule:
+    """Keep deadlines on the requested cadence, with at most one frame per step."""
+
+    def __init__(self, rate, now):
+        if not math.isfinite(rate) or rate <= 0:
+            raise ValueError('Camera update rate must be finite and positive')
+        self.period = 1.0 / rate
+        self.last_time = now
+        self.next_time = now + self.period
+
+    def due(self, now):
+        if now < self.last_time:
+            self.next_time = now + self.period
+        self.last_time = now
+        if now + 1e-9 < self.next_time:
+            return False
+        elapsed = max(0, math.floor((now - self.next_time + 1e-9) / self.period))
+        self.next_time += (elapsed + 1) * self.period
+        return True
+
+
+class WebotsCamera:
+    """One RGB or depth stream; preserves the stock driver's topic suffixes."""
+
+    def init(self, webots_node, properties):
+        self._robot = webots_node.robot
+        name = properties['device']
+        self._device = self._robot.getDevice(name)
+        if self._device is None:
+            raise ValueError(f'Camera device not found: {name}')
+        self._depth = properties.get('depth', 'false') == 'true'
+        rate = float(properties.get('updateRate', '30'))
+        self._schedule = FrameSchedule(rate, self._robot.getTime())
+        timestep = int(self._robot.getBasicTimeStep())
+        if timestep <= 0 or rate * timestep > 1000:
+            raise ValueError('Camera rate exceeds the Webots simulation step rate')
+        self._device.enable(timestep)
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        self._node = rclpy.create_node(f'{name}_publisher')
+        qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.RELIABLE)
+        topic = properties['topicName']
+        suffix = '/image' if self._depth else '/image_color'
+        self._publisher = self._node.create_publisher(Image, topic + suffix, qos)
+        self._info_publisher = self._node.create_publisher(CameraInfo, topic + '/camera_info', qos)
+        self._info = CameraInfo()
+        self._info.header.frame_id = properties['frameName']
+        self._info.width = self._device.getWidth()
+        self._info.height = self._device.getHeight()
+        focal = self._info.width / (2 * math.tan(self._device.getFov() / 2))
+        cx, cy = self._info.width / 2, self._info.height / 2
+        self._info.distortion_model = 'plumb_bob'
+        self._info.d = [0.0] * 5
+        self._info.k = [focal, 0.0, cx, 0.0, focal, cy, 0.0, 0.0, 1.0]
+        self._info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+        self._info.p = [focal, 0.0, cx, 0.0, 0.0, focal, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
+        if self._depth:
+            self._cloud_publisher = self._node.create_publisher(
+                PointCloud2, topic + '/point_cloud', qos)
+            u, v = np.meshgrid(np.arange(self._info.width), np.arange(self._info.height))
+            self._rays = np.stack(((u - cx) / focal, (v - cy) / focal,
+                                   np.ones_like(u)), axis=-1).astype(np.float32)
+
+    def step(self):
+        now = self._robot.getTime()
+        if not self._schedule.due(now):
+            return
+        if self._depth:
+            data = array.array('f', self._device.getRangeImage()).tobytes()
+        else:
+            data = self._device.getImage()
+        if data is None:
+            return
+        nanoseconds = round(now * 1_000_000_000)
+        self._info.header.stamp = Time(
+            sec=nanoseconds // 1_000_000_000, nanosec=nanoseconds % 1_000_000_000)
+        message = Image()
+        message.header = self._info.header
+        message.width, message.height = self._info.width, self._info.height
+        message.encoding = '32FC1' if self._depth else 'bgra8'
+        message.is_bigendian = int(sys.byteorder == 'big') if self._depth else 0
+        message.step = message.width * 4
+        message.data = data
+        self._publisher.publish(message)
+        self._info_publisher.publish(self._info)
+        if self._depth and self._cloud_publisher.get_subscription_count():
+            depth = np.frombuffer(data, dtype=np.float32).reshape(message.height, message.width)
+            # Optical coordinates: x right, y down, z forward, all in metres.
+            valid = np.isfinite(depth) & (depth > 0)
+            points = self._rays * np.where(valid, depth, 0.0)[..., None]
+            points[~valid] = np.nan
+            cloud = PointCloud2()
+            cloud.header = message.header
+            cloud.width, cloud.height = message.width, message.height
+            cloud.fields = [PointField(name=name, offset=4 * i,
+                                       datatype=PointField.FLOAT32, count=1)
+                            for i, name in enumerate(('x', 'y', 'z'))]
+            cloud.is_bigendian = bool(message.is_bigendian)
+            cloud.point_step, cloud.row_step = 12, message.width * 12
+            cloud.is_dense = bool(valid.all())
+            cloud.data = points.tobytes()
+            self._cloud_publisher.publish(cloud)

src/iiwa_utils/package.xml

@@ -11,7 +11,13 @@
   <depend>xacro</depend>
   <depend>ament_index_python</depend>
   <depend>webots_ros2_msgs</depend>
+  <exec_depend>webots_ros2_driver</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
   <depend>python3-yaml</depend>
+  <depend>iiwa_msgs</depend>
+  <depend>python3-numpy</depend>
+  <depend>python3-scipy</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/iiwa_utils/setup.py

@@ -12,6 +12,7 @@ setup(
         ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],
+    tests_require=['pytest'],
     zip_safe=True,
     maintainer='daniel',
     maintainer_email='grabardm@ml-dev.ru',
@@ -21,6 +22,7 @@ setup(
         'console_scripts': [
             "object_spawner = iiwa_utils.object_spawner:main",
             "camera_spawner = iiwa_utils.camera_spawner:main",
+            "tcp_gizmo_spawner = iiwa_utils.tcp_gizmo_spawner:main",
         ],
     },
 )

src/iiwa_utils/test/test_camera_timing.py

@@ -0,0 +1,33 @@
+"""Camera cadence must not halve when 30 Hz is sampled on 32 ms steps."""
+import pytest
+
+from iiwa_utils.webots_camera import FrameSchedule
+
+
+def test_30_hz_on_webots_32_ms_steps():
+    schedule = FrameSchedule(30, 0.0)
+    stamps = [tick * 0.032 for tick in range(1, 3126)
+              if schedule.due(tick * 0.032)]
+    assert len(stamps) == 3000
+    assert all(b > a for a, b in zip(stamps, stamps[1:]))
+    assert max(b - a for a, b in zip(stamps, stamps[1:])) <= 0.064001
+
+
+def test_delayed_step_does_not_publish_catchup_burst():
+    schedule = FrameSchedule(30, 0.0)
+    assert schedule.due(1.0)
+    assert not schedule.due(1.0)
+    assert not schedule.due(1.001)
+    assert schedule.due(1.034)
+
+
+def test_time_reset_restarts_schedule():
+    schedule = FrameSchedule(30, 10.0)
+    assert not schedule.due(0.0)
+    assert schedule.due(0.034)
+
+
+@pytest.mark.parametrize('rate', [0, -1, float('nan'), float('inf')])
+def test_invalid_rates_rejected(rate):
+    with pytest.raises(ValueError):
+        FrameSchedule(rate, 0.0)

src/iiwa_utils/test/test_gizmo_settings.py

@@ -0,0 +1,25 @@
+import dataclasses
+
+import pytest
+
+from iiwa_utils import setting_loader as settings
+
+
+def test_old_configuration_does_not_enable_new_input():
+    assert settings.parse_tcp_gizmo(None).enabled is False
+
+
+@pytest.mark.parametrize('value', ['false', 1, [], {'enabled': 'false'},
+                                  {'enabled': True, 'publish_period': 0},
+                                  {'max_linear_speed': float('nan')},
+                                  {'state_timeout': -1}])
+def test_bad_gizmo_settings_are_rejected(value):
+    with pytest.raises(settings.SettingsError):
+        settings.parse_tcp_gizmo(value)
+
+
+def test_partial_config_preserves_defaults():
+    cfg = settings.parse_tcp_gizmo({'enabled': True, 'max_linear_speed': 0.05})
+    assert cfg.enabled
+    assert cfg.max_linear_speed == 0.05
+    assert dataclasses.asdict(cfg)['max_angular_speed'] > 0

src/iiwa_utils/test/test_object_spawner_service.py

@@ -0,0 +1,30 @@
+"""Exercise our service requests with the installed Jazzy message schema."""
+from types import SimpleNamespace
+from unittest.mock import Mock
+
+import pytest
+
+from iiwa_utils import object_spawner
+
+
+@pytest.mark.parametrize('success,completed', [(True, True), (False, True), (True, False)])
+def test_service_schema_and_failure_stop(monkeypatch, success, completed):
+    node = Mock()
+    node.declare_parameter.side_effect = [SimpleNamespace(value='{}'),
+                                         SimpleNamespace(value='0 0 0'),
+                                         SimpleNamespace(value='0 0 1 0')]
+    monkeypatch.setattr(object_spawner, 'build_objects', lambda *args: ['Solid {}', 'Solid {}'])
+    monkeypatch.setattr(object_spawner.rclpy, 'spin_until_future_complete', lambda *a, **kw: None)
+    client = node.create_client.return_value
+    client.wait_for_service.return_value = True
+    future = client.call_async.return_value
+    future.done.return_value = completed
+    future.result.return_value = SimpleNamespace(success=success)
+    if success and completed:
+        object_spawner.spawn_objects(node)
+        assert client.call_async.call_count == 2
+        assert client.call_async.call_args.args[0].data == 'Solid {}'
+    else:
+        with pytest.raises(RuntimeError):
+            object_spawner.spawn_objects(node)
+        assert client.call_async.call_count == 1

src/iiwa_utils/test/test_object_spawning.py

@@ -0,0 +1,121 @@
+"""Configuration, model placement and textured asset regressions."""
+import math
+import random
+from pathlib import Path
+
+import pytest
+
+from iiwa_utils import object_scene as scene
+
+
+@pytest.fixture
+def model(tmp_path):
+    (tmp_path / 'texture.png').write_bytes(b'texture')
+    (tmp_path / 'hammer.mtl').write_text('newmtl wood\nmap_Kd texture.png\n')
+    path = tmp_path / 'hammer.obj'
+    path.write_text('mtllib hammer.mtl\nv 10 20 30\nv 12 24 36\nf 1 2 1\n')
+    return path
+
+
+def config(model, **updates):
+    raw = dict(enabled=True, count=5, models=[str(model)],
+               area=dict(x=[1, 1], y=[2, 2], z=[3, 3]))
+    raw.update(updates)
+    return scene.parse_objects(raw, lambda p: p)
+
+
+def test_default_disabled():
+    assert not scene.parse_objects(None, lambda p: p).enabled
+
+
+@pytest.mark.parametrize('updates', [
+    {'count': -1}, {'count': True}, {'count': 1.5}, {'enabled': 'true'},
+    {'models': []}, {'models': 'hammer.obj'},
+    {'area': {'x': [2, 1], 'y': [0, 1], 'z': [0, 1]}},
+    {'area': {'x': [0, math.nan], 'y': [0, 1], 'z': [0, 1]}},
+    {'area': {'x': [False, 1], 'y': [0, 1], 'z': [0, 1]}},
+])
+def test_reject_bad_configuration(model, updates):
+    with pytest.raises(ValueError):
+        config(model, **updates)
+
+
+def test_rotation_about_translated_robot():
+    assert scene.world_position((1, 2, 3), '10 20 30', f'0 0 2 {math.pi/2}') == pytest.approx((8, 21, 33))
+    assert scene.world_position((0, 1, 0), '0 0 0', f'1 0 0 {math.pi/2}') == pytest.approx((0, 0, 1))
+
+
+@pytest.mark.parametrize('translation,rotation', [('0 0', '0 0 1 0'), ('0 nan 0', '0 0 1 0'), ('0 0 0', '0 0 0 1')])
+def test_bad_robot_pose(translation, rotation):
+    with pytest.raises(ValueError):
+        scene.world_position((0, 0, 0), translation, rotation)
+
+
+def test_exact_count_repeats_and_centered_geometry(model):
+    nodes = scene.build_objects(config(model), '10 20 30', '0 0 1 0', random.Random(1))
+    assert len(nodes) == 5
+    assert len(set(nodes)) == 5
+    for node in nodes:
+        assert 'translation 11 22 33' in node
+        assert 'CadShape' in node and str(model) in node
+        assert 'translation -11 -22 -30' in node  # XY centered, bottom at local z=0
+        assert 'size 2 4 6' in node
+    assert scene.build_objects(config(model, count=0), '0 0 0', '0 0 1 0') == []
+
+
+def test_missing_texture_fails_before_spawning(model):
+    (model.parent / 'texture.png').unlink()
+    with pytest.raises(ValueError, match='texture'):
+        scene.build_objects(config(model), '0 0 0', '0 0 1 0')
+
+
+def test_repository_models_have_materials_and_textures():
+    root = Path(__file__).resolve().parents[2] / 'iiwa_description/objects/hammer'
+    models = sorted(root.glob('*.obj'))
+    assert len(models) == 5
+    for path in models:
+        asset = scene.load_obj(path)
+        assert all(size > 0 for size in asset.size)
+        assert asset.textures
+
+
+def test_per_model_rotation_and_serialization(model):
+    from dataclasses import asdict
+    cfg = config(model, models=[str(model), {'path': str(model), 'rotation_deg': [90, 0, 180]}])
+    assert cfg.models[0].rotation_deg == (0.0, 0.0, 0.0)
+    assert cfg.models[1].rotation_deg == (90.0, 0.0, 180.0)
+    assert scene.parse_objects(asdict(cfg), lambda p: p) == cfg
+
+
+@pytest.mark.parametrize('entry', [
+    {}, {'path': 12}, {'path': 'a.obj', 'rotation_deg': [0, 0]},
+    {'path': 'a.obj', 'rotation_deg': [True, 0, 0]},
+    {'path': 'a.obj', 'rotation_deg': [0, float('inf'), 0]},
+    {'path': 'a.obj', 'rotation_deg': '90 0 0'},
+    {'path': 'a.obj', 'rotaton_deg': [90, 0, 0]},
+])
+def test_invalid_model_orientation(model, entry):
+    with pytest.raises(ValueError):
+        config(model, models=[entry])
+
+
+def test_quarter_turn_repositions_geometry_and_collision(model):
+    cfg = config(model, count=1, models=[{'path': str(model), 'rotation_deg': [90, 0, 0]}])
+    node = scene.build_objects(cfg, '10 20 30', f'0 0 1 {math.pi/2}')[0]
+    # Anchor (1,2,3) rotates about the ROBOT, not about the model's X axis.
+    assert 'translation 8 21 33' in node
+    # Raw centre (11,22,33) -> (11,-33,22); rotated half-height = 2.
+    assert 'translation -11 33 -20' in node
+    assert 'translation 0 0 2' in node
+    # Rendering AND the original 2x4x6 collision box receive the model rotation.
+    assert node.count('rotation 1 0 0 1.57079632679') == 2
+    assert 'size 2 4 6' in node
+
+
+def test_rotation_order_xyz():
+    # Apply Rx(90), then Ry(0), then Rz(90): x -> y, y -> z, z -> x.
+    orient = scene.model_rotation([90, 0, 90])
+    encoded = ' '.join(map(str, orient))
+    assert scene.world_position((1, 0, 0), '0 0 0', encoded) == pytest.approx((0, 1, 0))
+    assert scene.world_position((0, 1, 0), '0 0 0', encoded) == pytest.approx((0, 0, 1))
+    assert scene.world_position((0, 0, 1), '0 0 0', encoded) == pytest.approx((1, 0, 0))

src/iiwa_utils/test/test_tcp_gizmo.py

@@ -0,0 +1,162 @@
+"""Regressions for coordinate conversion and rejected/stale drag targets."""
+import math
+
+import numpy as np
+import pytest
+
+from iiwa_utils import tcp_gizmo_model as model
+
+
+def pose(x=0.0):
+    result = np.eye(4)
+    result[0, 3] = x
+    return result
+
+
+def test_world_to_base_includes_rotated_robot_mount():
+    mount = model.webots_pose([1, 2, 3], [0, 0, 1, math.pi / 2])
+    target = model.webots_pose([1, 3, 3], [0, 0, 1, math.pi / 2])
+    np.testing.assert_allclose(np.linalg.inv(mount) @ target, pose(1), atol=1e-12)
+
+
+def test_invalid_axis_or_nonfinite_translation_rejected():
+    for xyz, rotation in [([float('nan'), 0, 0], [0, 0, 1, 0]),
+                          ([0, 0, 0], [0, 0, 0, 1])]:
+        with pytest.raises(ValueError):
+            model.webots_pose(xyz, rotation)
+
+
+def test_drag_retains_target_until_actual_tcp_reaches_it():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    shown, command = drag.step(pose(), False)
+    assert command is None
+    shown, command = drag.step(pose(0.1), True)
+    assert command is None
+    drag.update_feedback(pose(0.05), True, 1)
+    shown, command = drag.step(shown, False)
+    np.testing.assert_array_equal(command, pose(0.1))
+    # A release is a single committed goal, not a stream of new commands.
+    shown, command = drag.step(shown, False)
+    assert command is None
+    drag.update_feedback(pose(0.1), True, 1)
+    shown, command = drag.step(shown, False)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(0.1))
+
+
+def test_rejection_resets_to_actual_and_requires_release_before_new_drag():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.5), True)
+    drag.update_feedback(pose(0.01), True, 2)
+    shown, command = drag.step(pose(0.5), True)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(0.01))
+    drag.step(pose(0.8), True)
+    shown, command = drag.step(shown, False)
+    assert command is None
+    shown, command = drag.step(pose(0.02), True)
+    assert command is None
+    shown, command = drag.step(shown, False)
+    np.testing.assert_array_equal(command, pose(0.02))
+
+
+def test_high_priority_motion_discards_old_target_after_completion():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.5), True)
+    drag.update_feedback(pose(0.2), False, 2)
+    shown, command = drag.step(pose(0.5), False)
+    assert command is None
+    drag.update_feedback(pose(0.3), True, 2)
+    shown, command = drag.step(shown, False)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(0.3))
+
+
+def test_stale_feedback_cancels_target():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.5), True)
+    drag.invalidate()
+    shown, command = drag.step(pose(0.5), True)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose())
+
+
+@pytest.mark.parametrize('offset', [0.00001, 0.02, 0.5])
+def test_idle_marker_readback_difference_never_starts_motion(offset):
+    """Delayed Webots field reads or settling are not mouse input."""
+    drag = model.DragTarget()
+    drag.update_feedback(pose(1), True, 1)
+    drag.step(pose(), False)
+    shown, command = drag.step(pose(1 - offset), False)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(1))
+
+
+def test_idle_tcp_feedback_changes_do_not_turn_into_drag_commands():
+    drag = model.DragTarget()
+    previous = pose()
+    for index in range(100):
+        actual = pose(0.1 + index * 0.002)
+        drag.update_feedback(actual, True, 1)
+        shown, command = drag.step(previous, False)
+        assert command is None
+        np.testing.assert_array_equal(shown, actual)
+        previous = shown
+
+
+def test_readback_after_mouse_release_does_not_replace_active_target():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.1), True)
+    shown, command = drag.step(pose(0.08), False)
+    np.testing.assert_array_equal(command, pose(0.1))
+
+
+def test_idle_rotated_marker_readback_never_starts_motion():
+    drag = model.DragTarget()
+    actual = model.webots_pose([0.1, 0.2, 0.3], [1, 0, 0, 0.7])
+    drag.update_feedback(actual, True, 1)
+    drag.step(pose(), False)
+    shown, command = drag.step(pose(), False)
+    assert command is None
+    np.testing.assert_allclose(shown, actual)
+
+
+def test_no_motion_during_long_drag_and_only_final_pose_is_committed():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    for index in range(1, 100):
+        shown, command = drag.step(pose(index / 1000), True)
+        assert command is None
+    shown, command = drag.step(shown, False)
+    np.testing.assert_allclose(command, pose(.099))
+    for _ in range(100):
+        shown, command = drag.step(shown, False)
+        assert command is None
+
+
+def test_next_drag_waits_for_controller_completion_epoch():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(.1), True)
+    drag.step(pose(.1), False)
+    # Looking close enough is not proof that the controller has finished.
+    drag.update_feedback(pose(.1), True, 1)
+    shown, command = drag.step(pose(.2), True)
+    assert command is None
+    drag.update_feedback(pose(.1), True, 2)
+    drag.step(shown, False)
+    shown, command = drag.step(pose(.2), True)
+    assert command is None
+    shown, command = drag.step(shown, False)
+    np.testing.assert_allclose(command, pose(.2))

src/iiwa_utils/test/test_tcp_gizmo_plugin.py

@@ -0,0 +1,112 @@
+"""Exercise the real plugin step with Webots field/mouse adapters."""
+import copy
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from iiwa_msgs.msg import GizmoState
+from iiwa_utils import tcp_gizmo as plugin
+from iiwa_utils.tcp_gizmo_model import DragTarget
+
+
+class Field:
+    def __init__(self, value):
+        self.value = value
+
+    def getSFVec3f(self):
+        return self.value
+
+    getSFRotation = getSFVec3f
+
+    def setSFVec3f(self, value):
+        self.value = value
+
+    setSFRotation = setSFVec3f
+
+
+def make_plugin(monkeypatch):
+    instance = plugin.WebotsTcpGizmo()
+    instance._node = None
+    instance._frame = 'base_link'
+    instance._mount = np.eye(4)
+    instance._inverse_mount = np.eye(4)
+    instance._state_timeout = 0.5
+    instance._drag = DragTarget()
+    instance._last_state = 0
+    instance._last_stamp = None
+    instance._translation = Field([0, 0, 0])
+    instance._rotation = Field([0, 0, 1, 0])
+    mouse = SimpleNamespace(left=False)
+    instance._mouse = SimpleNamespace(getState=lambda: mouse)
+    marker = SimpleNamespace(getId=lambda: 42, getParentNode=lambda: None)
+    selection = SimpleNamespace(node=marker)
+    instance._marker_id = 42
+    instance._robot = SimpleNamespace(getSelected=lambda: selection.node)
+    commands = []
+    instance._publisher = SimpleNamespace(publish=lambda msg: commands.append(copy.deepcopy(msg)))
+    monkeypatch.setattr(plugin.rclpy, 'spin_once', lambda *args, **kwargs: None)
+    return instance, mouse, selection, commands
+
+
+def feedback(instance, epoch=1):
+    state = GizmoState()
+    state.actual.header.frame_id = 'base_link'
+    state.actual.pose.position.x = 0.1
+    state.actual.pose.orientation.w = 1.0
+    state.enabled = True
+    state.epoch = epoch
+    instance._feedback(state)
+
+
+def test_plugin_publishes_nothing_at_startup_even_with_delayed_fields(monkeypatch):
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    for _ in range(100):
+        feedback(instance)
+        instance._translation.value = [0, 0, 0]
+        instance.step()
+    assert commands == []
+    assert instance._translation.value == [0.1, 0.0, 0.0]
+
+
+def test_plugin_mouse_drag_sends_command_and_rejection_stops_it(monkeypatch):
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    feedback(instance)
+    instance.step()
+    mouse.left = True
+    instance._translation.value = [0.2, 0, 0]
+    instance.step()
+    assert len(commands) == 0
+    mouse.left = False
+    instance.step()
+    assert len(commands) == 1
+    assert commands[0].target.pose.position.x == pytest.approx(0.2)
+    feedback(instance, epoch=2)
+    instance.step()
+    assert len(commands) == 1
+    assert instance._translation.value == [0.1, 0.0, 0.0]
+
+
+def test_drag_still_commands_when_webots_has_no_selected_node(monkeypatch):
+    """The 3-D handle may not be represented as a selected scene-tree node."""
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    feedback(instance)
+    instance.step()
+    mouse.left = True
+    selection.node = None
+    instance._translation.value = [0.2, 0, 0]
+    instance.step()
+    assert len(commands) == 0
+    mouse.left = False
+    instance.step()
+    assert len(commands) == 1
+
+
+def test_clicking_other_object_does_not_start_tcp_motion(monkeypatch):
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    feedback(instance)
+    instance.step()
+    mouse.left = True
+    selection.node = None
+    instance.step()
+    assert commands == []

src/iiwa_utils/test/test_tool_tcp.py

@@ -0,0 +1,36 @@
+"""TCP must be at the active tool's working end in the visual model."""
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+from iiwa_utils.tool_manager import _write_xacro, load_registry
+
+ROOT = Path(__file__).resolve().parents[2]
+
+
+def test_patron_tcp_is_on_the_distal_face_of_its_mesh():
+    tool = ET.parse(ROOT / 'iiwa_description/urdf/tools/patron.xacro').getroot()
+    joint = tool.find("joint[@name='patron_tcp']")
+    assert joint.find('parent').attrib['link'] == 'patron'
+    assert joint.find('child').attrib['link'] == 'tcp'
+    tcp = np.fromstring(joint.find('origin').attrib['xyz'], sep=' ')
+    mesh = ET.parse(ROOT / 'iiwa_description/resource/meshes/tools/patron.dae').getroot()
+    ns = {'c': 'http://www.collada.org/2005/11/COLLADASchema'}
+    assert float(mesh.find('c:asset/c:unit', ns).attrib['meter']) == 1.
+    geometry = mesh.find('.//c:mesh', ns)
+    ref = geometry.find("c:vertices/c:input[@semantic='POSITION']", ns).attrib['source'][1:]
+    values = geometry.find(f"c:source[@id='{ref}']/c:float_array", ns)
+    points = np.fromstring(values.text, sep=' ').reshape(-1, 3)
+    np.testing.assert_allclose(tcp, [0., 0., points[:, 2].max()], atol=1e-6)
+
+
+def test_without_tool_tcp_coincides_with_flange(tmp_path):
+    registry = load_registry(ROOT / 'iiwa_config/config/tools.yaml')
+    assert registry['none']['tcp_link'] == 'tcp'
+    output = tmp_path / 'tool.xacro'
+    _write_xacro(registry['none'], output)
+    joint = ET.parse(output).getroot().find("joint[@name='link_ee_to_tcp']")
+    assert joint.find('parent').attrib['link'] == 'link_ee'
+    assert joint.find('child').attrib['link'] == 'tcp'
+    np.testing.assert_array_equal(np.fromstring(joint.find('origin').attrib['xyz'], sep=' '), [0, 0, 0])

src/iiwa_utils/test/test_webots_camera.py

@@ -0,0 +1,66 @@
+"""Check published pixels, units and calibration with a small device adapter."""
+import array
+import copy
+import math
+from types import SimpleNamespace
+
+import pytest
+import numpy as np
+
+from iiwa_utils import webots_camera as camera
+
+
+@pytest.mark.parametrize('depth', [False, True])
+def test_messages_match_device_and_share_timestamp(monkeypatch, depth):
+    messages = {}
+    now = [0.0]
+    device = SimpleNamespace(
+        enable=lambda period: None,
+        getWidth=lambda: 2, getHeight=lambda: 1,
+        getFov=lambda: math.pi / 2,
+        getImage=lambda: bytes([1, 2, 3, 255, 4, 5, 6, 255]),
+        getRangeImage=lambda: [1.25, float('inf')],
+    )
+    robot = SimpleNamespace(getDevice=lambda name: device,
+                            getBasicTimeStep=lambda: 32, getTime=lambda: now[0])
+
+    def publisher(message_type, topic, qos):
+        messages[topic] = []
+        return SimpleNamespace(publish=lambda msg: messages[topic].append(copy.deepcopy(msg)),
+                               get_subscription_count=lambda: 1)
+
+    monkeypatch.setattr(camera.rclpy, 'ok', lambda: True)
+    monkeypatch.setattr(camera.rclpy, 'create_node',
+                        lambda name: SimpleNamespace(create_publisher=publisher))
+    plugin = camera.WebotsCamera()
+    plugin.init(SimpleNamespace(robot=robot), dict(
+        device='test_camera', depth=str(depth).lower(), updateRate='30',
+        topicName='/test', frameName='test_optical_frame'))
+    for tick in range(1, 101):
+        now[0] = tick * 0.032
+        plugin.step()
+    images = messages['/test/image' if depth else '/test/image_color']
+    infos = messages['/test/camera_info']
+    assert len(images) == len(infos) == 96
+    image, info = images[-1], infos[-1]
+    assert image.header == info.header
+    assert image.header.frame_id == 'test_optical_frame'
+    assert (image.width, image.height, image.step) == (2, 1, 8)
+    assert image.header.stamp.sec == 3
+    assert image.header.stamp.nanosec == 200_000_000
+    assert info.k[0] == pytest.approx(1.0)
+    assert info.k[2] == 1.0
+    assert info.k[5] == 0.5
+    if depth:
+        assert image.encoding == '32FC1'
+        values = array.array('f', image.data.tobytes())
+        assert values[0] == 1.25
+        assert math.isinf(values[1])
+        cloud = messages['/test/point_cloud'][-1]
+        assert cloud.header == image.header
+        xyz = np.frombuffer(cloud.data, dtype=np.float32).reshape(-1, 3)
+        np.testing.assert_allclose(xyz[0], [-1.25, -0.625, 1.25])
+        assert np.isnan(xyz[1]).all()
+    else:
+        assert image.encoding == 'bgra8'
+        assert image.data.tobytes() == device.getImage()