test

src/iiwa_controller/CMakeLists.txt

@@ -16,6 +16,11 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(realtime_tools REQUIRED)
 find_package(std_msgs REQUIRED)
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+endif()
+
 # FRI SDK
 set(FRI_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/libFRI)
 
@@ -75,6 +80,28 @@ pluginlib_export_plugin_description_file(
   iiwa_hardware_interface_plugin.xml
 )
 
+if(BUILD_TESTING)
+  ament_add_gtest(
+    test_guards
+    test/test_guards.cpp
+  )
+  target_include_directories(test_guards PRIVATE
+    include
+    ${FRI_SDK_DIR}/include
+    ${FRI_SDK_DIR}/src/nanopb-0.2.8
+    ${FRI_SDK_DIR}/src/protobuf
+    ${FRI_SDK_DIR}/src/protobuf_gen
+    ${FRI_SDK_DIR}/src/base
+    ${FRI_SDK_DIR}/src/client_lbr
+    ${FRI_SDK_DIR}/src/connection
+  )
+
+  ament_add_pytest_test(
+    test_safety_contract
+    test/test_safety_contract.py
+  )
+endif()
+
 # Установка — только библиотека и заголовки, без config/launch/urdf
 install(TARGETS ${PROJECT_NAME}
   EXPORT export_${PROJECT_NAME}
@@ -87,10 +114,14 @@ install(DIRECTORY include/
   DESTINATION include
 )
 
+install(DIRECTORY external/libFRI/include/
+  DESTINATION include
+)
+
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(export_${PROJECT_NAME})
 ament_export_dependencies(
   controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs)
 
-ament_package()
+ament_package()

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -2,26 +2,35 @@
 
 #include <array>
 #include <atomic>
-#include <mutex>
+#include <cstddef>
+#include <cstdint>
 
 #include "friClientApplication.h"
 #include "friLBRClient.h"
 #include "friUdpConnection.h"
 
+#include "iiwa_controller/filters.hpp"
+
 namespace iiwa_controller
 {
 
 
-// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке
-// и так же за один lock читается из read() в потоке управления.
+// Снимок состояния робота публикуется через короткий seqlock: FRI-поток не
+// блокируется на mutex, а ros2_control читает только согласованный снимок.
 struct IIWAStateSnapshot
 {
-  std::array<double, 7> measured_pos{}; // в Commanding = filtered_pos_ (open-loop)
+  std::array<double, 7> measured_pos{}; // фактическая позиция из FRI
   std::array<double, 7> measured_tau{}; // измеренные моменты [Нм]
   std::array<double, 7> external_tau{}; // внешние моменты без компенсации модели [Нм]
   std::array<double, 7> ipo_pos{};      // позиция интерполятора [рад], только в Commanding
   double sample_time{0.005};            // период цикла FRI [с]
   KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
+  KUKA::FRI::ESafetyState safety_state{KUKA::FRI::SAFETY_STOP_LEVEL_2};
+  KUKA::FRI::EOperationMode operation_mode{KUKA::FRI::TEST_MODE_1};
+  KUKA::FRI::EDriveState drive_state{KUKA::FRI::OFF};
+  KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::NO_COMMAND_MODE};
+  KUKA::FRI::EControlMode control_mode{KUKA::FRI::NO_CONTROL};
+  double tracking_performance{0.0};
   bool ipo_valid{false};                // в Monitor-режиме IPO недоступна
   unsigned int time_stamp_sec{0};       // Unix-время пакета [с]
   unsigned int time_stamp_nano_sec{0};  // наносекундная часть [нс]
@@ -45,22 +54,39 @@ public:
   void onStateChange(
     KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
 
-  // Потокобезопасное API для ros2_control, вызывается из read() и write()
+  // API для ros2_control. Обмен данными не использует mutex в RT-пути.
   void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
   IIWAStateSnapshot getStateSnapshot() const;
   bool isCommandingActive() const;
   KUKA::FRI::ESessionState getSessionState() const;
+  uint64_t getSnapshotGeneration() const;
 
 private:
   double joint_position_tau_;
   std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
 
-  mutable std::mutex data_mutex_;
-  std::array<double, N_JOINTS> target_pos_{};
+  // Цель публикуется поэлементно атомарно, чтобы read()/write() не
+  // блокировали FRI callback и не создавали data race.
+  std::array<std::atomic<double>, N_JOINTS> target_pos_atomic_{};
   // Сглаженная позиция, которую реально отправляем роботу.
   // Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
   std::array<double, N_JOINTS> filtered_pos_{};
-  IIWAStateSnapshot snapshot_{};
+  std::array<std::atomic<double>, N_JOINTS> measured_pos_{};
+  std::array<std::atomic<double>, N_JOINTS> measured_tau_{};
+  std::array<std::atomic<double>, N_JOINTS> external_tau_{};
+  std::array<std::atomic<double>, N_JOINTS> ipo_pos_{};
+  std::atomic<double> sample_time_{0.005};
+  std::atomic<KUKA::FRI::EConnectionQuality> quality_{KUKA::FRI::POOR};
+  std::atomic<KUKA::FRI::ESafetyState> safety_state_{KUKA::FRI::SAFETY_STOP_LEVEL_2};
+  std::atomic<KUKA::FRI::EOperationMode> operation_mode_{KUKA::FRI::TEST_MODE_1};
+  std::atomic<KUKA::FRI::EDriveState> drive_state_{KUKA::FRI::OFF};
+  std::atomic<KUKA::FRI::EClientCommandMode> client_command_mode_{KUKA::FRI::NO_COMMAND_MODE};
+  std::atomic<KUKA::FRI::EControlMode> control_mode_{KUKA::FRI::NO_CONTROL};
+  std::atomic<double> tracking_performance_{0.0};
+  std::atomic<bool> ipo_valid_{false};
+  std::atomic<unsigned int> time_stamp_sec_{0};
+  std::atomic<unsigned int> time_stamp_nano_sec_{0};
+  std::atomic<uint64_t> snapshot_generation_{0};
 
   // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна)
   void captureMonitoringData();

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -22,7 +22,9 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
+#include "iiwa_controller/CommandGuard.hpp"
 #include "iiwa_controller/FRIClient.h"
+#include "iiwa_controller/StateGuard.hpp"
 
 namespace iiwa_controller
 {
@@ -63,6 +65,11 @@ private:
   // EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей.
   // joint_velocity_tau = 0 отключает фильтр (raw finite difference).
   double joint_velocity_tau_{0.01};
+  std::array<double, N_JOINTS> command_min_{};
+  std::array<double, N_JOINTS> command_max_{};
+  std::array<double, N_JOINTS> command_max_velocity_{};
+  CommandGuard command_guard_;
+  StateGuard state_guard_;
 
   // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;
@@ -73,6 +80,7 @@ private:
   // read() лишь читает готовый снимок — без блокировки RT-потока.
   std::thread fri_thread_;
   std::atomic<bool> fri_running_{false};
+  std::atomic<bool> communication_fault_{false};
   void friThreadFunc();
 
   // Хэндлы интерфейсов состояния, заполняются в on_activate
@@ -90,7 +98,11 @@ private:
   unsigned int last_ts_sec_{0};
   unsigned int last_ts_nsec_{0};
   bool velocity_initialized_{false};
+  uint64_t last_snapshot_generation_{0};
+  unsigned int stale_snapshot_cycles_{0};
   void compute_velocity_(const IIWAStateSnapshot & snap);
+  std::array<double, N_JOINTS> last_command_pos_{};
+  bool command_initialized_{false};
 
   // Отслеживание сессии FRI для обнаружения потери управления
   KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE};

src/iiwa_controller/package.xml

@@ -19,6 +19,9 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>python3-pytest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/iiwa_controller/src/FRIClient.cpp

@@ -23,7 +23,13 @@ static const char * friStateName(KUKA::FRI::ESessionState s)
 FRIClient::FRIClient(double joint_position_tau)
 : joint_position_tau_(joint_position_tau)
 {
-  target_pos_.fill(0.0);
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    target_pos_atomic_[i].store(0.0, std::memory_order_relaxed);
+    measured_pos_[i].store(0.0, std::memory_order_relaxed);
+    measured_tau_[i].store(0.0, std::memory_order_relaxed);
+    external_tau_[i].store(0.0, std::memory_order_relaxed);
+    ipo_pos_[i].store(0.0, std::memory_order_relaxed);
+  }
   filtered_pos_.fill(0.0);
 }
 
@@ -31,20 +37,26 @@ FRIClient::FRIClient(double joint_position_tau)
 // В Monitor-режиме getIpoJointPosition() бросает FRIException, поэтому здесь не зовём.
 void FRIClient::captureMonitoringData()
 {
-  std::memcpy(
-    snapshot_.measured_pos.data(),
-    robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double));
-  std::memcpy(
-    snapshot_.measured_tau.data(),
-    robotState().getMeasuredTorque(), N_JOINTS * sizeof(double));
-  std::memcpy(
-    snapshot_.external_tau.data(),
-    robotState().getExternalTorque(), N_JOINTS * sizeof(double));
-  snapshot_.sample_time = robotState().getSampleTime();
-  snapshot_.quality = robotState().getConnectionQuality();
-  snapshot_.ipo_valid = false;
-  snapshot_.time_stamp_sec = robotState().getTimestampSec();
-  snapshot_.time_stamp_nano_sec = robotState().getTimestampNanoSec();
+  const auto & state = robotState();
+  const auto * measured_pos = state.getMeasuredJointPosition();
+  const auto * measured_tau = state.getMeasuredTorque();
+  const auto * external_tau = state.getExternalTorque();
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    measured_pos_[i].store(measured_pos[i], std::memory_order_relaxed);
+    measured_tau_[i].store(measured_tau[i], std::memory_order_relaxed);
+    external_tau_[i].store(external_tau[i], std::memory_order_relaxed);
+  }
+  sample_time_.store(state.getSampleTime(), std::memory_order_relaxed);
+  quality_.store(state.getConnectionQuality(), std::memory_order_relaxed);
+  safety_state_.store(state.getSafetyState(), std::memory_order_relaxed);
+  operation_mode_.store(state.getOperationMode(), std::memory_order_relaxed);
+  drive_state_.store(state.getDriveState(), std::memory_order_relaxed);
+  client_command_mode_.store(state.getClientCommandMode(), std::memory_order_relaxed);
+  control_mode_.store(state.getControlMode(), std::memory_order_relaxed);
+  tracking_performance_.store(state.getTrackingPerformance(), std::memory_order_relaxed);
+  ipo_valid_.store(false, std::memory_order_relaxed);
+  time_stamp_sec_.store(state.getTimestampSec(), std::memory_order_relaxed);
+  time_stamp_nano_sec_.store(state.getTimestampNanoSec(), std::memory_order_relaxed);
 }
 
 // Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE).
@@ -52,20 +64,18 @@ void FRIClient::captureMonitoringData()
 void FRIClient::captureCommandingData()
 {
   captureMonitoringData();
-  std::memcpy(
-    snapshot_.ipo_pos.data(),
-    robotState().getIpoJointPosition(), N_JOINTS * sizeof(double));
-  snapshot_.ipo_valid = true;
-  // Open-loop: JTC видит filtered_pos_ как «измеренную» позицию — как в lbr_fri_ros2_stack.
-  // Благодаря этому JTC не видит расхождения и не генерирует коррекций.
-  snapshot_.measured_pos = filtered_pos_;
+  const auto * ipo_pos = robotState().getIpoJointPosition();
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    ipo_pos_[i].store(ipo_pos[i], std::memory_order_relaxed);
+  }
+  ipo_valid_.store(true, std::memory_order_relaxed);
 }
 
 // Вызывается в MONITORING_WAIT и MONITORING_READY
 void FRIClient::monitor()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
   captureMonitoringData();
+  snapshot_generation_.fetch_add(1, std::memory_order_release);
 }
 
 // Вызывается в COMMANDING_WAIT.
@@ -76,35 +86,39 @@ void FRIClient::monitor()
 // статическое отклонение не даст выполниться этому условию.
 void FRIClient::waitForCommand()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
   captureCommandingData();
 
   // Инициализируем цель и фильтр IPO-позицией.
   // Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE.
-  std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
-  std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    const double ipo = ipo_pos_[i].load(std::memory_order_relaxed);
+    target_pos_atomic_[i].store(ipo, std::memory_order_relaxed);
+    filtered_pos_[i] = ipo;
+  }
 
   robotCommand().setJointPosition(filtered_pos_.data());
+  snapshot_generation_.fetch_add(1, std::memory_order_release);
 }
 
 // Вызывается в COMMANDING_ACTIVE, основной цикл управления
 void FRIClient::command()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
-
-  // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_
-  // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом).
-  // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA.
-  const double dt = robotState().getSampleTime();
-  const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0;
+  std::array<double, N_JOINTS> target{};
   for (size_t i = 0; i < N_JOINTS; ++i) {
-    filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
+    target[i] = target_pos_atomic_[i].load(std::memory_order_relaxed);
+  }
+
+  const double dt = robotState().getSampleTime();
+  const double alpha = exponentialFilterAlpha(joint_position_tau_, dt);
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    filtered_pos_[i] = alpha * target[i] + (1.0 - alpha) * filtered_pos_[i];
   }
 
   robotCommand().setJointPosition(filtered_pos_.data());
 
-  // Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил.
+  // Захватываем фактическое состояние FRI после отправки команды.
   captureCommandingData();
+  snapshot_generation_.fetch_add(1, std::memory_order_release);
 }
 
 void FRIClient::onStateChange(
@@ -132,14 +146,32 @@ void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
       return;
     }
   }
-  std::lock_guard<std::mutex> lock(data_mutex_);
-  target_pos_ = q;
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    target_pos_atomic_[i].store(q[i], std::memory_order_relaxed);
+  }
 }
 
 IIWAStateSnapshot FRIClient::getStateSnapshot() const
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
-  return snapshot_;
+  IIWAStateSnapshot result{};
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    result.measured_pos[i] = measured_pos_[i].load(std::memory_order_relaxed);
+    result.measured_tau[i] = measured_tau_[i].load(std::memory_order_relaxed);
+    result.external_tau[i] = external_tau_[i].load(std::memory_order_relaxed);
+    result.ipo_pos[i] = ipo_pos_[i].load(std::memory_order_relaxed);
+  }
+  result.sample_time = sample_time_.load(std::memory_order_relaxed);
+  result.quality = quality_.load(std::memory_order_relaxed);
+  result.safety_state = safety_state_.load(std::memory_order_relaxed);
+  result.operation_mode = operation_mode_.load(std::memory_order_relaxed);
+  result.drive_state = drive_state_.load(std::memory_order_relaxed);
+  result.client_command_mode = client_command_mode_.load(std::memory_order_relaxed);
+  result.control_mode = control_mode_.load(std::memory_order_relaxed);
+  result.tracking_performance = tracking_performance_.load(std::memory_order_relaxed);
+  result.ipo_valid = ipo_valid_.load(std::memory_order_relaxed);
+  result.time_stamp_sec = time_stamp_sec_.load(std::memory_order_relaxed);
+  result.time_stamp_nano_sec = time_stamp_nano_sec_.load(std::memory_order_relaxed);
+  return result;
 }
 
 bool FRIClient::isCommandingActive() const
@@ -152,4 +184,9 @@ KUKA::FRI::ESessionState FRIClient::getSessionState() const
   return session_state_.load(std::memory_order_relaxed);
 }
 
+uint64_t FRIClient::getSnapshotGeneration() const
+{
+  return snapshot_generation_.load(std::memory_order_acquire);
+}
+
 }  // namespace iiwa_controller

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -1,8 +1,11 @@
 #include "iiwa_controller/IIWAHardwareInterface.hpp"
 
+#include <algorithm>
 #include <chrono>
 #include <cmath>
 #include <cstdint>
+#include <limits>
+#include <stdexcept>
 #include <thread>
 
 #include "hardware_interface/hardware_info.hpp"
@@ -10,6 +13,7 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "friException.h"
 
 PLUGINLIB_EXPORT_CLASS(
   iiwa_controller::IIWAHardwareInterface,
@@ -43,11 +47,24 @@ CallbackReturn IIWAHardwareInterface::on_init(
 
   const auto & info = params.hardware_info;
 
-  robot_ip_            = getParam(info, "robot_ip", "192.170.10.2");
-  fri_port_            = std::stoi(getParam(info, "fri_port", "30200"));
-  simulate_            = (getParam(info, "simulate", "false") == "true");
-  joint_position_tau_  = std::stod(getParam(info, "joint_position_tau", "0.04"));
-  joint_velocity_tau_  = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
+  try {
+    robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
+    fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
+    const auto simulate = getParam(info, "simulate", "false");
+    simulate_ = simulate == "true" || simulate == "1" || simulate == "yes";
+    joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04"));
+    joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
+  } catch (const std::exception & ex) {
+    RCLCPP_FATAL(rclcpp::get_logger(LOG), "Некорректные параметры FRI: %s", ex.what());
+    return CallbackReturn::ERROR;
+  }
+
+  if (robot_ip_.empty() || fri_port_ < 1 || fri_port_ > 65535 ||
+      !std::isfinite(joint_position_tau_) || joint_position_tau_ < 0.0 ||
+      !std::isfinite(joint_velocity_tau_) || joint_velocity_tau_ < 0.0) {
+    RCLCPP_FATAL(rclcpp::get_logger(LOG), "Параметры FRI вне допустимого диапазона");
+    return CallbackReturn::ERROR;
+  }
 
   RCLCPP_INFO(rclcpp::get_logger(LOG),
     "on_init: ip=%s port=%d simulate=%s pos_tau=%.3f vel_tau=%.3f",
@@ -62,6 +79,84 @@ CallbackReturn IIWAHardwareInterface::on_init(
     return CallbackReturn::ERROR;
   }
 
+  command_min_.fill(-std::numeric_limits<double>::infinity());
+  command_max_.fill(std::numeric_limits<double>::infinity());
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    const std::string expected_name = "joint" + std::to_string(i + 1);
+    if (info.joints[i].name != expected_name) {
+      RCLCPP_FATAL(rclcpp::get_logger(LOG),
+        "Сустав %zu имеет имя '%s', ожидается '%s'", i,
+        info.joints[i].name.c_str(), expected_name.c_str());
+      return CallbackReturn::ERROR;
+    }
+
+    bool position_interface_found = false;
+    for (const auto & interface : info.joints[i].command_interfaces) {
+      if (interface.name != hardware_interface::HW_IF_POSITION) {
+        continue;
+      }
+      position_interface_found = true;
+      try {
+        auto min_value = interface.min;
+        auto max_value = interface.max;
+        if (min_value.empty()) {
+          const auto it = interface.parameters.find("min");
+          if (it != interface.parameters.end()) {
+            min_value = it->second;
+          }
+        }
+        if (max_value.empty()) {
+          const auto it = interface.parameters.find("max");
+          if (it != interface.parameters.end()) {
+            max_value = it->second;
+          }
+        }
+        if (!min_value.empty()) {
+          command_min_[i] = std::stod(min_value);
+        }
+        if (!max_value.empty()) {
+          command_max_[i] = std::stod(max_value);
+        }
+      } catch (const std::exception & ex) {
+        RCLCPP_FATAL(rclcpp::get_logger(LOG),
+          "Некорректные ограничения позиции для %s: %s",
+          expected_name.c_str(), ex.what());
+        return CallbackReturn::ERROR;
+      }
+      const auto limits_it = info.limits.find(expected_name);
+      if (limits_it != info.limits.end() && limits_it->second.has_position_limits) {
+        if (!std::isfinite(command_min_[i])) {
+          command_min_[i] = limits_it->second.min_position;
+        }
+        if (!std::isfinite(command_max_[i])) {
+          command_max_[i] = limits_it->second.max_position;
+        }
+      }
+    }
+    if (!position_interface_found || !std::isfinite(command_min_[i]) ||
+        !std::isfinite(command_max_[i]) || command_min_[i] > command_max_[i]) {
+      RCLCPP_FATAL(rclcpp::get_logger(LOG),
+        "Для %s отсутствует корректный position command interface", expected_name.c_str());
+      return CallbackReturn::ERROR;
+    }
+
+    const auto limits_it = info.limits.find(expected_name);
+    if (limits_it == info.limits.end() || !limits_it->second.has_velocity_limits ||
+        !std::isfinite(limits_it->second.max_velocity) ||
+        limits_it->second.max_velocity <= 0.0) {
+      RCLCPP_FATAL(rclcpp::get_logger(LOG),
+        "Для %s отсутствует корректное ограничение скорости", expected_name.c_str());
+      return CallbackReturn::ERROR;
+    }
+    command_max_velocity_[i] = limits_it->second.max_velocity;
+  }
+
+  CommandGuardLimits guard_limits;
+  guard_limits.min_position = command_min_;
+  guard_limits.max_position = command_max_;
+  guard_limits.max_velocity = command_max_velocity_;
+  command_guard_ = CommandGuard(guard_limits);
+
   prev_pos_.fill(0.0);
   velocity_.fill(0.0);
   velocity_raw_.fill(0.0);
@@ -105,6 +200,9 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State
   if (!app_->connect(fri_port_, robot_ip_.c_str())) {
     RCLCPP_FATAL(rclcpp::get_logger(LOG),
       "Не удалось открыть UDP-сокет на порту %d (робот: %s)", fri_port_, robot_ip_.c_str());
+    app_.reset();
+    connection_.reset();
+    fri_client_.reset();
     return CallbackReturn::ERROR;
   }
 
@@ -145,13 +243,20 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     return CallbackReturn::SUCCESS;
   }
 
+  if (!fri_client_ || !connection_ || !app_) {
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI не сконфигурирован перед активацией");
+    return CallbackReturn::ERROR;
+  }
+
+  communication_fault_.store(false, std::memory_order_release);
   fri_running_.store(true, std::memory_order_relaxed);
   fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
 
   constexpr int kTimeoutMs = 15000;
   constexpr int kPollMs    = 200;
   for (int elapsed = 0;
-       fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs;
+       fri_client_->getSessionState() < KUKA::FRI::MONITORING_READY &&
+       !communication_fault_.load(std::memory_order_acquire) && elapsed < kTimeoutMs;
        elapsed += kPollMs)
   {
     RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
@@ -159,10 +264,17 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs));
   }
 
-  if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
+  if (fri_client_->getSessionState() < KUKA::FRI::MONITORING_READY ||
+      communication_fault_.load(std::memory_order_acquire)) {
     RCLCPP_ERROR(rclcpp::get_logger(LOG),
       "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s",
       kTimeoutMs / 1000, robot_ip_.c_str());
+    fri_running_.store(false, std::memory_order_release);
+    if (fri_thread_.joinable()) {
+      fri_thread_.join();
+    }
+    app_->disconnect();
+    return CallbackReturn::ERROR;
   } else {
     RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI сессия установлена!");
     const auto snap = fri_client_->getStateSnapshot();
@@ -171,6 +283,10 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     last_ts_nsec_ = snap.time_stamp_nano_sec;
     velocity_.fill(0.0);
     velocity_initialized_ = true;
+    last_snapshot_generation_ = fri_client_->getSnapshotGeneration();
+    stale_snapshot_cycles_ = 0;
+    last_command_pos_ = snap.ipo_valid ? snap.ipo_pos : snap.measured_pos;
+    command_initialized_ = snap.ipo_valid;
   }
 
   previous_session_state_ = fri_client_->getSessionState();
@@ -184,12 +300,12 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
 {
   RCLCPP_INFO(rclcpp::get_logger(LOG), "Деактивация...");
 
-  if (!simulate_ && fri_running_.load()) {
-    fri_running_.store(false, std::memory_order_relaxed);
-    // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке.
-    // Только потом join(), иначе он зависнет навсегда.
-    if (app_) { app_->disconnect(); }
+  if (!simulate_) {
+    fri_running_.store(false, std::memory_order_release);
+    // UdpConnection не обещает потокобезопасный disconnect(). Сначала ждём
+    // завершения step() (таймаут сокета 100 мс), затем закрываем приложение.
     if (fri_thread_.joinable()) { fri_thread_.join(); }
+    if (app_) { app_->disconnect(); }
     RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток остановлен");
   }
 
@@ -199,6 +315,7 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
   }
 
   velocity_initialized_ = false;
+  command_initialized_ = false;
   return CallbackReturn::SUCCESS;
 }
 
@@ -207,9 +324,13 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
 
 CallbackReturn IIWAHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &)
 {
-  fri_client_.reset();
-  connection_.reset();
+  fri_running_.store(false, std::memory_order_release);
+  if (fri_thread_.joinable()) {
+    fri_thread_.join();
+  }
   app_.reset();
+  connection_.reset();
+  fri_client_.reset();
   return CallbackReturn::SUCCESS;
 }
 
@@ -219,11 +340,28 @@ void IIWAHardwareInterface::friThreadFunc()
 {
   RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен");
 
-  while (fri_running_.load(std::memory_order_relaxed)) {
-    if (!app_->step()) {
-      RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-        "FRI: step() вернул false, возможно потеряли соединение");
+  try {
+    while (fri_running_.load(std::memory_order_acquire)) {
+      if (!app_->step()) {
+        communication_fault_.store(true, std::memory_order_release);
+        fri_running_.store(false, std::memory_order_release);
+        RCLCPP_ERROR(rclcpp::get_logger(LOG),
+          "FRI: step() вернул false, соединение потеряно");
+        break;
+      }
     }
+  } catch (const KUKA::FRI::FRIException & ex) {
+    communication_fault_.store(true, std::memory_order_release);
+    fri_running_.store(false, std::memory_order_release);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Исключение FRI: %s", ex.getErrorMessage());
+  } catch (const std::exception & ex) {
+    communication_fault_.store(true, std::memory_order_release);
+    fri_running_.store(false, std::memory_order_release);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Ошибка FRI-потока: %s", ex.what());
+  } catch (...) {
+    communication_fault_.store(true, std::memory_order_release);
+    fri_running_.store(false, std::memory_order_release);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Неизвестная ошибка FRI-потока");
   }
 
   RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток завершён");
@@ -262,11 +400,15 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
     {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
   static constexpr double kVelDeadband = 1e-4;
 
+  if (dt < 0.0) {
+    communication_fault_.store(true, std::memory_order_release);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI timestamp пошёл назад");
+    return;
+  }
+
   if (dt > 0.0) {
     // EMA alpha для фильтра скорости: tau=0 → alpha=1 (без фильтра)
-    const double vel_alpha = (joint_velocity_tau_ > 0.0)
-      ? dt / (joint_velocity_tau_ + dt)
-      : 1.0;
+    const double vel_alpha = exponentialFilterAlpha(joint_velocity_tau_, dt);
 
     for (size_t i = 0; i < N_JOINTS; ++i) {
       const double raw     = (snap.measured_pos[i] - prev_pos_[i]) / dt;
@@ -300,7 +442,29 @@ hardware_interface::return_type IIWAHardwareInterface::read(
     return hardware_interface::return_type::OK;
   }
 
+  if (communication_fault_.load(std::memory_order_acquire)) {
+    return hardware_interface::return_type::ERROR;
+  }
+
   const auto snap = fri_client_->getStateSnapshot();
+  if (state_guard_.validateSnapshot(snap) != StateGuardResult::OK) {
+    communication_fault_.store(true, std::memory_order_release);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI передал некорректный снимок состояния");
+    return hardware_interface::return_type::ERROR;
+  }
+
+  const auto generation = fri_client_->getSnapshotGeneration();
+  if (generation == last_snapshot_generation_) {
+    ++stale_snapshot_cycles_;
+    if (stale_snapshot_cycles_ > 20U) {
+      communication_fault_.store(true, std::memory_order_release);
+      RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI не передаёт новые пакеты состояния");
+      return hardware_interface::return_type::ERROR;
+    }
+  } else {
+    last_snapshot_generation_ = generation;
+    stale_snapshot_cycles_ = 0;
+  }
 
   // Обнаружение потери управления: неожиданный выход из COMMANDING_ACTIVE.
   const auto current_state = fri_client_->getSessionState();
@@ -313,6 +477,12 @@ hardware_interface::return_type IIWAHardwareInterface::read(
   }
   previous_session_state_ = current_state;
 
+  if (snap.safety_state != KUKA::FRI::NORMAL_OPERATION) {
+    communication_fault_.store(true, std::memory_order_release);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI сообщил safety stop (%d)", snap.safety_state);
+    return hardware_interface::return_type::ERROR;
+  }
+
   compute_velocity_(snap);
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
@@ -334,16 +504,51 @@ hardware_interface::return_type IIWAHardwareInterface::write(
     return hardware_interface::return_type::OK;
   }
 
-  if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) {
+  if (communication_fault_.load(std::memory_order_acquire)) {
+    return hardware_interface::return_type::ERROR;
+  }
+
+  const auto current_state = fri_client_->getSessionState();
+  if (current_state != KUKA::FRI::COMMANDING_ACTIVE) {
     return hardware_interface::return_type::OK;
   }
 
+  const auto snap = fri_client_->getStateSnapshot();
+  const auto state_guard_result =
+    state_guard_.validatePositionState(snap, current_state);
+  if (state_guard_result != StateGuardResult::OK) {
+    communication_fault_.store(true, std::memory_order_release);
+    RCLCPP_ERROR_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 1000,
+      "StateGuard запретил position-команду: %s",
+      stateGuardResultName(state_guard_result));
+    return hardware_interface::return_type::ERROR;
+  }
+
   std::array<double, N_JOINTS> pos_cmd{};
   for (size_t i = 0; i < N_JOINTS; ++i) {
     get_command(h_cmd_pos_[i], pos_cmd[i], false);
+    if (!std::isfinite(pos_cmd[i]) || pos_cmd[i] < command_min_[i] ||
+        pos_cmd[i] > command_max_[i]) {
+      communication_fault_.store(true, std::memory_order_release);
+      RCLCPP_ERROR(rclcpp::get_logger(LOG),
+        "Команда %s вне допустимого диапазона: %.9f", info_.joints[i].name.c_str(), pos_cmd[i]);
+      return hardware_interface::return_type::ERROR;
+    }
+  }
+
+  const auto command_guard_result = command_guard_.validate(
+    pos_cmd, last_command_pos_, snap.sample_time, command_initialized_);
+  if (command_guard_result != CommandGuardResult::OK) {
+    communication_fault_.store(true, std::memory_order_release);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG),
+      "CommandGuard запретил position-команду: %s",
+      commandGuardResultName(command_guard_result));
+    return hardware_interface::return_type::ERROR;
   }
 
   fri_client_->setTargetJointPositions(pos_cmd);
+  last_command_pos_ = pos_cmd;
+  command_initialized_ = true;
 
   return hardware_interface::return_type::OK;
 }