From 392036caf879c4151da06e74076237142e4186b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 16 Sep 2026 09:59:58 +1000 Subject: [PATCH] Update control move --- cobot/commands/update.py | 48 ++++---- cobot/test/test_update.py | 72 +++++++++++ .../docs/troubleshooting/open-issues.en.md | 35 ------ .../docs/troubleshooting/open-issues.md | 37 ------ .../launch/supported/controllers.launch.py | 12 ++ src/iiwa_bringup/package.xml | 2 +- src/iiwa_controller/CMakeLists.txt | 7 +- src/iiwa_controller/README.en.md | 25 +++- src/iiwa_controller/README.md | 26 +++- .../include/iiwa_controller/FRIClient.h | 2 + .../include/iiwa_controller/FRICycleGate.hpp | 67 +++++++++++ .../iiwa_controller/IIWAHardwareInterface.hpp | 6 +- src/iiwa_controller/src/FRIClient.cpp | 28 +++-- .../src/IIWAHardwareInterface.cpp | 59 +++++++-- src/iiwa_controller/test/fake_fri_server.cpp | 112 ++++++++++++++++++ src/iiwa_controller/test/loopback_bind.cpp | 20 ++++ src/iiwa_controller/test/smoke_fri.py | 83 +++++++++++++ src/iiwa_controller/test/test_fri_client.cpp | 88 ++++++++++++++ src/iiwa_description/urdf/iiwa7.urdf.xacro | 4 +- 19 files changed, 603 insertions(+), 130 deletions(-) create mode 100644 cobot/test/test_update.py delete mode 100644 doc/lwc-doc/docs/troubleshooting/open-issues.en.md delete mode 100644 doc/lwc-doc/docs/troubleshooting/open-issues.md create mode 100644 src/iiwa_controller/include/iiwa_controller/FRICycleGate.hpp create mode 100644 src/iiwa_controller/test/fake_fri_server.cpp create mode 100644 src/iiwa_controller/test/loopback_bind.cpp create mode 100644 src/iiwa_controller/test/smoke_fri.py diff --git a/cobot/commands/update.py b/cobot/commands/update.py index 00410db..1ebf66e 100644 --- a/cobot/commands/update.py +++ b/cobot/commands/update.py @@ -4,7 +4,7 @@ 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 @@ -18,7 +18,7 @@ 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. @@ -32,42 +32,44 @@ 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) - p.raw(f"\n[bold]{behind} новых коммит(ов):[/bold]") - for line in _git("log", f"HEAD..origin/{branch}", "--oneline").splitlines(): - p.log(line) - - 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 + p.set(30, "Применение изменений...") + rc = process.stream(["git", "pull", "origin", branch], + cwd=str(_PROJECT_DIR), on_line=p.log) + if rc != 0: + done(False, f"git pull завершился с кодом {rc}") + return 1 p.set(80) 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): + if rc == 0: p.raw("[green]✓[/green] cobot переустановлен") + p.set(100, "Готово") else: - p.raw("[yellow]Предупреждение:[/yellow] переустановка не удалась") - p.set(100, "Готово") - except subprocess.CalledProcessError as exc: + ok = False + fail_msg = (f"Установка CLI завершилась с кодом {rc}. " + "Исходники обновлены, но установка не завершена. " + "Сохраните вывод ошибки выше и повторите cobot update.") + except (subprocess.CalledProcessError, OSError) as exc: ok, fail_msg = False, str(exc) done(ok, "Проект обновлён" if ok else fail_msg) + return 0 if ok else 1 def register(subparsers: argparse._SubParsersAction) -> None: @@ -76,4 +78,4 @@ def register(subparsers: argparse._SubParsersAction) -> None: def run(args: argparse.Namespace) -> None: - _update() + raise SystemExit(_update()) diff --git a/cobot/test/test_update.py b/cobot/test/test_update.py new file mode 100644 index 0000000..67f0c83 --- /dev/null +++ b/cobot/test/test_update.py @@ -0,0 +1,72 @@ +"""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("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) + self.assertIn("cobot update", 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, output, _ = self.run_update( + iter([0, 0, FileNotFoundError("uv not found")])) + self.assertNotEqual(code, 0) + self.assertIn("uv not found", output) + + 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() diff --git a/doc/lwc-doc/docs/troubleshooting/open-issues.en.md b/doc/lwc-doc/docs/troubleshooting/open-issues.en.md deleted file mode 100644 index c006ba9..0000000 --- a/doc/lwc-doc/docs/troubleshooting/open-issues.en.md +++ /dev/null @@ -1,35 +0,0 @@ -# Open issues after workstation test - -Recorded: 2026-09-15 - -## 1. `cobot update` error - -On another computer, `cobot update` failed while installing Python libraries. -The full output was not captured, so the cause is unknown. The next -investigation needs the Python version, distribution/environment, and the -complete log starting at the first error. - -## 2. Drive knocking in the `controller` branch - -While testing branch `controller` on the real KUKA iiwa, motion to known target -points became jerky and the drives audibly knocked/struck internally. A beep -was also heard at the end of the motion. - -Observed conditions: - -- motion to `work`, speed `0.1`: knocking continued throughout the motion and a - beep occurred at the end; -- motion to `home`, speed `0.5`: drive knocking was also present; -- branch `dev` did not show knocking under these conditions; it appeared only - rarely in earlier tests; -- earlier position plots showed isolated short-lived outliers after which the - drive could not reach the target in time. - -The issue may be related to the FRI/ros2_control implementation in -`controller`. Investigation should check the seven-value position format and -order, FRI timestamps and rate, `COMMANDING_WAIT`/`COMMANDING_ACTIVE` -synchronization, rate limiting, and the source of short-lived outliers. Until -investigated, repeat real-robot tests only with operator supervision and a -ready emergency-stop procedure. - -No fix is intentionally included in this record. diff --git a/doc/lwc-doc/docs/troubleshooting/open-issues.md b/doc/lwc-doc/docs/troubleshooting/open-issues.md deleted file mode 100644 index a603876..0000000 --- a/doc/lwc-doc/docs/troubleshooting/open-issues.md +++ /dev/null @@ -1,37 +0,0 @@ -# Открытые проблемы после испытания на рабочем ПК - -Дата фиксации: 15.09.2026 - -## 1. Ошибка `cobot update` - -На другом компьютере `cobot update` завершился с ошибкой во время установки -Python-библиотек. Полный вывод команды пока не зафиксирован, поэтому причина -не определена. Нужны версия Python, окружение/дистрибутив и полный лог начиная -с первой строки ошибки. - -## 2. Стук приводов в ветке `controller` - -При испытании ветки `controller` на реальном KUKA iiwa в режиме перемещения в -заранее известные точки наблюдалось дерганое движение со слышимым стуком -приводов (привод движется и ударяет/бьется внутри). В конце перемещения также -возникал писк. - -Наблюдавшиеся условия: - -- перемещение в `work`, скорость `0.1`: стук продолжался на всем перемещении, - в конце был писк; -- перемещение в `home`, скорость `0.5`: также наблюдался стук приводов; -- в ветке `dev` такие стуки не наблюдались в этих условиях; иногда они - возникали редко; -- ранее на графиках положения были замечены отдельные кратковременные - выбросы, после которых привод не успевал прийти в целевую точку. - -Проблема считается потенциально связанной с реализацией FRI/ros2_control в -ветке `controller`. Возможные направления проверки: формат и порядок семи -позиционных значений, временные метки и частота FRI, синхронизация -`COMMANDING_WAIT`/`COMMANDING_ACTIVE`, ограничение скорости и происхождение -кратковременных выбросов. До отдельного расследования повторные испытания на -реальном роботе выполнять только под наблюдением оператора и с готовой -аварийной остановкой. - -Решение намеренно не вносилось в рамках этой фиксации. diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 4c1a77b..438f049 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/controllers.launch.py @@ -5,6 +5,9 @@ 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 @@ -31,6 +34,7 @@ def _setup_controllers(context, *args, **kwargs): else: 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) robot_description = converter.load_robot_description( model_path=description, @@ -76,6 +80,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", @@ -84,6 +95,7 @@ def _setup_controllers(context, *args, **kwargs): {"robot_description": robot_description}, controller_path, {"update_rate": update_rate}, + {"hardware_synchronization.expect_blocking_read_write": True}, ], ) diff --git a/src/iiwa_bringup/package.xml b/src/iiwa_bringup/package.xml index 7fc6399..01f4001 100644 --- a/src/iiwa_bringup/package.xml +++ b/src/iiwa_bringup/package.xml @@ -26,7 +26,7 @@ rviz2 - controller_manager + controller_manager joint_state_broadcaster joint_trajectory_controller forward_command_controller diff --git a/src/iiwa_controller/CMakeLists.txt b/src/iiwa_controller/CMakeLists.txt index b668066..3b45727 100644 --- a/src/iiwa_controller/CMakeLists.txt +++ b/src/iiwa_controller/CMakeLists.txt @@ -94,6 +94,11 @@ 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) @@ -101,4 +106,4 @@ if(BUILD_TESTING) target_link_libraries(test_hardware ${PROJECT_NAME} fri_client_sdk hardware_interface::hardware_interface) endif() -ament_package() \ No newline at end of file +ament_package() diff --git a/src/iiwa_controller/README.en.md b/src/iiwa_controller/README.en.md index cb8fa5c..2907b2c 100644 --- a/src/iiwa_controller/README.en.md +++ b/src/iiwa_controller/README.en.md @@ -36,13 +36,21 @@ as described in [Writing a Hardware Component](https://control.ros.org/jazzy/doc - Hardware IP/port reach the URDF from `robot.ip`/`robot.port`; project defaults remain `192.170.10.2:30200`. Match `robot.fri_cycle_ms` to the Sunrise period. -One thread owns SDK `step()`. Cyclic exchange uses `try_lock`, so a busy mutex -never stalls either cycle. Readers retain a complete previous snapshot with a -freshness check; writers retry next cycle. Startup allows 15 seconds for the +One worker owns SDK `step()`. Each step is followed by exactly one +`read → JTC update → write` exchange. `read()` waits for a fresh snapshot; +`write()` releases the next SDK step. Waits are bounded to 100 ms. The computed +command is sent in the next SDK step, a one-cycle pipeline without EMA or a +queue of skipped points. Network I/O stays in the worker. The real-hardware +launch requires Jazzy Controller Manager **4.48.0 or later** and enables +`hardware_synchronization.expect_blocking_read_write`, using its +[hardware-paced control loop](https://github.com/ros-controls/ros2_control/blob/jazzy/controller_manager/src/ros2_control_node.cpp) +without an independent timer. `robot.fri_cycle_ms` is checked against Sunrise +telemetry. Startup allows 15 seconds for the first UDP packet. After that, a failed `step()` is terminal. Activation requires fresh telemetry, at least MONITORING_READY, and GOOD/EXCELLENT quality. -Missing telemetry/commands for 100 ms, non-increasing timestamps, invalid +Missing telemetry/commands for 100 ms, timestamp gaps in COMMANDING_ACTIVE, +mismatched sample periods, non-increasing timestamps, invalid telemetry, unexpected commanding mode, a safety stop, inactive drives or poor commanding quality stop command generation and surface ERROR to ROS. Leaving COMMANDING_ACTIVE requires explicit lifecycle recovery and a new Sunrise @@ -78,9 +86,16 @@ a separate hardware plugin; it does not validate UDP/FRI. No physical robot was run for this revision. Hardware testing requires the networking, limits, tool/load, mode, operator supervision and stop checks in AGENTS.md. -Revision results: all five affected ROS packages built; 14 C++ cases in two +Initial revision results: all five affected ROS packages built; 14 C++ cases in two CTest checks passed. The local smoke script loaded the actual URDF with our plugin in simulation mode, activated JSB/JTC and completed a +0.02 rad joint-1 trajectory with SUCCEEDED. On SIGINT, Controller Manager's `pal_statistics` reported an invalid shutdown context; hardware lifecycle shutdown succeeded. Webots and physical FRI were not run. + +2026-09-16 correction: reproduced a doubled 0.001 → 0.002 rad step when +independent cycles overwrite a command. Added synchronization, overwrite, +stop/timeout, sample-period and packet-gap tests. Synchronization prevents +inter-thread point loss; it does not by itself guarantee freedom from OS or +network delays. Elimination of physical drive knocking remains unverified; +the original hardware-test logs were not retained. diff --git a/src/iiwa_controller/README.md b/src/iiwa_controller/README.md index b7c5a51..a6e7402 100644 --- a/src/iiwa_controller/README.md +++ b/src/iiwa_controller/README.md @@ -40,14 +40,21 @@ commanded position, `waitForCommand()` — IPO; в обоих commanding-сос исходные значения проекта — `192.170.10.2:30200`. Период manager задаётся `robot.fri_cycle_ms`; его нужно согласовать с выбранным периодом Sunrise. -Один рабочий поток владеет `ClientApplication::step()`. Межпоточный обмен -в цикле использует `try_lock`: при конкуренции цикл не ждёт mutex. `read()` -использует последний целый снимок с проверкой давности, `write()` повторяет -передачу в следующем цикле. До первого UDP-пакета допускается ожидание Sunrise +Один рабочий поток владеет `ClientApplication::step()`. После каждого шага он +ожидает ровно один цикл `read → JTC update → write`; `read()` ожидает новый +снимок, `write()` разрешает следующий шаг SDK. Ожидания ограничены 100 мс. +Команда этого расчёта отправляется в следующем шаге SDK: задержка — один цикл, +без EMA и очереди пропущенных точек. Сетевой ввод-вывод остаётся в рабочем потоке. +Launch включает `hardware_synchronization.expect_blocking_read_write` и требует +Controller Manager **4.48.0 или новее** в Jazzy. Это режим +[синхронизации цикла с оборудованием](https://github.com/ros-controls/ros2_control/blob/jazzy/controller_manager/src/ros2_control_node.cpp), +без второго независимого таймера. Период из `robot.fri_cycle_ms` проверяется +по телеметрии Sunrise. До первого UDP-пакета допускается ожидание Sunrise до 15 с; после первого пакета ошибка `step()` завершает поток. Активация требует свежей сессии как минимум MONITORING_READY и качества GOOD/EXCELLENT. -При отсутствии пакетов/команд 100 мс, нерастущем timestamp, нечисловой +При отсутствии пакетов/команд 100 мс, пропуске timestamp в COMMANDING_ACTIVE, +несовпадении периода, нерастущем timestamp, нечисловой телеметрии, недопустимом режиме, safety stop, неактивных приводах или плохом качестве в commanding приложение прекращает выдачу команд; ROS получает ERROR. Выход из COMMANDING_ACTIVE требует явного восстановления lifecycle и новой @@ -87,9 +94,16 @@ simulator**; проверить, что `joint_state_broadcaster` и `iiwa_arm_c Для аппаратной проверки нужны оператор, свободная рабочая зона, проверенные сеть, tool/load data, режим, ограничения и процедура останова из AGENTS.md. -Результаты ревизии: сборка пяти затронутых ROS-пакетов прошла; 14 C++-сценариев +Результаты исходной ревизии: сборка пяти затронутых ROS-пакетов прошла; 14 C++-сценариев в двух CTest-проверках прошли. `smoke_jtc.py` загрузил настоящий URDF с локальной имитацией нашего плагина, активировал JSB/JTC и завершил траекторию +0.02 рад сустава 1 со статусом SUCCEEDED. При SIGINT библиотека `pal_statistics` Controller Manager вывела сообщение об остановленном контексте; lifecycle аппаратного плагина завершился успешно. Webots и физический FRI не запускались. + +Исправление 16.09.2026: воспроизведено удвоение шага 0,001 → 0,002 рад при +перезаписи команды независимыми циклами. Добавлены тесты синхронизации, +запрета перезаписи, остановки/таймаута, периода и пропуска пакетов. Синхронизация +устраняет потерю точек между потоками, но сама по себе не гарантирует отсутствие +задержек планировщика ОС или сети. Исчезновение стука на физическом роботе пока +не подтверждено; исходные журналы испытания не сохранились. diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index 1f7798a..7f5287d 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -48,6 +48,7 @@ public: // 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; @@ -64,6 +65,7 @@ private: Joints lower_, upper_, max_velocity_; bool initialized_{false}; bool requested_{false}; + 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_ diff --git a/src/iiwa_controller/include/iiwa_controller/FRICycleGate.hpp b/src/iiwa_controller/include/iiwa_controller/FRICycleGate.hpp new file mode 100644 index 0000000..2341d7a --- /dev/null +++ b/src/iiwa_controller/include/iiwa_controller/FRICycleGate.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include +#include + +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 lock(mutex_); + stopped_ = available_ = acquired_ = completed_ = false; + } + void stop() + { + std::lock_guard lock(mutex_); + stopped_ = true; + cv_.notify_all(); + } + bool publishAndWait() + { + std::unique_lock 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 lock(mutex_); + return cv_.wait_for(lock, timeout, [&] {return stopped_ || available_;}) && !stopped_; + } + bool acquire() + { + std::unique_lock 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 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 diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 5cc7df1..3b71043 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -20,6 +20,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "iiwa_controller/FRIClient.h" +#include "iiwa_controller/FRICycleGate.hpp" namespace iiwa_controller { @@ -59,6 +60,7 @@ private: // Параметры из секции в URDF std::string robot_ip_; int fri_port_{30200}; + int fri_cycle_ms_{5}; bool simulate_{false}; // Объекты FRI SDK std::unique_ptr fri_client_; @@ -66,8 +68,10 @@ private: std::unique_ptr 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 fri_running_{false}; void friThreadFunc(); void stopFRI(); diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index 056c682..d62748b 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/src/iiwa_controller/src/FRIClient.cpp @@ -53,6 +53,18 @@ void FRIClient::captureData(bool commanding) 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 differs from robot.fri_cycle_ms"); + } + if (current_.valid && current_.session == COMMANDING_ACTIVE) { + const double elapsed = static_cast(sec - current_.time_stamp_sec) + + (static_cast(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]) || @@ -117,11 +129,12 @@ void FRIClient::command() initialized_ = true; } { - std::unique_lock lock(command_mutex_, std::try_to_lock); - if (lock.owns_lock() && requested_ && requested_at_ > last_command_at_) { + std::lock_guard 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)) { @@ -156,12 +169,13 @@ void FRIClient::setTargetJointPositions(const Joints & q) throw std::invalid_argument("Invalid FRI position command"); } } - std::unique_lock lock(command_mutex_, std::try_to_lock); - if (lock.owns_lock()) { - requested_pos_ = q; - requested_at_ = std::chrono::steady_clock::now(); - requested_ = true; + std::lock_guard lock(command_mutex_); + if (requested_) { + throw std::logic_error("Previous FRI command has not been consumed"); } + requested_pos_ = q; + requested_at_ = std::chrono::steady_clock::now(); + requested_ = true; } IIWAStateSnapshot FRIClient::getStateSnapshot() const diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index d6282ad..7f4579a 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -82,6 +82,11 @@ CallbackReturn IIWAHardwareInterface::on_init( 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 simulation = getParam(info, "simulate", "false"); if (simulation != "true" && simulation != "false") { throw std::invalid_argument("simulate must be true or false"); @@ -172,6 +177,7 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State releaseFRI(); fri_client_ = std::make_unique(); fri_client_->setLimits(lower_, upper_, max_velocity_); + fri_client_->setExpectedSampleTime(fri_cycle_ms_ * 0.001); connection_ = std::make_unique(fri_running_); app_ = std::make_unique(*connection_, *fri_client_); return CallbackReturn::SUCCESS; @@ -201,15 +207,11 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State } fri_fault_.store(false); fri_running_.store(true); + cycle_gate_.reset(); fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this); - const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15); - while (fri_running_.load() && std::chrono::steady_clock::now() < deadline) { - last_snapshot_ = fri_client_->getStateSnapshot(); - if (last_snapshot_.valid && last_snapshot_.quality >= KUKA::FRI::GOOD && - last_snapshot_.session >= KUKA::FRI::MONITORING_READY) {break;} - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - } - if (fri_fault_.load() || !last_snapshot_.valid || + 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)) @@ -246,6 +248,7 @@ 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();} @@ -288,6 +291,8 @@ CallbackReturn IIWAHardwareInterface::on_error(const rclcpp_lifecycle::State & s void IIWAHardwareInterface::friThreadFunc() { try { + bool synchronized = false; + const auto startup_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15); while (fri_running_.load()) { if (!app_->step()) { throw std::runtime_error("FRI step failed"); @@ -295,6 +300,17 @@ void IIWAHardwareInterface::friThreadFunc() 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"); + } } } catch (const KUKA::FRI::FRIException & e) { fri_fault_.store(true); @@ -307,6 +323,7 @@ void IIWAHardwareInterface::friThreadFunc() RCLCPP_ERROR(rclcpp::get_logger(LOG), "Unknown FRI failure"); } fri_running_.store(false); + cycle_gate_.stop(); } // ── compute_velocity_ ────────────────────────────────────────────────────────── @@ -369,7 +386,13 @@ hardware_interface::return_type IIWAHardwareInterface::read( if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) { return hardware_interface::return_type::ERROR; } - fri_client_->tryGetStateSnapshot(last_snapshot_); + 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)) @@ -398,11 +421,15 @@ hardware_interface::return_type IIWAHardwareInterface::write( if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) { return hardware_interface::return_type::ERROR; } - if (!fri_client_->isCommandingActive()) {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 pos_cmd{}; for (size_t i = 0; i < N_JOINTS; ++i) { if (!get_command(h_cmd_pos_[i], pos_cmd[i], false)) { - return hardware_interface::return_type::OK; // retry next cycle; watchdog stays armed + 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); @@ -410,8 +437,14 @@ hardware_interface::return_type IIWAHardwareInterface::write( return hardware_interface::return_type::ERROR; } } - fri_client_->setTargetJointPositions(pos_cmd); - return hardware_interface::return_type::OK; + try { + fri_client_->setTargetJointPositions(pos_cmd); + } 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 diff --git a/src/iiwa_controller/test/fake_fri_server.cpp b/src/iiwa_controller/test/fake_fri_server.cpp new file mode 100644 index 0000000..5d50118 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#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(&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 q{}, zero{}, received{}; + std::array 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(&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 = ⌖ + 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(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; +} diff --git a/src/iiwa_controller/test/loopback_bind.cpp b/src/iiwa_controller/test/loopback_bind.cpp new file mode 100644 index 0000000..1f95734 --- /dev/null +++ b/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 +#include +#include + +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(dlsym(RTLD_NEXT, "bind")); + if (address->sa_family == AF_INET && length == sizeof(sockaddr_in)) { + auto local = *reinterpret_cast(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(&local), sizeof(local)); + } + } + return real_bind(fd, address, length); +} diff --git a/src/iiwa_controller/test/smoke_fri.py b/src/iiwa_controller/test/smoke_fri.py new file mode 100644 index 0000000..8c97fe7 --- /dev/null +++ b/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'] = 'false' +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 'false' in xml +assert '127.0.0.2' in xml +assert '30283' 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) diff --git a/src/iiwa_controller/test/test_fri_client.cpp b/src/iiwa_controller/test/test_fri_client.cpp index 76dfd50..be42daf 100644 --- a/src/iiwa_controller/test/test_fri_client.cpp +++ b/src/iiwa_controller/test/test_fri_client.cpp @@ -5,6 +5,9 @@ #include #include #include "iiwa_controller/FRIClient.h" +#include "iiwa_controller/FRICycleGate.hpp" +#include +#include #include "friClientData.h" #include "pb_frimessages_callbacks.h" @@ -143,6 +146,18 @@ TEST(FRIClient, DuplicateTimestampIsNotFreshTelemetry) 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; @@ -172,3 +187,76 @@ TEST(FRIClient, StalledRosCommandStreamStopsBeforeSending) 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.401); +} + +TEST(FRIClient, SynchronizedCyclesPreserveEveryPointDespiteSchedulingJitter) +{ + ClientFixture client; + client.cycle(COMMANDING_WAIT); + iiwa_controller::FRICycleGate gate; + gate.reset(); + std::atomic worker_ok{true}; + std::vector 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); + for (size_t frame = 1; frame < sent.size(); ++frame) { + EXPECT_NEAR(sent[frame] - sent[frame - 1], 0.001, 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()); +} diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 801b080..9ca3b82 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -6,6 +6,7 @@ + @@ -143,6 +144,7 @@ iiwa_controller/IIWAHardwareInterface $(arg robot_ip) $(arg fri_port) + $(arg fri_cycle_ms) false @@ -241,4 +243,4 @@ - \ No newline at end of file +