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
+