test

.gitignore

@@ -10,7 +10,9 @@ venv
 .venv
 .claude
 .codex
+.agents
 __pycache__
+.pytest_cache
 
 *.egg-info
 **.FCBak

README.md

@@ -65,7 +65,7 @@ Docker можно использовать как альтернативную
 
 ### Требования
 
-- Ubuntu 24.04 LTS;
+- Ubuntu 24.044 LTS;
 - доступ в интернет;
 - права `sudo`;
 - физический KUKA LBR iiwa 7 R800 либо компьютер для работы только с симулятором.

cobot-setting.yaml

@@ -50,6 +50,7 @@ web:
   enabled: true
   host: "0.0.0.0"
   port: 8007
+  token: "ysXOyL_p3-f2YH2WqQ808KgQUA32qACziKdfqbhsJsPI0GGkoRbGn9eU22FV8mdS"
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 

cobot/commands/robot_setup.py

@@ -109,6 +109,8 @@ _BLOCKS: List[_Block] = [
                    options=["0.0.0.0", "127.0.0.1"]),
             _Field("port", "Порт HTTP:", "8007",
                    note="Порт FastAPI-сервера (по умолчанию 8007)"),
+            _Field("token", "Токен Bearer для REST API и MCP:", "",
+                   note="Длинный секрет. Хранится в cobot-setting.yaml и нужен при внешнем host; пустое значение отключает авторизацию"),
         ],
     ),
     _Block(

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

@@ -122,6 +122,7 @@ web:
   enabled: true
   host: "0.0.0.0"
   port: 8007
+  token: "replace-with-a-long-secret-token"
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 ```
@@ -131,10 +132,18 @@ web:
 | `enabled` | Enable (`true`) or disable (`false`) the web server |
 | `host` | Listening address: `0.0.0.0` for all interfaces or `127.0.0.1` for local access only |
 | `port` | HTTP API port; default: `8007` |
+| `token` | Bearer token for the REST API and MCP; required for an external `host` |
 | `endpoints` | Path to the REST endpoint description |
 | `joint_limits` | Path to joint limits used for command validation |
 
-After startup, the REST API is available at `http://<host>:8007`, and MCP is available at `/mcp`.
+When `token` is set, every request to a protected REST or MCP route must include
+the `Authorization: Bearer <token>` header. The `/docs`, `/redoc`, and
+`/openapi.json` resources are public only to load Swagger UI. An empty `token`
+is allowed only for local access (`127.0.0.1` or `localhost`); the server
+refuses to start with an external `host`. Protect the settings file and do not
+publish the token.
+
+After startup, the REST API is available at `http://<host>:8007`, and MCP is available at `/mcp/mcp`.
 
 ---
 

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

@@ -124,6 +124,7 @@ web:
   enabled: true
   host: "0.0.0.0"
   port: 8007
+  token: "замените-на-длинный-секретный-токен"
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 ```
@@ -133,10 +134,18 @@ web:
 | `enabled` | Включить (`true`) или отключить (`false`) веб-сервер |
 | `host` | Адрес прослушивания: `0.0.0.0` — все интерфейсы, `127.0.0.1` — только локально |
 | `port` | Порт HTTP API (по умолчанию `8007`) |
+| `token` | Bearer-токен для REST API и MCP; обязателен для внешнего `host` |
 | `endpoints` | Путь к описанию REST-эндпоинтов |
 | `joint_limits` | Путь к файлу ограничений суставов для валидации команд |
 
-После запуска REST API доступен по адресу `http://<host>:8007`, MCP — по пути `/mcp`.
+Если `token` заполнен, каждый запрос к защищённому REST- или MCP-маршруту
+должен содержать заголовок `Authorization: Bearer <token>`. Страницы
+`/docs`, `/redoc` и `/openapi.json` доступны без токена только для загрузки
+Swagger UI. Пустой `token` допустим только для локального доступа
+(`127.0.0.1` или `localhost`); при внешнем `host` сервер не запустится.
+Храните файл настроек с ограниченными правами доступа и не публикуйте токен.
+
+После запуска REST API доступен по адресу `http://<host>:8007`, MCP — по пути `/mcp/mcp`.
 
 ---
 

doc/lwc-doc/docs/getting-started/control/rest-api.en.md

@@ -16,6 +16,7 @@ web:
   enabled: true
   host: 0.0.0.0
   port: 8007
+  token: "replace-with-a-long-secret-token"
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 ~~~
@@ -28,7 +29,65 @@ After starting the stack with **cobot run**, the server is available at **http:/
 
 There is no separate health-check endpoint. If Swagger UI opens, the HTTP server is running. Readiness of ROS components is checked when a specific endpoint is called.
 
-By default, the server listens on all network interfaces and does not use authentication. Do not expose port 8007 to an untrusted network. For local access, set **host: 127.0.0.1**. For remote access, restrict the network with firewall rules or a VPN.
+When `host` is not `localhost`, `127.0.0.1`, or another loopback address, the
+`token` field is required; otherwise the server exits during startup. When
+`token` is set, Bearer authentication applies to every REST route and to the
+MCP route `/mcp/mcp`, including with a local `host`.
+
+The `/docs`, `/redoc`, and `/openapi.json` resources are available without a
+header so the browser can load Swagger UI. This does not expose control
+commands. Open `/docs`, click **Authorize**, paste the `web.token` value without
+the word `Bearer`, and confirm. Swagger adds the header to API requests.
+
+The token is stored in **cobot-setting.yaml**. Do not add it to documentation,
+scripts, or public repositories. If it is exposed, replace it and restart the
+stack. For remote access, also restrict port 8007 with a firewall or VPN.
+
+### REST and MCP authentication
+
+Every request to a protected REST route or MCP must include the following header
+when `web.token` is set:
+
+~~~ http
+Authorization: Bearer <web.token value>
+~~~
+
+Client setup examples:
+
+=== "curl"
+
+    ~~~ bash
+    HOST=http://localhost:8007
+    API_TOKEN='copy web.token from cobot-setting.yaml'
+    AUTH_HEADER="Authorization: Bearer ${API_TOKEN}"
+    curl -sS -H "${AUTH_HEADER}" $HOST/robot/joint_states
+    ~~~
+
+=== "Python"
+
+    ~~~ python
+    import httpx
+
+    HOST = "http://localhost:8007"
+    API_TOKEN = "copy web.token from cobot-setting.yaml"
+    HEADERS = {"Authorization": f"Bearer {API_TOKEN}"}
+    response = httpx.get(f"{HOST}/robot/joint_states", headers=HEADERS, timeout=10)
+    response.raise_for_status()
+    ~~~
+
+=== "MATLAB"
+
+    ~~~ matlab
+    HOST = 'http://localhost:8007';
+    API_TOKEN = 'copy web.token from cobot-setting.yaml';
+    readOpts = weboptions('Timeout', 10, ...
+        'HeaderFields', {'Authorization', ['Bearer ' API_TOKEN]});
+    jointState = webread([HOST '/robot/joint_states'], readOpts);
+    ~~~
+
+Pass the same header when connecting an MCP client to
+`http://<host>:8007/mcp/mcp`. If the client supports custom HTTP headers, set
+`Authorization: Bearer <web.token value>` in its connection settings.
 
 ## Preparing the examples
 
@@ -648,4 +707,4 @@ When troubleshooting, proceed from simple checks to more complex ones:
 3. Make sure that the complete stack is running: `controller_manager`, MoveIt, and `iiwa_motion_server`.
 4. After starting a sequence, inspect **/sequences/logs**. After publishing a trajectory, inspect **/trajectory/logs**.
 
-The MCP server runs in the same process but provides a separate interface at **http://server-address:8007/mcp/mcp**. For ordinary HTTP integrations, use the endpoints documented on this page.
+The MCP server runs in the same process but provides a separate interface at **http://server-address:8007/mcp/mcp**. It uses the same Bearer token; pass the `Authorization` header when connecting an MCP client. For ordinary HTTP integrations, use the endpoints documented on this page.

doc/lwc-doc/docs/getting-started/control/rest-api.md

@@ -16,6 +16,7 @@ web:
   enabled: true
   host: 0.0.0.0
   port: 8007
+  token: "замените-на-длинный-секретный-токен"
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 ~~~
@@ -28,7 +29,65 @@ web:
 
 Отдельного health-check в сервере нет. Если открывается Swagger UI, HTTP-сервер запущен. Готовность ROS-компонентов проверяется при обращении к конкретному маршруту.
 
-По умолчанию сервер слушает все сетевые интерфейсы и не использует аутентификацию. Не публикуйте порт 8007 в недоверенную сеть. Для локальной работы укажите **host: 127.0.0.1**, а для удалённого доступа ограничьте сеть правилами firewall или VPN.
+Если `host` отличается от `localhost`, `127.0.0.1` или другого loopback-адреса,
+поле `token` обязательно: без него сервер завершит запуск с ошибкой. Если
+`token` заполнен, Bearer-аутентификация применяется ко всем REST-маршрутам и
+к MCP-маршруту `/mcp/mcp`, в том числе при локальном `host`.
+
+Страницы `/docs`, `/redoc` и схема `/openapi.json` доступны без заголовка,
+чтобы браузер мог загрузить Swagger UI. Это не открывает команды управления.
+Откройте `/docs`, нажмите **Authorize**, вставьте значение `web.token` без
+слова `Bearer` и подтвердите. Swagger сам добавит нужный заголовок к запросам.
+
+Токен хранится в **cobot-setting.yaml**. Не добавляйте его в документацию,
+скрипты или публичные репозитории; после утечки замените значение и перезапустите
+стек. Для удалённого доступа дополнительно ограничьте порт 8007 firewall или VPN.
+
+### Аутентификация REST и MCP
+
+Каждый запрос к защищённому REST-маршруту или MCP при заполненном `web.token`
+должен содержать:
+
+~~~ http
+Authorization: Bearer <значение-web.token>
+~~~
+
+Примеры подготовки клиентов:
+
+=== "curl"
+
+    ~~~ bash
+    HOST=http://localhost:8007
+    API_TOKEN='скопируйте значение web.token из cobot-setting.yaml'
+    AUTH_HEADER="Authorization: Bearer ${API_TOKEN}"
+    curl -sS -H "${AUTH_HEADER}" $HOST/robot/joint_states
+    ~~~
+
+=== "Python"
+
+    ~~~ python
+    import httpx
+
+    HOST = "http://localhost:8007"
+    API_TOKEN = "скопируйте значение web.token из cobot-setting.yaml"
+    HEADERS = {"Authorization": f"Bearer {API_TOKEN}"}
+    response = httpx.get(f"{HOST}/robot/joint_states", headers=HEADERS, timeout=10)
+    response.raise_for_status()
+    ~~~
+
+=== "MATLAB"
+
+    ~~~ matlab
+    HOST = 'http://localhost:8007';
+    API_TOKEN = 'скопируйте значение web.token из cobot-setting.yaml';
+    readOpts = weboptions('Timeout', 10, ...
+        'HeaderFields', {'Authorization', ['Bearer ' API_TOKEN]});
+    jointState = webread([HOST '/robot/joint_states'], readOpts);
+    ~~~
+
+Тот же заголовок передаётся MCP-клиенту при подключении к
+`http://<host>:8007/mcp/mcp`. Если клиент поддерживает пользовательские HTTP
+заголовки, укажите `Authorization: Bearer <значение-web.token>` в его настройках.
 
 ## Подготовка к примерам
 
@@ -96,7 +155,7 @@ GET **/robot/joint_states** возвращает последнее сообще
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/robot/joint_states", timeout=T_READ)
+    response = httpx.get(f"{HOST}/robot/joint_states", headers=HEADERS, timeout=T_READ)
     response.raise_for_status()
     state = response.json()
     print(dict(zip(state["name"], state["position"])))
@@ -139,7 +198,7 @@ GET **/robot/pose** вычисляет прямую кинематику чер
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/robot/pose", timeout=T_READ)
+    response = httpx.get(f"{HOST}/robot/pose", headers=HEADERS, timeout=T_READ)
     response.raise_for_status()
     pose = response.json()
     print(pose["position"])
@@ -172,7 +231,7 @@ GET **/robot/positions** читает положения group_state из SRDF.
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/robot/positions", timeout=T_READ)
+    response = httpx.get(f"{HOST}/robot/positions", headers=HEADERS, timeout=T_READ)
     response.raise_for_status()
     for position in response.json():
         print(position["name"], "—", position["description"])
@@ -223,6 +282,7 @@ POST **/robot/move/named** перемещает манипулятор в пол
     ~~~ python
     response = httpx.post(
         f"{HOST}/robot/move/named",
+        headers=HEADERS,
         json={"name": "home", "speed": 0.1, "accel_scale": 0.0},
         timeout=T_MOVE,
     )
@@ -274,7 +334,7 @@ POST **/robot/move/pose** принимает положение TCP в метр
         "a": 0.0, "b": 3.14159, "c": 0.0,
         "speed": 0.1, "planner": "ptp", "frame_id": "",
     }
-    response = httpx.post(f"{HOST}/robot/move/pose", json=target, timeout=T_MOVE)
+    response = httpx.post(f"{HOST}/robot/move/pose", headers=HEADERS, json=target, timeout=T_MOVE)
     response.raise_for_status()
     print(response.json())
     ~~~
@@ -319,6 +379,7 @@ POST **/robot/move/joints** принимает ровно семь углов в
     ~~~ python
     response = httpx.post(
         f"{HOST}/robot/move/joints",
+        headers=HEADERS,
         json={
             "joints": [0.0, 0.5, 0.0, -1.57, 0.0, 1.57, 0.0],
             "speed": 0.1,
@@ -382,7 +443,7 @@ POST **/trajectory/send** принимает одну или несколько
         ],
         "validate_limits": True,
     }
-    response = httpx.post(f"{HOST}/trajectory/send", json=trajectory, timeout=T_READ)
+    response = httpx.post(f"{HOST}/trajectory/send", headers=HEADERS, json=trajectory, timeout=T_READ)
     response.raise_for_status()
     print(response.json())
     ~~~
@@ -430,6 +491,7 @@ joint1,joint2,joint3,joint4,joint5,joint6,joint7,t
     with open("trajectory.csv", "rb") as csv_file:
         response = httpx.post(
             f"{HOST}/trajectory/send_csv",
+            headers=HEADERS,
             params={"separator": ",", "validate_limits": True},
             files={"file": ("trajectory.csv", csv_file, "text/csv")},
             timeout=T_READ,
@@ -468,7 +530,7 @@ GET **/trajectory/logs?n=50** возвращает до 300 последних
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/trajectory/logs", params={"n": 20}, timeout=T_READ)
+    response = httpx.get(f"{HOST}/trajectory/logs", headers=HEADERS, params={"n": 20}, timeout=T_READ)
     response.raise_for_status()
     for line in response.json()["lines"]:
         print(line)
@@ -538,6 +600,7 @@ POST **/sequences/start** запускает отдельный процесс m
     with open("motion_sequence_config.json", "rb") as config:
         response = httpx.post(
             f"{HOST}/sequences/start",
+            headers=HEADERS,
             files={"config": ("motion_sequence_config.json", config, "application/json")},
             data={"n_iterations": "3", "delay_between_iterations": "5.0"},
             timeout=T_READ,
@@ -577,11 +640,11 @@ POST **/sequences/start** запускает отдельный процесс m
 === "Python"
 
     ~~~ python
-    status = httpx.get(f"{HOST}/sequences/status", timeout=T_READ)
+    status = httpx.get(f"{HOST}/sequences/status", headers=HEADERS, timeout=T_READ)
     status.raise_for_status()
     print(status.json())
 
-    logs = httpx.get(f"{HOST}/sequences/logs", params={"n": 50}, timeout=T_READ)
+    logs = httpx.get(f"{HOST}/sequences/logs", headers=HEADERS, params={"n": 50}, timeout=T_READ)
     logs.raise_for_status()
     for line in logs.json()["lines"]:
         print(line)
@@ -617,7 +680,7 @@ POST **/stop** останавливает запущенный runner, публ
 === "Python"
 
     ~~~ python
-    response = httpx.post(f"{HOST}/stop", timeout=T_READ)
+    response = httpx.post(f"{HOST}/stop", headers=HEADERS, timeout=T_READ)
     response.raise_for_status()
     print(response.json())
     ~~~
@@ -648,4 +711,4 @@ POST **/stop** останавливает запущенный runner, публ
 3. Убедитесь, что стек запущен полностью: controller_manager, MoveIt и iiwa_motion_server.
 4. После запуска последовательности посмотрите **/sequences/logs**; после публикации траектории — **/trajectory/logs**.
 
-MCP-сервер работает в том же процессе, но это отдельный интерфейс: его адрес — **http://адрес-сервера:8007/mcp/mcp**. Для обычных HTTP-интеграций используйте маршруты из этой страницы.
+MCP-сервер работает в том же процессе, но это отдельный интерфейс: его адрес — **http://адрес-сервера:8007/mcp/mcp**. Он использует тот же Bearer-токен; передайте заголовок `Authorization` при подключении MCP-клиента. Для обычных HTTP-интеграций используйте маршруты из этой страницы.

src/iiwa_bringup/launch/iiwa.launch.py

@@ -4,18 +4,21 @@ import sys
 sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-
-from iiwa_utils import converter, setting_loader
-
 from supported.moveit_nodes import make_moveit_nodes
+from supported.optional_nodes import make_foxglove_node, make_web_server_node
 from supported.rviz_nodes import make_rviz_nodes
 from supported.simulation_nodes import make_simulation_nodes
-from supported.optional_nodes import make_foxglove_node, make_web_server_node
+
+from iiwa_utils import converter, setting_loader
 
 
 def _runtime_setup(context, *args, **kwargs):
@@ -52,6 +55,7 @@ def _runtime_setup(context, *args, **kwargs):
             "fri_port": str(settings.robot.port),
             "simulate": "false",
             "joint_position_tau": str(settings.robot.joint_position_tau),
+            "joint_velocity_tau": str(settings.robot.joint_velocity_tau),
         }
 
     robot_description = converter.load_robot_description(
@@ -118,6 +122,7 @@ def _runtime_setup(context, *args, **kwargs):
             "controller_timer": str(settings.digital_twin.webots.controller_timer),
             "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
             "joint_position_tau": str(settings.robot.joint_position_tau),
+            "joint_velocity_tau": str(settings.robot.joint_velocity_tau),
             "controller": settings.robot.active_controller,
         }
 

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

@@ -1,12 +1,10 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
 from launch.event_handlers import OnProcessExit
-from launch.actions import RegisterEventHandler
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from webots_ros2_driver.urdf_spawner import URDFSpawner
 
-
 from iiwa_utils import converter
 
 
@@ -22,6 +20,7 @@ def _setup_controllers(context, *args, **kwargs):
     controller = LaunchConfiguration("controller").perform(context)  # "jtc" | "forward"
     fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context))
     joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context)
+    joint_velocity_tau = LaunchConfiguration("joint_velocity_tau").perform(context)
     update_rate = 1000 // fri_cycle_ms
 
     xacro_args = {"initial_positions_file": initial_positions_file}
@@ -30,6 +29,7 @@ def _setup_controllers(context, *args, **kwargs):
         xacro_args["simulate"] = "true"
     else:
         xacro_args["joint_position_tau"] = joint_position_tau
+        xacro_args["joint_velocity_tau"] = joint_velocity_tau
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -139,6 +139,7 @@ def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument("fri_cycle_ms", default_value="5"),
         DeclareLaunchArgument("joint_position_tau", default_value="0.04"),
+        DeclareLaunchArgument("joint_velocity_tau", default_value="0.01"),
         DeclareLaunchArgument("controller", default_value="jtc"),
         OpaqueFunction(function=_setup_controllers),
     ])

src/iiwa_bringup/launch/supported/optional_nodes.py

@@ -18,11 +18,15 @@ def make_foxglove_node(settings, use_sim_time: bool) -> Node:
 
 
 def make_web_server_node(settings, use_sim_time: bool) -> Node:
+    token = getattr(settings.web, "token", None)
+    additional_env = {"IIWA_WEB_TOKEN": str(token)} if token else None
+
     return Node(
         package="iiwa_web",
         executable="iiwa_web_server",
         output="screen",
         name="iiwa_web_server",
+        additional_env=additional_env,
         parameters=[{
             "host": settings.web.host,
             "port": settings.web.port,

src/iiwa_config/config/setting.yaml

@@ -48,8 +48,9 @@ planning:
 
 web:
   enabled: true
-  host: "0.0.0.0"
+  host: "127.0.0.1"
   port: 8007
+  token: "ysXOyL_p3-f2YH2WqQ808KgQUA32qACziKdfqbhsJsPI0GGkoRbGn9eU22FV8mdS"
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 

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,6 +114,10 @@ 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})

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;
 
+  try {
     robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
     fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
-  simulate_            = (getParam(info, "simulate", "false") == "true");
+    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,12 +340,29 @@ void IIWAHardwareInterface::friThreadFunc()
 {
   RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен");
 
-  while (fri_running_.load(std::memory_order_relaxed)) {
+  try {
+    while (fri_running_.load(std::memory_order_acquire)) {
       if (!app_->step()) {
-      RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-        "FRI: step() вернул false, возможно потеряли соединение");
+        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;
 }

src/iiwa_planning/CMakeLists.txt

@@ -54,5 +54,20 @@ install(FILES
   DESTINATION share/${PROJECT_NAME}
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  ament_add_pytest_test(
+    test_move_to_pose_server
+    test/test_move_to_pose_server.py
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR}
+  )
+
+  ament_add_pytest_test(
+    test_motion_sequence_runner
+    test/test_motion_sequence_runner.py
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR}
+  )
+endif()
 
 ament_package()

src/iiwa_planning/package.xml

@@ -28,6 +28,8 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>python3-pytest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/iiwa_planning/scripts/motion_sequence_runner.py

@@ -14,6 +14,7 @@
 import json
 import shutil
 import threading
+import tempfile
 import time
 from pathlib import Path
 
@@ -30,6 +31,9 @@ from rosidl_runtime_py.utilities import get_message
 from iiwa_msgs.action import MoveToJoints, MoveToPose
 
 
+ACTION_RESULT_TIMEOUT_SEC = 300.0
+
+
 def _is_joints_waypoint(wp: dict) -> bool:
     return "joints" in wp
 
@@ -92,8 +96,15 @@ class MotionSequenceRunner(Node):
 
 
     def _init_bag(self):
-        bag_dir = Path(self._bag_path)
+        bag_dir = Path(self._bag_path).expanduser().resolve(strict=False)
+        tmp_root = Path(tempfile.gettempdir()).resolve(strict=False)
+        if bag_dir == tmp_root or tmp_root not in bag_dir.parents:
+            raise ValueError(f'bag_path must stay under temporary root {tmp_root}')
+
+        self._bag_path = str(bag_dir)
         if bag_dir.exists():
+            if not bag_dir.is_dir():
+                raise ValueError(f'bag_path already exists and is not a directory: {bag_dir}')
             shutil.rmtree(bag_dir)
             self.get_logger().info(f'Removed existing bag at {self._bag_path}')
 
@@ -171,6 +182,7 @@ class MotionSequenceRunner(Node):
             return False
 
         done = threading.Event()
+        goal_handle_holder = [None]
         result_holder: list[bool] = [False]
 
         def _on_result(future):
@@ -180,6 +192,7 @@ class MotionSequenceRunner(Node):
 
         def _on_goal(future):
             gh = future.result()
+            goal_handle_holder[0] = gh
             if not gh.accepted:
                 self.get_logger().error('MoveToJoints goal rejected')
                 done.set()
@@ -187,7 +200,12 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
-        done.wait()
+        if not done.wait(timeout=ACTION_RESULT_TIMEOUT_SEC):
+            gh = goal_handle_holder[0]
+            if gh is not None:
+                gh.cancel_goal_async()
+            self.get_logger().error('MoveToJoints action timed out and was canceled')
+            return False
         return result_holder[0]
 
     def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool:
@@ -206,6 +224,7 @@ class MotionSequenceRunner(Node):
             return False
 
         done = threading.Event()
+        goal_handle_holder = [None]
         result_holder: list[bool] = [False]
 
         def _on_result(future):
@@ -215,6 +234,7 @@ class MotionSequenceRunner(Node):
 
         def _on_goal(future):
             gh = future.result()
+            goal_handle_holder[0] = gh
             if not gh.accepted:
                 self.get_logger().error('MoveToPose goal rejected')
                 done.set()
@@ -222,7 +242,12 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
-        done.wait()
+        if not done.wait(timeout=ACTION_RESULT_TIMEOUT_SEC):
+            gh = goal_handle_holder[0]
+            if gh is not None:
+                gh.cancel_goal_async()
+            self.get_logger().error('MoveToPose action timed out and was canceled')
+            return False
         return result_holder[0]
 
     def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool:
@@ -236,12 +261,18 @@ class MotionSequenceRunner(Node):
             for i in range(self._n_iter):
                 self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
 
-                self._send_waypoint(self._home)
+                if not self._send_waypoint(self._home):
+                    self.get_logger().info('Home waypoint failed, stopping sequence')
+                    return
 
                 for idx, wp in enumerate(self._waypoints):
-                    self._send_waypoint(wp, idx=idx)
+                    if not self._send_waypoint(wp, idx=idx):
+                        self.get_logger().info(f'Waypoint #{idx} failed, stopping sequence')
+                        return
 
-                self._send_waypoint(self._home)
+                if not self._send_waypoint(self._home):
+                    self.get_logger().info('Final home waypoint failed, stopping sequence')
+                    return
                 time.sleep(self._delay)
 
             self.get_logger().info('======= Sequence complete =======')

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 
+import math
 import threading
+import time
 
 import rclpy
 from rclpy.node import Node
@@ -28,6 +30,10 @@ PLANNERS = {
     "chomp": ("chomp", "", 10.0),
 }
 
+DEFAULT_EXECUTION_TIMEOUT_SEC = 30.0
+EXECUTION_POLL_PERIOD_SEC = 0.05
+STOP_WAIT_TIMEOUT_SEC = 0.1
+
 
 def _abc_to_quaternion(a: float, b: float, c: float):
     """ZYX Euler (радианы, конвенция KUKA ABC) → (qx, qy, qz, qw)."""
@@ -59,12 +65,14 @@ class IiwaMotionServer(Node):
         self.declare_parameter("default_frame", "base_link")
         self.declare_parameter("default_planner", "ompl")
         self.declare_parameter("planning_attempts", 3)
+        self.declare_parameter("execution_timeout_sec", DEFAULT_EXECUTION_TIMEOUT_SEC)
 
         self._pose_link = self.get_parameter("pose_link").value
         self._planning_group = self.get_parameter("planning_group").value
         self._default_frame = self.get_parameter("default_frame").value
         self._default_planner = self.get_parameter("default_planner").value
         self._planning_attempts = self.get_parameter("planning_attempts").value
+        self._execution_timeout_sec = float(self.get_parameter("execution_timeout_sec").value)
 
     def _setup_moveit(self):
         self._moveit = MoveItPy(node_name="iiwa_motion_server")
@@ -73,17 +81,19 @@ class IiwaMotionServer(Node):
 
     def _setup_servers(self):
         cb = ReentrantCallbackGroup()
+        self._motion_lock = threading.Lock()
+        self._active_motion_kind: str | None = None
 
         ActionServer(
             self, MoveToPose, "cobot/move_to_pose", self._execute_pose,
             callback_group=cb,
-            goal_callback=lambda _: GoalResponse.ACCEPT,
+            goal_callback=self._handle_pose_goal,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
         ActionServer(
             self, MoveToJoints, "cobot/move_to_joints", self._execute_joints,
             callback_group=cb,
-            goal_callback=lambda _: GoalResponse.ACCEPT,
+            goal_callback=self._handle_joints_goal,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
         self.create_service(MoveToNamedPose, "cobot/move_to_named", self._handle_named, callback_group=cb)
@@ -99,6 +109,148 @@ class IiwaMotionServer(Node):
         params.max_acceleration_scaling_factor = accel_scale if accel_scale is not None else velocity_scale
         return params
 
+    def _busy_motion_message(self) -> str:
+        current = self._active_motion_kind or "motion"
+        return f"Another {current} is already in progress; parallel motions are not allowed"
+
+    def _reserve_motion(self, motion_kind: str) -> bool:
+        if not self._motion_lock.acquire(blocking=False):
+            return False
+        self._active_motion_kind = motion_kind
+        return True
+
+    def _release_motion(self):
+        if self._motion_lock.locked():
+            self._active_motion_kind = None
+            self._motion_lock.release()
+
+    def _handle_pose_goal(self, _goal_request):
+        if self._reserve_motion("pose"):
+            return GoalResponse.ACCEPT
+        self.get_logger().error(self._busy_motion_message())
+        return GoalResponse.REJECT
+
+    def _handle_joints_goal(self, _goal_request):
+        if self._reserve_motion("joints"):
+            return GoalResponse.ACCEPT
+        self.get_logger().error(self._busy_motion_message())
+        return GoalResponse.REJECT
+
+    def _ensure_motion_reservation(self, motion_kind: str) -> bool:
+        if self._motion_lock.locked():
+            return False
+        if self._reserve_motion(motion_kind):
+            return True
+        raise RuntimeError(self._busy_motion_message())
+
+    def _normalize_scale(self, raw_value, label: str, minimum: float = 0.01) -> tuple[float | None, str | None]:
+        try:
+            value = float(raw_value)
+        except (TypeError, ValueError):
+            return None, f"Invalid {label}: expected a finite number"
+
+        if not math.isfinite(value):
+            return None, f"Invalid {label}: expected a finite number"
+
+        return max(minimum, min(1.0, value)), None
+
+    def _normalize_named_accel(self, raw_value, velocity_scale: float) -> tuple[float | None, str | None]:
+        try:
+            value = float(raw_value)
+        except (TypeError, ValueError):
+            return None, "Invalid accel_scale: expected a finite number"
+
+        if not math.isfinite(value):
+            return None, "Invalid accel_scale: expected a finite number"
+
+        if value <= 0.0:
+            return velocity_scale, None
+
+        return max(0.01, min(1.0, value)), None
+
+    def _validate_pose_goal(self, request) -> str | None:
+        values = (request.x, request.y, request.z, request.a, request.b, request.c)
+        try:
+            finite = all(math.isfinite(float(value)) for value in values)
+        except (TypeError, ValueError):
+            finite = False
+        if not finite:
+            return "Invalid pose: all coordinates and ABC angles must be finite"
+        return None
+
+    def _joint_bounds_for_group(self) -> list[tuple[float | None, float | None]] | None:
+        if hasattr(self._robot_model, "get_joint_bounds"):
+            return list(self._robot_model.get_joint_bounds(self._planning_group))
+        return None
+
+    def _validate_joint_goal(self, raw_joints) -> tuple[list[float] | None, str | None]:
+        try:
+            joints = [float(value) for value in raw_joints]
+        except (TypeError, ValueError):
+            return None, "Invalid joints: expected numeric joint targets"
+
+        if not all(math.isfinite(value) for value in joints):
+            return None, "Invalid joints: expected finite joint targets"
+
+        bounds = self._joint_bounds_for_group()
+        expected_count = len(bounds) if bounds is not None else 7
+        if len(joints) != expected_count:
+            return None, f"Invalid joints: expected {expected_count} values"
+
+        if bounds is None:
+            return joints, None
+
+        for index, (value, bound_pair) in enumerate(zip(joints, bounds)):
+            lower, upper = bound_pair
+            if lower is not None and value < lower:
+                return None, f"Joint {index} violates limit [{lower}, {upper}]"
+            if upper is not None and value > upper:
+                return None, f"Joint {index} violates limit [{lower}, {upper}]"
+
+        return joints, None
+
+    def _plan_result_valid(self, plan_result) -> bool:
+        if not plan_result:
+            return False
+
+        trajectory = getattr(plan_result, "trajectory", None)
+        if trajectory is None:
+            return False
+
+        error_code = getattr(plan_result, "error_code", None)
+        if error_code is not None and getattr(error_code, "val", error_code) != 1:
+            return False
+
+        success = getattr(plan_result, "success", None)
+        if success is not None and not bool(success):
+            return False
+
+        return True
+
+    def _execution_result_successful(self, execution_result) -> bool:
+        if execution_result is None:
+            return False
+
+        success = getattr(execution_result, "success", None)
+        if success is not None:
+            return bool(success)
+
+        error_code = getattr(execution_result, "error_code", None)
+        if error_code is not None:
+            return getattr(error_code, "val", error_code) == 1
+
+        status = getattr(execution_result, "status", None)
+        if status is not None:
+            return status == 1
+
+        return bool(execution_result)
+
+    def _stop_execution(self):
+        try:
+            self._moveit.get_trajectory_execution_manager().stop_execution()
+        except Exception:
+            pass
+
     def _plan_and_execute(self, plan_params: PlanRequestParameters, goal_handle):
         """Планирует траекторию и выполняет её с поддержкой отмены.
 
@@ -106,7 +258,7 @@ class IiwaMotionServer(Node):
         (None, 'canceled') если цель была отменена.
         """
         plan_result = self._arm.plan(single_plan_parameters=plan_params)
-        if not plan_result:
+        if not self._plan_result_valid(plan_result):
             return False, "Планирование не удалось: поза недостижима или в столкновении"
 
         if goal_handle.is_cancel_requested:
@@ -114,10 +266,14 @@ class IiwaMotionServer(Node):
 
         done = threading.Event()
         failed = threading.Event()
+        execution_success = {"ok": False}
 
         def do_execute():
             try:
-                self._moveit.execute(plan_result.trajectory, controllers=[])
+                execution_result = self._moveit.execute(plan_result.trajectory, controllers=[])
+                execution_success["ok"] = self._execution_result_successful(execution_result)
+                if not execution_success["ok"]:
+                    failed.set()
             except Exception as exc:
                 self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
                 failed.set()
@@ -125,15 +281,17 @@ class IiwaMotionServer(Node):
                 done.set()
 
         threading.Thread(target=do_execute, daemon=True).start()
+        started_at = time.monotonic()
 
-        while not done.wait(timeout=0.05):
+        while not done.wait(timeout=EXECUTION_POLL_PERIOD_SEC):
             if goal_handle.is_cancel_requested:
-                try:
-                    self._moveit.get_trajectory_execution_manager().stop_execution()
-                except Exception:
-                    pass
-                done.wait()
+                self._stop_execution()
+                done.wait(timeout=STOP_WAIT_TIMEOUT_SEC)
                 return None, "canceled"
+            if time.monotonic() - started_at >= self._execution_timeout_sec:
+                self._stop_execution()
+                done.wait(timeout=STOP_WAIT_TIMEOUT_SEC)
+                return False, f"Trajectory execution timeout after {self._execution_timeout_sec:.2f} s"
 
         if failed.is_set():
             return False, "Выполнение траектории завершилось ошибкой"
@@ -154,19 +312,28 @@ class IiwaMotionServer(Node):
         return result
 
     def _execute_pose(self, goal_handle):
+        reserved_here = False
         req = goal_handle.request
         feedback = MoveToPose.Feedback()
         result = MoveToPose.Result()
 
-        velocity_scale = max(0.01, min(1.0, float(req.speed)))
-        planner_key = (req.planner or self._default_planner).lower()
+        try:
+            reserved_here = self._ensure_motion_reservation("pose")
+            velocity_scale, error = self._normalize_scale(req.speed, "speed")
+            if error is not None:
+                return self._finish_action(goal_handle, result, False, error)
+            error = self._validate_pose_goal(req)
+            if error is not None:
+                return self._finish_action(goal_handle, result, False, error)
 
+            planner_key = (req.planner or self._default_planner).lower()
             if planner_key not in PLANNERS:
-            result.success = False
-            result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
-            self.get_logger().error(result.message)
-            goal_handle.abort()
-            return result
+                return self._finish_action(
+                    goal_handle,
+                    result,
+                    False,
+                    f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}",
+                )
 
             pipeline, planner_id, plan_time = PLANNERS[planner_key]
 
@@ -199,14 +366,25 @@ class IiwaMotionServer(Node):
             plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
             ok, msg = self._plan_and_execute(plan_params, goal_handle)
             return self._finish_action(goal_handle, result, ok, msg)
+        finally:
+            if reserved_here or self._motion_lock.locked():
+                self._release_motion()
 
     def _execute_joints(self, goal_handle):
+        reserved_here = False
         req = goal_handle.request
         feedback = MoveToJoints.Feedback()
         result = MoveToJoints.Result()
 
-        velocity_scale = max(0.01, min(1.0, float(req.speed)))
-        joints = list(req.joints)
+        try:
+            reserved_here = self._ensure_motion_reservation("joints")
+            velocity_scale, error = self._normalize_scale(req.speed, "speed")
+            if error is not None:
+                return self._finish_action(goal_handle, result, False, error)
+
+            joints, error = self._validate_joint_goal(req.joints)
+            if error is not None:
+                return self._finish_action(goal_handle, result, False, error)
 
             self.get_logger().info(
                 f"[joints] {[f'{v:.3f}' for v in joints]}  speed={velocity_scale:.2f}"
@@ -215,7 +393,6 @@ class IiwaMotionServer(Node):
             feedback.state = "planning"
             goal_handle.publish_feedback(feedback)
 
-        # Формируем целевое состояние по суставным координатам
             goal_state = RobotState(self._robot_model)
             goal_state.set_joint_group_positions(self._planning_group, joints)
             goal_state.update()
@@ -231,12 +408,32 @@ class IiwaMotionServer(Node):
             )
             ok, msg = self._plan_and_execute(plan_params, goal_handle)
             return self._finish_action(goal_handle, result, ok, msg)
+        finally:
+            if reserved_here or self._motion_lock.locked():
+                self._release_motion()
 
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
+        if not self._reserve_motion("named"):
+            response.success = False
+            response.message = self._busy_motion_message()
+            self.get_logger().error(response.message)
+            return response
+
+        try:
             name = request.name.strip()
-        velocity_scale = max(0.01, min(1.0, float(request.speed)))
-        raw_accel = float(request.accel_scale)
-        accel_scale = max(0.01, min(1.0, raw_accel)) if raw_accel > 0.0 else velocity_scale
+            velocity_scale, error = self._normalize_scale(request.speed, "speed")
+            if error is not None:
+                response.success = False
+                response.message = error
+                self.get_logger().error(response.message)
+                return response
+
+            accel_scale, error = self._normalize_named_accel(request.accel_scale, velocity_scale)
+            if error is not None:
+                response.success = False
+                response.message = error
+                self.get_logger().error(response.message)
+                return response
 
             self.get_logger().info(
                 f"[named] name='{name}'  speed={velocity_scale:.2f}  accel={accel_scale:.2f}"
@@ -255,22 +452,24 @@ class IiwaMotionServer(Node):
                 "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
             )
             plan_result = self._arm.plan(single_plan_parameters=plan_params)
-        if not plan_result:
+            if not self._plan_result_valid(plan_result):
                 response.success = False
                 response.message = f"Не удалось построить траекторию для '{name}'"
                 self.get_logger().error(response.message)
                 return response
 
             exec_result = self._moveit.execute(plan_result.trajectory, controllers=[])
-        if exec_result:
+            if self._execution_result_successful(exec_result):
                 response.success = True
                 response.message = f"Переместился в '{name}'"
                 self.get_logger().info(response.message)
             else:
                 response.success = False
-            response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)"
+                response.message = f"Выполнение траектории для '{name}' завершилось ошибкой"
                 self.get_logger().error(response.message)
             return response
+        finally:
+            self._release_motion()
 
     def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
         try:

src/iiwa_sunrise/src/ServerFriRos2.java

@@ -362,9 +362,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
         try {
             _lbr.move(posHold.addMotionOverlay(_friOverlay));
         } catch (Exception e) {
-            // Normal exit path when the ROS 2 client closes the FRI session.
-            // Штатный путь выхода при закрытии FRI-сессии со стороны ROS 2.
-            getLogger().info("FRI сеанс закрыт.");
+            getLogger().error("Ошибка Position режима [" + e.getClass().getSimpleName()
+                + "]: " + String.valueOf(e.getMessage()));
         }
 
         closeFriSession();
@@ -414,7 +413,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
         try {
             _lbr.move(posHold);
         } catch (Exception e) {
-            getLogger().info("FRI сеанс закрыт.");
+            getLogger().error("Ошибка Monitor режима [" + e.getClass().getSimpleName()
+                + "]: " + String.valueOf(e.getMessage()));
         }
 
         closeFriSession();

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -75,6 +75,7 @@ class WebCfg:
     enabled: bool
     host: str
     port: int
+    token: Optional[str]
     endpoints: str    # resolved absolute path to api_endpoints.yaml
     joint_limits: str  # resolved absolute path to joint_limits.yaml
 
@@ -189,6 +190,7 @@ _WEB_DEFAULTS: Dict[str, Any] = {
     "enabled": False,
     "host": "0.0.0.0",
     "port": 8007,
+    "token": None,
     "endpoints": "pkg://iiwa_config/config/api_endpoints.yaml",
     "joint_limits": "pkg://iiwa_config/config/moveit/joint_limits.yaml",
 }
@@ -201,10 +203,14 @@ def _parse_web(raw: Optional[Dict[str, Any]], settings_dir: str) -> WebCfg:
     def get(key: str) -> Any:
         return raw.get(key, _WEB_DEFAULTS[key])
 
+    raw_token = get("token")
+    token = str(raw_token).strip() if raw_token is not None else ""
+
     return WebCfg(
         enabled=bool(get("enabled")),
         host=str(get("host")),
         port=int(get("port")),
+        token=token or None,
         endpoints=resolve_path(str(get("endpoints")), settings_dir),
         joint_limits=resolve_path(str(get("joint_limits")), settings_dir),
     )

src/iiwa_web/iiwa_web/main.py

@@ -1,9 +1,14 @@
+import hmac
+import ipaddress
+import os
 import threading
 from contextlib import asynccontextmanager
 
 import rclpy
 import uvicorn
-from fastapi import FastAPI
+from fastapi import FastAPI, Request
+from fastapi.openapi.utils import get_openapi
+from fastapi.responses import JSONResponse
 from fastmcp import FastMCP
 from sensor_msgs.msg import JointState
 from std_srvs.srv import Trigger
@@ -13,11 +18,100 @@ from .ros_node import CobotWebNode, get_bridge, set_bridge
 from . import runner, trajectory, positions
 
 
+_PUBLIC_AUTH_PATHS = frozenset({
+    "/docs",
+    "/docs/oauth2-redirect",
+    "/openapi.json",
+    "/redoc",
+})
+
+
+def is_public_auth_path(path: str) -> bool:
+    return path in _PUBLIC_AUTH_PATHS
+
+
+def install_bearer_openapi(app: FastAPI, token: str | None) -> None:
+    """Expose the Bearer scheme so Swagger UI can configure authenticated calls."""
+    if not token:
+        return
+
+    def custom_openapi():
+        if app.openapi_schema:
+            return app.openapi_schema
+
+        schema = get_openapi(
+            title=app.title,
+            version=app.version,
+            description=app.description,
+            routes=app.routes,
+        )
+        schema.setdefault("components", {}).setdefault("securitySchemes", {})[
+            "BearerAuth"
+        ] = {
+            "type": "http",
+            "scheme": "bearer",
+            "bearerFormat": "opaque-token",
+        }
+
+        for path_item in schema.get("paths", {}).values():
+            for operation in path_item.values():
+                if isinstance(operation, dict):
+                    operation["security"] = [{"BearerAuth": []}]
+
+        app.openapi_schema = schema
+        return app.openapi_schema
+
+    app.openapi = custom_openapi
+
+
+def ensure_safe_bind(host: str, token: str | None) -> None:
+    normalized = host.strip().lower()
+    if normalized == "localhost":
+        return
+
+    try:
+        if ipaddress.ip_address(normalized).is_loopback:
+            return
+    except ValueError:
+        pass
+
+    if not token:
+        raise RuntimeError("Внешний bind Web API разрешён только при заданном IIWA_WEB_TOKEN")
+
+
+def has_valid_bearer_token(auth_header: str | None, token: str | None) -> bool:
+    if not token:
+        return True
+    if not auth_header:
+        return False
+
+    scheme, _, provided_token = auth_header.partition(" ")
+    if scheme.lower() != "bearer" or not provided_token:
+        return False
+    return hmac.compare_digest(provided_token, token)
+
+
+def install_token_auth(app: FastAPI, token: str | None) -> None:
+    if not token:
+        return
+
+    @app.middleware("http")
+    async def require_bearer_token(request: Request, call_next):
+        if is_public_auth_path(request.url.path):
+            return await call_next(request)
+
+        if not has_valid_bearer_token(request.headers.get("authorization"), token):
+            return JSONResponse(
+                status_code=401,
+                content={"detail": "Bearer token required"},
+                headers={"WWW-Authenticate": "Bearer"},
+            )
+        return await call_next(request)
+
+
 def main():
     rclpy.init()
     node = CobotWebNode()
-    set_bridge(node)
-    threading.Thread(target=rclpy.spin, args=(node,), daemon=True).start()
 
     host = node.get_parameter('host').value
     port = node.get_parameter('port').value
@@ -25,6 +119,16 @@ def main():
     joint_limits_path = node.get_parameter('joint_limits_path').value or None
 
     positions.init()
+    token = os.getenv("IIWA_WEB_TOKEN") or None
+    try:
+        ensure_safe_bind(host, token)
+    except Exception:
+        node.destroy_node()
+        rclpy.shutdown()
+        raise
+
+    set_bridge(node)
+    threading.Thread(target=rclpy.spin, args=(node,), daemon=True).start()
 
     _schema_app = FastAPI()
     _schema_app.include_router(build_dynamic_router(endpoints_path, joint_limits_path))
@@ -58,6 +162,8 @@ def main():
         except RuntimeError:
             return {"status": "stopped", "success": True, "message": "Планировщик не запущен"}
 
+    install_bearer_openapi(app, token)
+    install_token_auth(app, token)
     uvicorn.run(app, host=host, port=port)
 
 

src/iiwa_web/iiwa_web/ros_node.py

@@ -11,7 +11,7 @@ class CobotWebNode(Node):
     def __init__(self):
         super().__init__('cobot_web_node')
 
-        self.declare_parameter('host', '0.0.0.0')
+        self.declare_parameter('host', '127.0.0.1')
         self.declare_parameter('port', 8007)
         self.declare_parameter('endpoints_path', '')
         self.declare_parameter('joint_limits_path', '')
@@ -102,7 +102,11 @@ class CobotWebNode(Node):
         deadline = time.monotonic() + timeout
         while not result_future.done():
             if time.monotonic() > deadline:
-                raise TimeoutError(f"Таймаут выполнения action '{action_name}'")
+                cancel_future = goal_handle.cancel_goal_async()
+                cancel_deadline = time.monotonic() + min(2.0, max(timeout, 0.1))
+                while not cancel_future.done() and time.monotonic() <= cancel_deadline:
+                    time.sleep(0.01)
+                raise TimeoutError(f"Таймаут выполнения action '{action_name}', отправлена отмена goal")
             time.sleep(0.05)
 
         return result_future.result()

src/iiwa_web/iiwa_web/runner.py

@@ -1,4 +1,5 @@
 import os
+import re
 import signal
 import subprocess
 import threading
@@ -13,8 +14,12 @@ from .ros_node import get_bridge
 
 router = APIRouter(prefix="/sequences", tags=["sequences"])
 
-_UPLOAD_DIR = Path(tempfile.gettempdir()) / "iiwa_configs"
-_UPLOAD_DIR.mkdir(exist_ok=True)
+_TEMP_ROOT = Path(tempfile.gettempdir()) / "iiwa_web"
+_UPLOAD_DIR = _TEMP_ROOT / "uploads"
+_BAG_ROOT = _TEMP_ROOT / "bags"
+_UPLOAD_DIR.mkdir(parents=True, exist_ok=True)
+_BAG_ROOT.mkdir(parents=True, exist_ok=True)
+MAX_UPLOAD_BYTES = 1_048_576
 
 _LOG_BUFFER = 300
 
@@ -22,12 +27,43 @@ _process: Optional[subprocess.Popen] = None
 _log_lines: deque[str] = deque(maxlen=_LOG_BUFFER)
 _lock = threading.Lock()
 
+_SAFE_NAME_RE = re.compile(r"[^A-Za-z0-9._-]+")
+
 
 def _stream_output(proc: subprocess.Popen) -> None:
     for line in proc.stdout:
         _log_lines.append(line.rstrip("\n"))
 
 
+def _sanitize_upload_name(filename: str | None) -> str:
+    candidate = Path(filename or "config.json").name
+    if candidate in {"", ".", ".."}:
+        raise HTTPException(422, "Имя файла конфигурации недопустимо")
+
+    sanitized = _SAFE_NAME_RE.sub("_", candidate)
+    if sanitized in {"", ".", ".."}:
+        raise HTTPException(422, "Имя файла конфигурации недопустимо")
+    return sanitized
+
+
+def _resolve_bag_path(raw_bag_path: str) -> str:
+    if not raw_bag_path:
+        return ""
+
+    candidate = Path(raw_bag_path)
+    resolved = candidate.resolve(strict=False) if candidate.is_absolute() else (_BAG_ROOT / candidate).resolve(strict=False)
+    allowed_root = _BAG_ROOT.resolve(strict=False)
+    if resolved == allowed_root:
+        raise HTTPException(422, "bag_path должен указывать на каталог внутри разрешённого временного каталога")
+    try:
+        resolved.relative_to(allowed_root)
+    except ValueError as exc:
+        raise HTTPException(422, "bag_path должен находиться внутри разрешённого временного каталога") from exc
+
+    resolved.parent.mkdir(parents=True, exist_ok=True)
+    return str(resolved)
+
+
 def _build_cmd(config_path: str, n_iterations: int, delay: float,
                bag_path: str, topics: list[str],
                joints_action: str, pose_action: str) -> list[str]:
@@ -63,9 +99,14 @@ async def start_runner(
         if _process and _process.poll() is None:
             raise HTTPException(409, f"Runner уже запущен (pid={_process.pid})")
 
-        filename = config.filename or "config.json"
+        filename = _sanitize_upload_name(config.filename)
         dest = _UPLOAD_DIR / filename
-        dest.write_bytes(await config.read())
+        content = await config.read()
+        if len(content) > MAX_UPLOAD_BYTES:
+            raise HTTPException(413, f"Файл конфигурации слишком большой: максимум {MAX_UPLOAD_BYTES} байт")
+        dest.write_bytes(content)
+
+        safe_bag_path = _resolve_bag_path(bag_path)
 
         topics_list = [t.strip() for t in topics.split(",") if t.strip()]
 
@@ -74,7 +115,7 @@ async def start_runner(
             config_path=str(dest),
             n_iterations=n_iterations,
             delay=delay_between_iterations,
-            bag_path=bag_path,
+            bag_path=safe_bag_path,
             topics=topics_list,
             joints_action=joints_action,
             pose_action=pose_action,

src/iiwa_web/iiwa_web/trajectory.py

@@ -1,10 +1,11 @@
 import csv
 import io
+import math
 from collections import deque
 from datetime import datetime
 from builtin_interfaces.msg import Duration
 from fastapi import APIRouter, File, HTTPException, Query, UploadFile
-from pydantic import BaseModel, Field
+from pydantic import BaseModel, Field, field_validator, model_validator
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
 from .config_loader import load_joint_limits, load_joint_names
@@ -41,6 +42,32 @@ def _validate_limits(points: list[list[float]]) -> None:
                 )
 
 
+def _validate_rows(rows: list[tuple[list[float], float]]) -> None:
+    previous_time = -math.inf
+    for row_idx, (positions, point_time) in enumerate(rows):
+        if len(positions) != N_JOINTS:
+            raise HTTPException(
+                422,
+                f"Точка {row_idx + 1}: ожидалось {N_JOINTS} суставов, получено {len(positions)}",
+            )
+        for joint_idx, position in enumerate(positions):
+            if not math.isfinite(position):
+                raise HTTPException(
+                    422,
+                    f"Точка {row_idx + 1}, сустав {joint_idx + 1}: позиция должна быть конечным числом",
+                )
+        if not math.isfinite(point_time):
+            raise HTTPException(422, f"Точка {row_idx + 1}: time_from_start должен быть конечным числом")
+        if point_time < previous_time:
+            raise HTTPException(
+                422,
+                "Время точек траектории должно быть монотонно неубывающим",
+            )
+        previous_time = point_time
+
+    _validate_limits([positions for positions, _time in rows])
+
+
 def _build_msg(rows: list[tuple[list[float], float]]) -> JointTrajectory:
     msg = JointTrajectory()
     msg.joint_names = JOINT_NAMES
@@ -63,11 +90,35 @@ class Waypoint(BaseModel):
     )
     time_from_start: float = Field(..., ge=0.0, description="Время от начала траектории [с]")
 
+    @field_validator("positions")
+    @classmethod
+    def validate_positions(cls, value: list[float]) -> list[float]:
+        for joint_idx, position in enumerate(value):
+            if not math.isfinite(position):
+                raise ValueError(f"Позиция сустава {joint_idx + 1} должна быть конечным числом")
+        return value
+
+    @field_validator("time_from_start")
+    @classmethod
+    def validate_time_from_start(cls, value: float) -> float:
+        if not math.isfinite(value):
+            raise ValueError("time_from_start должен быть конечным числом")
+        return value
+
 
 class SendRequest(BaseModel):
     points: list[Waypoint] = Field(..., min_length=1, description="Точки траектории")
     validate_limits: bool = Field(True, description="Проверять лимиты суставов")
 
+    @model_validator(mode="after")
+    def validate_time_monotonicity(self) -> "SendRequest":
+        previous_time = -math.inf
+        for waypoint in self.points:
+            if waypoint.time_from_start < previous_time:
+                raise ValueError("Время точек траектории должно быть монотонно неубывающим")
+            previous_time = waypoint.time_from_start
+        return self
+
 
 @router.post("/send", summary="Отправить траекторию вручную (JSON)")
 def send_trajectory(req: SendRequest):
@@ -76,9 +127,7 @@ def send_trajectory(req: SendRequest):
     Публикует `JointTrajectory` в `/iiwa_arm_controller/joint_trajectory`.
     """
     rows = [(wp.positions, wp.time_from_start) for wp in req.points]
-
-    if req.validate_limits:
-        _validate_limits([r[0] for r in rows])
+    _validate_rows(rows)
 
     _publish(_build_msg(rows))
     _log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
@@ -163,8 +212,7 @@ async def send_csv_trajectory(
     if not rows:
         raise HTTPException(422, "CSV не содержит точек траектории")
 
-    if validate_limits:
-        _validate_limits([r[0] for r in rows])
+    _validate_rows(rows)
 
     _publish(_build_msg(rows))
     _log(f"[csv] {file.filename} → {len(rows)} точек, t_end={rows[-1][1]:.2f}с")

src/iiwa_web/package.xml

@@ -11,8 +11,18 @@
   <exec_depend>python3-uvicorn</exec_depend>
   <exec_depend>python3-multipart</exec_depend>
   <exec_depend>python3-fastmcp</exec_depend>
+  <exec_depend>python3-pydantic</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_runtime_py</exec_depend>
+  <exec_depend>rosbag2_py</exec_depend>
+  <exec_depend>iiwa_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>tf2_py</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
   <exec_depend>moveit_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>

src/iiwa_web/setup.py

@@ -14,11 +14,14 @@ setup(
     install_requires=[
         'setuptools',
         'fastapi>=0.100.0',
+        'pydantic>=2.0.0',
+        'PyYAML',
         'starlette>=0.27.0',
         'uvicorn[standard]',
         'python-multipart',
         'fastmcp',
     ],
+    tests_require=['pytest'],
     zip_safe=True,
     maintainer='daniel',
     maintainer_email='grabardm@ml-dev.ru',