new control robot

.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
 
@@ -79,4 +80,4 @@ foxglove:
     - assets
   include_hidden: false # Показывать клиенту скрытые топики и сервисы (начинаются с _)
   asset_uri_allowlist: ['^package://(?:[-\w%]+/)*[-\w%.]+\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$'] # Regex-список URI вида package://..., из которых bridge разрешает отдавать файлы-ассеты (URDF, mesh и т.п.)
-  ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте)
+  ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте)

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/CommandGuard.hpp

@@ -0,0 +1,89 @@
+#pragma once
+
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+
+namespace iiwa_controller
+{
+
+constexpr std::size_t COMMAND_GUARD_JOINTS = 7;
+
+struct CommandGuardLimits
+{
+  std::array<double, COMMAND_GUARD_JOINTS> min_position{};
+  std::array<double, COMMAND_GUARD_JOINTS> max_position{};
+  std::array<double, COMMAND_GUARD_JOINTS> max_velocity{};
+};
+
+enum class CommandGuardResult
+{
+  OK,
+  INVALID_PERIOD,
+  NON_FINITE,
+  POSITION_LIMIT,
+  VELOCITY_LIMIT,
+};
+
+inline const char * commandGuardResultName(const CommandGuardResult result)
+{
+  switch (result) {
+    case CommandGuardResult::OK: return "OK";
+    case CommandGuardResult::INVALID_PERIOD: return "INVALID_PERIOD";
+    case CommandGuardResult::NON_FINITE: return "NON_FINITE";
+    case CommandGuardResult::POSITION_LIMIT: return "POSITION_LIMIT";
+    case CommandGuardResult::VELOCITY_LIMIT: return "VELOCITY_LIMIT";
+    default: return "UNKNOWN";
+  }
+}
+
+class CommandGuard
+{
+public:
+  CommandGuard()
+  {
+    limits_.min_position.fill(-std::numeric_limits<double>::infinity());
+    limits_.max_position.fill(std::numeric_limits<double>::infinity());
+    limits_.max_velocity.fill(std::numeric_limits<double>::infinity());
+  }
+
+  explicit CommandGuard(const CommandGuardLimits & limits)
+  : limits_(limits) {}
+
+  CommandGuardResult validate(
+    const std::array<double, COMMAND_GUARD_JOINTS> & command,
+    const std::array<double, COMMAND_GUARD_JOINTS> & previous_command,
+    const double sample_time,
+    const bool previous_command_initialized) const
+  {
+    if (!std::isfinite(sample_time) || sample_time <= 0.0) {
+      return CommandGuardResult::INVALID_PERIOD;
+    }
+
+    for (std::size_t i = 0; i < COMMAND_GUARD_JOINTS; ++i) {
+      if (!std::isfinite(command[i])) {
+        return CommandGuardResult::NON_FINITE;
+      }
+      if (command[i] < limits_.min_position[i] || command[i] > limits_.max_position[i]) {
+        return CommandGuardResult::POSITION_LIMIT;
+      }
+      if (previous_command_initialized) {
+        if (!std::isfinite(previous_command[i]) || !std::isfinite(limits_.max_velocity[i])) {
+          return CommandGuardResult::NON_FINITE;
+        }
+        const double max_delta = limits_.max_velocity[i] * sample_time;
+        if (std::abs(command[i] - previous_command[i]) > max_delta + 1e-9) {
+          return CommandGuardResult::VELOCITY_LIMIT;
+        }
+      }
+    }
+
+    return CommandGuardResult::OK;
+  }
+
+private:
+  CommandGuardLimits limits_{};
+};
+
+}  // namespace iiwa_controller

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/include/iiwa_controller/StateGuard.hpp

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <cmath>
+#include <cstddef>
+
+#include "iiwa_controller/FRIClient.h"
+
+namespace iiwa_controller
+{
+
+enum class StateGuardResult
+{
+  OK,
+  INVALID_SNAPSHOT,
+  SESSION_STATE,
+  CONNECTION_QUALITY,
+  SAFETY_STOP,
+  OPERATION_MODE,
+  DRIVE_STATE,
+  CLIENT_COMMAND_MODE,
+  CONTROL_MODE,
+  TRACKING_PERFORMANCE,
+};
+
+inline const char * stateGuardResultName(const StateGuardResult result)
+{
+  switch (result) {
+    case StateGuardResult::OK: return "OK";
+    case StateGuardResult::INVALID_SNAPSHOT: return "INVALID_SNAPSHOT";
+    case StateGuardResult::SESSION_STATE: return "SESSION_STATE";
+    case StateGuardResult::CONNECTION_QUALITY: return "CONNECTION_QUALITY";
+    case StateGuardResult::SAFETY_STOP: return "SAFETY_STOP";
+    case StateGuardResult::OPERATION_MODE: return "OPERATION_MODE";
+    case StateGuardResult::DRIVE_STATE: return "DRIVE_STATE";
+    case StateGuardResult::CLIENT_COMMAND_MODE: return "CLIENT_COMMAND_MODE";
+    case StateGuardResult::CONTROL_MODE: return "CONTROL_MODE";
+    case StateGuardResult::TRACKING_PERFORMANCE: return "TRACKING_PERFORMANCE";
+    default: return "UNKNOWN";
+  }
+}
+
+class StateGuard
+{
+public:
+  StateGuardResult validateSnapshot(const IIWAStateSnapshot & snapshot) const
+  {
+    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
+      if (!std::isfinite(snapshot.measured_pos[i]) ||
+          !std::isfinite(snapshot.measured_tau[i]) ||
+          !std::isfinite(snapshot.external_tau[i])) {
+        return StateGuardResult::INVALID_SNAPSHOT;
+      }
+    }
+    if (!std::isfinite(snapshot.sample_time) || snapshot.sample_time <= 0.0 ||
+        snapshot.time_stamp_nano_sec >= 1000000000U ||
+        !std::isfinite(snapshot.tracking_performance)) {
+      return StateGuardResult::INVALID_SNAPSHOT;
+    }
+    return StateGuardResult::OK;
+  }
+
+  StateGuardResult validatePositionState(
+    const IIWAStateSnapshot & snapshot,
+    const KUKA::FRI::ESessionState session_state) const
+  {
+    auto result = validateSnapshot(snapshot);
+    if (result != StateGuardResult::OK) {
+      return result;
+    }
+    if (session_state != KUKA::FRI::COMMANDING_ACTIVE) {
+      return StateGuardResult::SESSION_STATE;
+    }
+    if (snapshot.quality < KUKA::FRI::GOOD) {
+      return StateGuardResult::CONNECTION_QUALITY;
+    }
+    if (snapshot.safety_state != KUKA::FRI::NORMAL_OPERATION) {
+      return StateGuardResult::SAFETY_STOP;
+    }
+    if (snapshot.operation_mode != KUKA::FRI::AUTOMATIC_MODE) {
+      return StateGuardResult::OPERATION_MODE;
+    }
+    if (snapshot.drive_state != KUKA::FRI::ACTIVE) {
+      return StateGuardResult::DRIVE_STATE;
+    }
+    if (snapshot.client_command_mode != KUKA::FRI::POSITION) {
+      return StateGuardResult::CLIENT_COMMAND_MODE;
+    }
+    if (snapshot.control_mode != KUKA::FRI::POSITION_CONTROL_MODE &&
+        snapshot.control_mode != KUKA::FRI::JOINT_IMP_CONTROL_MODE) {
+      return StateGuardResult::CONTROL_MODE;
+    }
+    if (snapshot.tracking_performance < 0.5) {
+      return StateGuardResult::TRACKING_PERFORMANCE;
+    }
+    return StateGuardResult::OK;
+  }
+};
+
+}  // namespace iiwa_controller

src/iiwa_controller/include/iiwa_controller/filters.hpp

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <cmath>
+
+namespace iiwa_controller
+{
+
+// Continuous-time first-order low-pass filter discretization used by
+// lbr_fri_ros2_stack. tau == 0 intentionally disables smoothing.
+inline double exponentialFilterAlpha(const double tau, const double sample_time)
+{
+  if (tau <= 0.0) {
+    return 1.0;
+  }
+  return 1.0 - std::exp(-sample_time / tau);
+}
+
+}  // namespace iiwa_controller

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;
 }

src/iiwa_controller/test/test_guards.cpp

@@ -0,0 +1,120 @@
+#include <array>
+#include <cmath>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+#include "iiwa_controller/CommandGuard.hpp"
+#include "iiwa_controller/FRIClient.h"
+#include "iiwa_controller/StateGuard.hpp"
+#include "iiwa_controller/filters.hpp"
+
+namespace iiwa_controller::test
+{
+
+TEST(ExponentialFilter, UsesContinuousTimeAlpha)
+{
+  EXPECT_NEAR(exponentialFilterAlpha(0.04, 0.01), 1.0 - std::exp(-0.25), 1e-12);
+  EXPECT_DOUBLE_EQ(exponentialFilterAlpha(0.0, 0.01), 1.0);
+}
+
+CommandGuard makeGuard()
+{
+  CommandGuardLimits limits;
+  limits.min_position.fill(-1.0);
+  limits.max_position.fill(1.0);
+  limits.max_velocity.fill(2.0);
+  return CommandGuard(limits);
+}
+
+TEST(CommandGuard, AcceptsFiniteCommandWithinPositionAndVelocityLimits)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = 0.01;
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::OK);
+}
+
+TEST(CommandGuard, RejectsPositionLimitViolation)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = 1.01;
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::POSITION_LIMIT);
+}
+
+TEST(CommandGuard, RejectsVelocityLimitViolation)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = 0.1;
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::VELOCITY_LIMIT);
+}
+
+TEST(CommandGuard, RejectsNonFiniteCommand)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = std::numeric_limits<double>::quiet_NaN();
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::NON_FINITE);
+}
+
+IIWAStateSnapshot nominalSnapshot()
+{
+  IIWAStateSnapshot snapshot;
+  snapshot.sample_time = 0.01;
+  snapshot.quality = KUKA::FRI::EXCELLENT;
+  snapshot.safety_state = KUKA::FRI::NORMAL_OPERATION;
+  snapshot.operation_mode = KUKA::FRI::AUTOMATIC_MODE;
+  snapshot.drive_state = KUKA::FRI::ACTIVE;
+  snapshot.client_command_mode = KUKA::FRI::POSITION;
+  snapshot.control_mode = KUKA::FRI::POSITION_CONTROL_MODE;
+  snapshot.tracking_performance = 0.9;
+  return snapshot;
+}
+
+TEST(StateGuard, AcceptsNominalPositionState)
+{
+  const StateGuard guard;
+  EXPECT_EQ(
+    guard.validatePositionState(nominalSnapshot(), KUKA::FRI::COMMANDING_ACTIVE),
+    StateGuardResult::OK);
+}
+
+TEST(StateGuard, ReportsSafetyStopSeparately)
+{
+  auto snapshot = nominalSnapshot();
+  snapshot.safety_state = KUKA::FRI::SAFETY_STOP_LEVEL_1;
+
+  EXPECT_EQ(
+    StateGuard().validatePositionState(snapshot, KUKA::FRI::COMMANDING_ACTIVE),
+    StateGuardResult::SAFETY_STOP);
+}
+
+TEST(StateGuard, ReportsWrongControlModeSeparately)
+{
+  auto snapshot = nominalSnapshot();
+  snapshot.control_mode = KUKA::FRI::NO_CONTROL;
+
+  EXPECT_EQ(
+    StateGuard().validatePositionState(snapshot, KUKA::FRI::COMMANDING_ACTIVE),
+    StateGuardResult::CONTROL_MODE);
+}
+
+}  // namespace iiwa_controller::test

src/iiwa_controller/test/test_safety_contract.py

@@ -0,0 +1,34 @@
+from pathlib import Path
+
+
+ROOT = Path(__file__).resolve().parents[1]
+HEADER = (ROOT / "include/iiwa_controller/FRIClient.h").read_text()
+FRI_CLIENT = (ROOT / "src/FRIClient.cpp").read_text()
+HARDWARE = (ROOT / "src/IIWAHardwareInterface.cpp").read_text()
+
+
+def test_fri_data_exchange_does_not_use_a_mutex_in_the_rt_path():
+    assert "#include <mutex>" not in HEADER
+    assert "lock_guard<std::mutex>" not in FRI_CLIENT
+
+
+def test_cleanup_releases_application_before_its_dependencies():
+    cleanup = HARDWARE[HARDWARE.index("on_cleanup"):]
+    assert cleanup.index("app_.reset()") < cleanup.index("connection_.reset()")
+    assert cleanup.index("connection_.reset()") < cleanup.index("fri_client_.reset()")
+
+
+def test_fri_step_failure_stops_the_session():
+    step_pos = HARDWARE.index("app_->step()")
+    after_step = HARDWARE[step_pos:step_pos + 700]
+    assert "fri_running_.store(false" in after_step
+
+
+def test_activation_timeout_is_an_error():
+    timeout_pos = HARDWARE.index("FRI не подключился")
+    after_timeout = HARDWARE[timeout_pos:timeout_pos + 500]
+    assert "CallbackReturn::ERROR" in after_timeout
+
+
+def test_commanding_snapshot_keeps_measured_position_from_fri():
+    assert "snapshot_.measured_pos = filtered_pos_" not in FRI_CLIENT

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,123 +312,164 @@ 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)
 
-        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
+            planner_key = (req.planner or self._default_planner).lower()
+            if planner_key not in PLANNERS:
+                return self._finish_action(
+                    goal_handle,
+                    result,
+                    False,
+                    f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}",
+                )
 
-        pipeline, planner_id, plan_time = PLANNERS[planner_key]
+            pipeline, planner_id, plan_time = PLANNERS[planner_key]
 
-        pose = PoseStamped()
-        pose.header.frame_id = req.frame_id or self._default_frame
-        pose.pose.position.x = req.x
-        pose.pose.position.y = req.y
-        pose.pose.position.z = req.z
-        qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c)
-        pose.pose.orientation.x = qx
-        pose.pose.orientation.y = qy
-        pose.pose.orientation.z = qz
-        pose.pose.orientation.w = qw
+            pose = PoseStamped()
+            pose.header.frame_id = req.frame_id or self._default_frame
+            pose.pose.position.x = req.x
+            pose.pose.position.y = req.y
+            pose.pose.position.z = req.z
+            qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c)
+            pose.pose.orientation.x = qx
+            pose.pose.orientation.y = qy
+            pose.pose.orientation.z = qz
+            pose.pose.orientation.w = qw
 
-        self.get_logger().info(
-            f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f})  "
-            f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад  "
-            f"speed={velocity_scale:.2f}  planner={planner_key}"
-        )
+            self.get_logger().info(
+                f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f})  "
+                f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад  "
+                f"speed={velocity_scale:.2f}  planner={planner_key}"
+            )
 
-        feedback.state = "planning"
-        goal_handle.publish_feedback(feedback)
+            feedback.state = "planning"
+            goal_handle.publish_feedback(feedback)
 
-        self._arm.set_start_state_to_current_state()
-        self._arm.set_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link)
+            self._arm.set_start_state_to_current_state()
+            self._arm.set_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link)
 
-        feedback.state = "executing"
-        goal_handle.publish_feedback(feedback)
+            feedback.state = "executing"
+            goal_handle.publish_feedback(feedback)
 
-        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)
+            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)
 
-        self.get_logger().info(
-            f"[joints] {[f'{v:.3f}' for v in joints]}  speed={velocity_scale:.2f}"
-        )
+            joints, error = self._validate_joint_goal(req.joints)
+            if error is not None:
+                return self._finish_action(goal_handle, result, False, error)
 
-        feedback.state = "planning"
-        goal_handle.publish_feedback(feedback)
+            self.get_logger().info(
+                f"[joints] {[f'{v:.3f}' for v in joints]}  speed={velocity_scale:.2f}"
+            )
 
-        # Формируем целевое состояние по суставным координатам
-        goal_state = RobotState(self._robot_model)
-        goal_state.set_joint_group_positions(self._planning_group, joints)
-        goal_state.update()
+            feedback.state = "planning"
+            goal_handle.publish_feedback(feedback)
 
-        self._arm.set_start_state_to_current_state()
-        self._arm.set_goal_state(robot_state=goal_state)
+            goal_state = RobotState(self._robot_model)
+            goal_state.set_joint_group_positions(self._planning_group, joints)
+            goal_state.update()
 
-        feedback.state = "executing"
-        goal_handle.publish_feedback(feedback)
+            self._arm.set_start_state_to_current_state()
+            self._arm.set_goal_state(robot_state=goal_state)
 
-        plan_params = self._make_plan_params(
-            "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
-        )
-        ok, msg = self._plan_and_execute(plan_params, goal_handle)
-        return self._finish_action(goal_handle, result, ok, msg)
+            feedback.state = "executing"
+            goal_handle.publish_feedback(feedback)
+
+            plan_params = self._make_plan_params(
+                "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+            )
+            ok, msg = self._plan_and_execute(plan_params, goal_handle)
+            return self._finish_action(goal_handle, result, ok, msg)
+        finally:
+            if reserved_here or self._motion_lock.locked():
+                self._release_motion()
 
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
-        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
+        if not self._reserve_motion("named"):
+            response.success = False
+            response.message = self._busy_motion_message()
+            self.get_logger().error(response.message)
+            return response
 
-        self.get_logger().info(
-            f"[named] name='{name}'  speed={velocity_scale:.2f}  accel={accel_scale:.2f}"
-        )
-
-        self._arm.set_start_state_to_current_state()
         try:
-            self._arm.set_goal_state(configuration_name=name)
-        except Exception as exc:
-            response.success = False
-            response.message = f"Неизвестное состояние '{name}': {exc}"
-            self.get_logger().error(response.message)
-            return response
+            name = request.name.strip()
+            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
 
-        plan_params = self._make_plan_params(
-            "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
-        )
-        plan_result = self._arm.plan(single_plan_parameters=plan_params)
-        if not plan_result:
-            response.success = False
-            response.message = f"Не удалось построить траекторию для '{name}'"
-            self.get_logger().error(response.message)
-            return response
+            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
 
-        exec_result = self._moveit.execute(plan_result.trajectory, controllers=[])
-        if exec_result:
-            response.success = True
-            response.message = f"Переместился в '{name}'"
-            self.get_logger().info(response.message)
-        else:
-            response.success = False
-            response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)"
-            self.get_logger().error(response.message)
-        return response
+            self.get_logger().info(
+                f"[named] name='{name}'  speed={velocity_scale:.2f}  accel={accel_scale:.2f}"
+            )
+
+            self._arm.set_start_state_to_current_state()
+            try:
+                self._arm.set_goal_state(configuration_name=name)
+            except Exception as exc:
+                response.success = False
+                response.message = f"Неизвестное состояние '{name}': {exc}"
+                self.get_logger().error(response.message)
+                return response
+
+            plan_params = self._make_plan_params(
+                "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
+            )
+            plan_result = self._arm.plan(single_plan_parameters=plan_params)
+            if not 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 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}' завершилось ошибкой"
+                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_planning/test/conftest.py

@@ -0,0 +1,232 @@
+import importlib.util
+import sys
+import types
+import uuid
+from pathlib import Path
+
+import pytest
+
+
+class _DummyActionServer:
+    def __init__(self, *args, **kwargs):
+        self.args = args
+        self.kwargs = kwargs
+
+
+class _DummyGoalResponse:
+    ACCEPT = "accept"
+    REJECT = "reject"
+
+
+class _DummyCancelResponse:
+    ACCEPT = "accept"
+    REJECT = "reject"
+
+
+class _DummyNode:
+    pass
+
+
+class _DummyExecutor:
+    def add_node(self, _node):
+        return None
+
+    def spin(self):
+        return None
+
+    def spin_once(self, timeout_sec=None):
+        return None
+
+
+class _DummyPoseStamped:
+    def __init__(self):
+        self.header = types.SimpleNamespace(frame_id="")
+        self.pose = types.SimpleNamespace(
+            position=types.SimpleNamespace(x=0.0, y=0.0, z=0.0),
+            orientation=types.SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
+        )
+
+
+class _DummyPlanRequestParameters:
+    def __init__(self, _moveit, _group):
+        self.planning_pipeline = ""
+        self.planner_id = ""
+        self.planning_time = 0.0
+        self.planning_attempts = 0
+        self.max_velocity_scaling_factor = 0.0
+        self.max_acceleration_scaling_factor = 0.0
+
+
+class _DummyRobotState:
+    def __init__(self, _robot_model):
+        self.group = None
+        self.joints = None
+
+    def set_joint_group_positions(self, group, joints):
+        self.group = group
+        self.joints = list(joints)
+
+    def update(self):
+        return None
+
+
+class _DummyTrigger:
+    class Request:
+        pass
+
+    class Response:
+        def __init__(self):
+            self.success = False
+            self.message = ""
+
+
+class _DummyMoveToPose:
+    class Goal:
+        pass
+
+    class Result:
+        def __init__(self):
+            self.success = False
+            self.message = ""
+
+    class Feedback:
+        def __init__(self):
+            self.state = ""
+
+
+class _DummyMoveToJoints:
+    class Goal:
+        pass
+
+    class Result:
+        def __init__(self):
+            self.success = False
+            self.message = ""
+
+    class Feedback:
+        def __init__(self):
+            self.state = ""
+
+
+class _DummyMoveToNamedPose:
+    class Request:
+        pass
+
+    class Response:
+        def __init__(self):
+            self.success = False
+            self.message = ""
+
+
+class _DummyActionClient:
+    def __init__(self, *args, **kwargs):
+        self.args = args
+        self.kwargs = kwargs
+
+
+class _DummySequentialWriter:
+    def __init__(self):
+        self.open_calls = []
+        self.topics = []
+        self.messages = []
+
+    def open(self, storage_opts, converter_opts):
+        self.open_calls.append((storage_opts, converter_opts))
+
+    def create_topic(self, topic_metadata):
+        self.topics.append(topic_metadata)
+
+    def write(self, topic, payload, timestamp):
+        self.messages.append((topic, payload, timestamp))
+
+
+class _DummyStorageOptions:
+    def __init__(self, uri, storage_id):
+        self.uri = uri
+        self.storage_id = storage_id
+
+
+class _DummyConverterOptions:
+    def __init__(self, input_serialization_format, output_serialization_format):
+        self.input_serialization_format = input_serialization_format
+        self.output_serialization_format = output_serialization_format
+
+
+class _DummyTopicMetadata:
+    def __init__(self, id, name, type, serialization_format):
+        self.id = id
+        self.name = name
+        self.type = type
+        self.serialization_format = serialization_format
+
+
+@pytest.fixture
+def load_script_module(monkeypatch):
+    def _loader(relative_path: str):
+        module_defs = {
+            "rclpy": types.ModuleType("rclpy"),
+            "rclpy.node": types.ModuleType("rclpy.node"),
+            "rclpy.action": types.ModuleType("rclpy.action"),
+            "rclpy.callback_groups": types.ModuleType("rclpy.callback_groups"),
+            "rclpy.executors": types.ModuleType("rclpy.executors"),
+            "rclpy.serialization": types.ModuleType("rclpy.serialization"),
+            "geometry_msgs": types.ModuleType("geometry_msgs"),
+            "geometry_msgs.msg": types.ModuleType("geometry_msgs.msg"),
+            "moveit": types.ModuleType("moveit"),
+            "moveit.planning": types.ModuleType("moveit.planning"),
+            "moveit.core": types.ModuleType("moveit.core"),
+            "moveit.core.robot_state": types.ModuleType("moveit.core.robot_state"),
+            "std_srvs": types.ModuleType("std_srvs"),
+            "std_srvs.srv": types.ModuleType("std_srvs.srv"),
+            "iiwa_msgs": types.ModuleType("iiwa_msgs"),
+            "iiwa_msgs.action": types.ModuleType("iiwa_msgs.action"),
+            "iiwa_msgs.srv": types.ModuleType("iiwa_msgs.srv"),
+            "rosbag2_py": types.ModuleType("rosbag2_py"),
+            "rosidl_runtime_py": types.ModuleType("rosidl_runtime_py"),
+            "rosidl_runtime_py.utilities": types.ModuleType("rosidl_runtime_py.utilities"),
+        }
+
+        module_defs["rclpy"].init = lambda *args, **kwargs: None
+        module_defs["rclpy"].shutdown = lambda *args, **kwargs: None
+        module_defs["rclpy.node"].Node = _DummyNode
+        module_defs["rclpy.action"].ActionServer = _DummyActionServer
+        module_defs["rclpy.action"].ActionClient = _DummyActionClient
+        module_defs["rclpy.action"].GoalResponse = _DummyGoalResponse
+        module_defs["rclpy.action"].CancelResponse = _DummyCancelResponse
+        module_defs["rclpy.callback_groups"].ReentrantCallbackGroup = object
+        module_defs["rclpy.executors"].MultiThreadedExecutor = _DummyExecutor
+        module_defs["rclpy.executors"].ExternalShutdownException = RuntimeError
+        module_defs["rclpy.serialization"].serialize_message = lambda msg: b"serialized"
+
+        module_defs["geometry_msgs.msg"].PoseStamped = _DummyPoseStamped
+
+        module_defs["moveit.planning"].MoveItPy = object
+        module_defs["moveit.planning"].PlanningComponent = object
+        module_defs["moveit.planning"].PlanRequestParameters = _DummyPlanRequestParameters
+        module_defs["moveit.core.robot_state"].RobotState = _DummyRobotState
+
+        module_defs["std_srvs.srv"].Trigger = _DummyTrigger
+
+        module_defs["iiwa_msgs.action"].MoveToPose = _DummyMoveToPose
+        module_defs["iiwa_msgs.action"].MoveToJoints = _DummyMoveToJoints
+        module_defs["iiwa_msgs.srv"].MoveToNamedPose = _DummyMoveToNamedPose
+
+        module_defs["rosbag2_py"].SequentialWriter = _DummySequentialWriter
+        module_defs["rosbag2_py"].StorageOptions = _DummyStorageOptions
+        module_defs["rosbag2_py"].ConverterOptions = _DummyConverterOptions
+        module_defs["rosbag2_py"].TopicMetadata = _DummyTopicMetadata
+
+        module_defs["rosidl_runtime_py.utilities"].get_message = lambda _type_name: object
+
+        for name, module in module_defs.items():
+            monkeypatch.setitem(sys.modules, name, module)
+
+        script_path = Path("/home/daniel/dev/ros2_iiwa7") / relative_path
+        module_name = f"test_{script_path.stem}_{uuid.uuid4().hex}"
+        spec = importlib.util.spec_from_file_location(module_name, script_path)
+        loaded_module = importlib.util.module_from_spec(spec)
+        assert spec.loader is not None
+        spec.loader.exec_module(loaded_module)
+        return loaded_module
+
+    return _loader

src/iiwa_planning/test/test_motion_sequence_runner.py

@@ -0,0 +1,73 @@
+import threading
+from pathlib import Path
+from types import SimpleNamespace
+
+
+def _make_logger():
+    return SimpleNamespace(info=lambda *args, **kwargs: None, warn=lambda *args, **kwargs: None)
+
+
+def _build_runner(module):
+    runner = module.MotionSequenceRunner.__new__(module.MotionSequenceRunner)
+    runner.get_logger = lambda: _make_logger()
+    runner.close_bag = lambda: None
+    runner._n_iter = 1
+    runner._delay = 0.0
+    runner._home = {"joints": [0.0] * 7}
+    runner._waypoints = [{"x": 0.5}, {"x": 0.6}]
+    return runner
+
+
+def test_run_stops_after_first_failed_waypoint(load_script_module):
+    module = load_script_module("src/iiwa_planning/scripts/motion_sequence_runner.py")
+    runner = _build_runner(module)
+    calls = []
+    results = iter([True, False, True, True])
+
+    def fake_send_waypoint(waypoint, idx=None):
+        calls.append((waypoint, idx))
+        return next(results)
+
+    runner._send_waypoint = fake_send_waypoint
+
+    done_event = threading.Event()
+    runner.run(done_event)
+
+    assert done_event.is_set()
+    assert calls == [
+        (runner._home, None),
+        (runner._waypoints[0], 0),
+    ]
+
+
+def test_init_bag_refuses_existing_directory_outside_temp_root(load_script_module, monkeypatch, tmp_path):
+    module = load_script_module("src/iiwa_planning/scripts/motion_sequence_runner.py")
+    runner = module.MotionSequenceRunner.__new__(module.MotionSequenceRunner)
+    runner.get_logger = lambda: _make_logger()
+
+    unsafe_bag_dir = Path("/home/daniel/dev/ros2_iiwa7") / "tmp-existing-bag"
+    unsafe_bag_dir.mkdir(exist_ok=True)
+    (unsafe_bag_dir / "keep.txt").write_text("keep")
+    runner._bag_path = str(unsafe_bag_dir)
+
+    removed = []
+
+    def fake_rmtree(path):
+        removed.append(Path(path))
+
+    monkeypatch.setattr(module.shutil, "rmtree", fake_rmtree)
+
+    try:
+        try:
+            runner._init_bag()
+        except ValueError as exc:
+            assert "temporary" in str(exc).lower() or "tmp" in str(exc).lower()
+        else:
+            raise AssertionError("Expected ValueError for unsafe bag path")
+
+        assert removed == []
+        assert unsafe_bag_dir.exists()
+        assert (unsafe_bag_dir / "keep.txt").exists()
+    finally:
+        (unsafe_bag_dir / "keep.txt").unlink(missing_ok=True)
+        unsafe_bag_dir.rmdir()

src/iiwa_planning/test/test_move_to_pose_server.py

@@ -0,0 +1,206 @@
+import math
+import threading
+import time
+from types import SimpleNamespace
+
+import pytest
+
+
+class _FakeLogger:
+    def __init__(self):
+        self.errors = []
+        self.infos = []
+
+    def info(self, message):
+        self.infos.append(message)
+
+    def error(self, message):
+        self.errors.append(message)
+
+
+class _FakeGoalHandle:
+    def __init__(self, request):
+        self.request = request
+        self.is_cancel_requested = False
+        self.feedback_states = []
+        self.aborted = False
+        self.succeeded = False
+        self.cancelled = False
+
+    def publish_feedback(self, feedback):
+        self.feedback_states.append(feedback.state)
+
+    def abort(self):
+        self.aborted = True
+
+    def succeed(self):
+        self.succeeded = True
+
+    def canceled(self):
+        self.cancelled = True
+
+
+class _FakeArm:
+    def __init__(self, plan_result):
+        self.plan_result = plan_result
+        self.goal_state_calls = []
+
+    def set_start_state_to_current_state(self):
+        return None
+
+    def set_goal_state(self, **kwargs):
+        self.goal_state_calls.append(kwargs)
+
+    def plan(self, single_plan_parameters=None):
+        return self.plan_result
+
+
+class _FakeTrajectoryExecutionManager:
+    def __init__(self):
+        self.stop_calls = 0
+
+    def stop_execution(self):
+        self.stop_calls += 1
+
+
+class _FakeMoveIt:
+    def __init__(self, execute_result):
+        self.execute_result = execute_result
+        self.execution_manager = _FakeTrajectoryExecutionManager()
+
+    def execute(self, trajectory, controllers=None):
+        return self.execute_result(trajectory, controllers)
+
+    def get_trajectory_execution_manager(self):
+        return self.execution_manager
+
+
+class _FakeRobotModel:
+    def __init__(self, bounds):
+        self._bounds = bounds
+
+    def get_joint_bounds(self, group_name):
+        return list(self._bounds)
+
+
+def _build_server(module, *, plan_result=None, execute_result=None):
+    server = module.IiwaMotionServer.__new__(module.IiwaMotionServer)
+    server._planning_group = "iiwa_arm"
+    server._pose_link = "tcp"
+    server._default_frame = "base_link"
+    server._default_planner = "ompl"
+    server._planning_attempts = 1
+    server._execution_timeout_sec = 0.05
+    server._logger = _FakeLogger()
+    server.get_logger = lambda: server._logger
+    server._robot_model = _FakeRobotModel([(-1.0, 1.0)] * 7)
+    server._arm = _FakeArm(plan_result or SimpleNamespace(trajectory=object()))
+    server._moveit = _FakeMoveIt(execute_result or (lambda *_args, **_kwargs: True))
+    server._motion_lock = threading.Lock()
+    server._active_motion_kind = None
+    return server
+
+
+@pytest.mark.parametrize(
+    ("joints", "speed", "expected_fragment"),
+    [
+        ([0.0, 0.0, 0.0], 0.2, "7"),
+        ([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0], 0.2, "limit"),
+        ([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, math.nan], 0.2, "finite"),
+        ([0.0] * 7, "fast", "speed"),
+    ],
+)
+def test_execute_joints_rejects_invalid_joint_targets(load_script_module, joints, speed, expected_fragment):
+    module = load_script_module("src/iiwa_planning/scripts/move_to_pose_server.py")
+    server = _build_server(module)
+    goal_handle = _FakeGoalHandle(SimpleNamespace(joints=joints, speed=speed))
+
+    result = server._execute_joints(goal_handle)
+
+    assert result.success is False
+    assert goal_handle.aborted is True
+    assert expected_fragment.lower() in result.message.lower()
+
+
+def test_parallel_motion_rejects_second_goal_and_named_pose(load_script_module):
+    module = load_script_module("src/iiwa_planning/scripts/move_to_pose_server.py")
+    server = _build_server(module)
+
+    assert server._handle_pose_goal(object()) == module.GoalResponse.ACCEPT
+    assert server._handle_joints_goal(object()) == module.GoalResponse.REJECT
+
+    response = module.MoveToNamedPose.Response()
+    response = server._handle_named(
+        SimpleNamespace(name="home", speed=0.2, accel_scale=0.2),
+        response,
+    )
+
+    assert response.success is False
+    assert "busy" in response.message.lower() or "parallel" in response.message.lower()
+
+
+def test_plan_and_execute_reports_moveit_execution_failure(load_script_module):
+    module = load_script_module("src/iiwa_planning/scripts/move_to_pose_server.py")
+    server = _build_server(module, execute_result=lambda *_args, **_kwargs: False)
+    goal_handle = _FakeGoalHandle(SimpleNamespace())
+
+    ok, message = server._plan_and_execute(SimpleNamespace(), goal_handle)
+
+    assert ok is False
+    assert "ошиб" in message.lower() or "fail" in message.lower()
+
+
+def test_plan_and_execute_times_out_without_hanging(load_script_module):
+    module = load_script_module("src/iiwa_planning/scripts/move_to_pose_server.py")
+    block_event = threading.Event()
+
+    def hanging_execute(*_args, **_kwargs):
+        block_event.wait(timeout=1.0)
+        return True
+
+    server = _build_server(module, execute_result=hanging_execute)
+    goal_handle = _FakeGoalHandle(SimpleNamespace())
+
+    outcome = {}
+
+    def run_plan():
+        outcome["result"] = server._plan_and_execute(SimpleNamespace(), goal_handle)
+
+    worker = threading.Thread(target=run_plan, daemon=True)
+    worker.start()
+    worker.join(timeout=0.3)
+
+    assert not worker.is_alive(), "Expected timeout handling to finish promptly"
+    assert outcome["result"][0] is False
+    assert "timeout" in outcome["result"][1].lower() or "time" in outcome["result"][1].lower()
+
+
+def test_plan_and_execute_cancel_finishes_without_waiting_forever(load_script_module):
+    module = load_script_module("src/iiwa_planning/scripts/move_to_pose_server.py")
+    block_event = threading.Event()
+
+    def hanging_execute(*_args, **_kwargs):
+        block_event.wait(timeout=1.0)
+        return True
+
+    server = _build_server(module, execute_result=hanging_execute)
+    goal_handle = _FakeGoalHandle(SimpleNamespace())
+    goal_handle.is_cancel_requested = False
+
+    outcome = {}
+
+    def request_cancel():
+        time.sleep(0.05)
+        goal_handle.is_cancel_requested = True
+
+    def run_plan():
+        outcome["result"] = server._plan_and_execute(SimpleNamespace(), goal_handle)
+
+    threading.Thread(target=request_cancel, daemon=True).start()
+    worker = threading.Thread(target=run_plan, daemon=True)
+    worker.start()
+    worker.join(timeout=0.3)
+
+    assert not worker.is_alive(), "Expected cancel handling to finish promptly"
+    assert outcome["result"][0] is None
+    assert "cancel" in outcome["result"][1].lower()

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',

src/iiwa_web/test/conftest.py

@@ -0,0 +1,198 @@
+import sys
+import types
+from contextlib import asynccontextmanager
+from pathlib import Path
+
+
+PACKAGE_ROOT = Path(__file__).resolve().parents[1]
+if str(PACKAGE_ROOT) not in sys.path:
+    sys.path.insert(0, str(PACKAGE_ROOT))
+
+
+def _install_ros_stubs() -> None:
+    ament_index_python = types.ModuleType("ament_index_python")
+    ament_index_packages = types.ModuleType("ament_index_python.packages")
+
+    def get_package_share_directory(package_name: str) -> str:
+        return str(PACKAGE_ROOT.parent / package_name)
+
+    ament_index_packages.get_package_share_directory = get_package_share_directory
+    ament_index_python.packages = ament_index_packages
+    sys.modules.setdefault("ament_index_python", ament_index_python)
+    sys.modules.setdefault("ament_index_python.packages", ament_index_packages)
+
+    builtin_interfaces = types.ModuleType("builtin_interfaces")
+    builtin_interfaces_msg = types.ModuleType("builtin_interfaces.msg")
+
+    class Duration:
+        def __init__(self, sec: int = 0, nanosec: int = 0):
+            self.sec = sec
+            self.nanosec = nanosec
+
+    builtin_interfaces_msg.Duration = Duration
+    builtin_interfaces.msg = builtin_interfaces_msg
+    sys.modules.setdefault("builtin_interfaces", builtin_interfaces)
+    sys.modules.setdefault("builtin_interfaces.msg", builtin_interfaces_msg)
+
+    trajectory_msgs = types.ModuleType("trajectory_msgs")
+    trajectory_msgs_msg = types.ModuleType("trajectory_msgs.msg")
+
+    class JointTrajectoryPoint:
+        def __init__(self):
+            self.positions = []
+            self.time_from_start = None
+
+    class JointTrajectory:
+        def __init__(self):
+            self.joint_names = []
+            self.points = []
+
+    trajectory_msgs_msg.JointTrajectory = JointTrajectory
+    trajectory_msgs_msg.JointTrajectoryPoint = JointTrajectoryPoint
+    trajectory_msgs.msg = trajectory_msgs_msg
+    sys.modules.setdefault("trajectory_msgs", trajectory_msgs)
+    sys.modules.setdefault("trajectory_msgs.msg", trajectory_msgs_msg)
+
+    sensor_msgs = types.ModuleType("sensor_msgs")
+    sensor_msgs_msg = types.ModuleType("sensor_msgs.msg")
+
+    class JointState:
+        def __init__(self, position=()):
+            self.position = position
+
+    sensor_msgs_msg.JointState = JointState
+    sensor_msgs.msg = sensor_msgs_msg
+    sys.modules.setdefault("sensor_msgs", sensor_msgs)
+    sys.modules.setdefault("sensor_msgs.msg", sensor_msgs_msg)
+
+    std_srvs = types.ModuleType("std_srvs")
+    std_srvs_srv = types.ModuleType("std_srvs.srv")
+
+    class Trigger:
+        class Request:
+            pass
+
+    std_srvs_srv.Trigger = Trigger
+    std_srvs.srv = std_srvs_srv
+    sys.modules.setdefault("std_srvs", std_srvs)
+    sys.modules.setdefault("std_srvs.srv", std_srvs_srv)
+
+    fastmcp = types.ModuleType("fastmcp")
+
+    class _Router:
+        @asynccontextmanager
+        async def lifespan_context(self, _):
+            yield
+
+    class _HttpApp:
+        def __init__(self):
+            self.router = _Router()
+
+    class FastMCP:
+        @classmethod
+        def from_fastapi(cls, app):
+            return cls()
+
+        def http_app(self, path="/mcp"):
+            return _HttpApp()
+
+    fastmcp.FastMCP = FastMCP
+    sys.modules.setdefault("fastmcp", fastmcp)
+
+    rclpy = types.ModuleType("rclpy")
+    rclpy_time = types.ModuleType("rclpy.time")
+    rclpy_duration = types.ModuleType("rclpy.duration")
+    rclpy_node = types.ModuleType("rclpy.node")
+    rclpy_action = types.ModuleType("rclpy.action")
+
+    class Time:
+        pass
+
+    class DurationValue:
+        def __init__(self, seconds: float = 0.0):
+            self.seconds = seconds
+
+    class _Logger:
+        def info(self, *_args, **_kwargs):
+            return None
+
+    class _Parameter:
+        def __init__(self, value):
+            self.value = value
+
+    class _Publisher:
+        def __init__(self):
+            self.published = []
+
+        def publish(self, msg):
+            self.published.append(msg)
+
+    class _Client:
+        def wait_for_service(self, timeout_sec):
+            return True
+
+        def call_async(self, request):
+            future = types.SimpleNamespace()
+            future.done = lambda: True
+            future.result = lambda: types.SimpleNamespace(success=True, message="")
+            return future
+
+    class Node:
+        def __init__(self, name):
+            self.name = name
+            self._parameters = {}
+            self._logger = _Logger()
+
+        def declare_parameter(self, name, default):
+            self._parameters[name] = default
+
+        def get_parameter(self, name):
+            return _Parameter(self._parameters[name])
+
+        def create_subscription(self, *_args, **_kwargs):
+            return object()
+
+        def create_publisher(self, *_args, **_kwargs):
+            return _Publisher()
+
+        def create_client(self, *_args, **_kwargs):
+            return _Client()
+
+        def get_logger(self):
+            return self._logger
+
+    class ActionClient:
+        def __init__(self, *_args, **_kwargs):
+            pass
+
+    rclpy.init = lambda: None
+    rclpy.spin = lambda node: None
+    rclpy.time = rclpy_time
+    rclpy.duration = rclpy_duration
+    rclpy.Time = Time
+    rclpy_time.Time = Time
+    rclpy_duration.Duration = DurationValue
+    rclpy_node.Node = Node
+    rclpy_action.ActionClient = ActionClient
+    sys.modules.setdefault("rclpy", rclpy)
+    sys.modules.setdefault("rclpy.time", rclpy_time)
+    sys.modules.setdefault("rclpy.duration", rclpy_duration)
+    sys.modules.setdefault("rclpy.node", rclpy_node)
+    sys.modules.setdefault("rclpy.action", rclpy_action)
+
+    tf2_ros = types.ModuleType("tf2_ros")
+
+    class Buffer:
+        def lookup_transform(self, *_args, **_kwargs):
+            raise RuntimeError("tf unavailable in tests")
+
+    class TransformListener:
+        def __init__(self, *_args, **_kwargs):
+            pass
+
+    tf2_ros.Buffer = Buffer
+    tf2_ros.TransformListener = TransformListener
+    sys.modules.setdefault("tf2_ros", tf2_ros)
+
+
+_install_ros_stubs()

src/iiwa_web/test/test_ros_action_timeout.py

@@ -0,0 +1,65 @@
+import itertools
+
+import pytest
+
+from iiwa_web.ros_node import CobotWebNode
+from iiwa_web import ros_node
+
+
+class DoneFuture:
+    def __init__(self, result):
+        self._result = result
+
+    def done(self):
+        return True
+
+    def result(self):
+        return self._result
+
+
+class PendingFuture:
+    def done(self):
+        return False
+
+    def result(self):
+        return None
+
+
+class GoalHandle:
+    accepted = True
+
+    def __init__(self):
+        self.cancel_calls = 0
+
+    def get_result_async(self):
+        return PendingFuture()
+
+    def cancel_goal_async(self):
+        self.cancel_calls += 1
+        return DoneFuture(None)
+
+
+class ActionClientStub:
+    def __init__(self, goal_handle):
+        self.goal_handle = goal_handle
+
+    def wait_for_server(self, timeout_sec):
+        return True
+
+    def send_goal_async(self, goal):
+        return DoneFuture(self.goal_handle)
+
+
+def test_send_action_cancels_goal_when_result_times_out(monkeypatch):
+    node = object.__new__(CobotWebNode)
+    goal_handle = GoalHandle()
+    node._action_clients = {"cobot/move_to_pose": ActionClientStub(goal_handle)}
+
+    timestamps = itertools.chain([0.0, 0.3, 0.6], itertools.repeat(0.9))
+    monkeypatch.setattr(ros_node.time, "monotonic", lambda: next(timestamps))
+    monkeypatch.setattr(ros_node.time, "sleep", lambda _seconds: None)
+
+    with pytest.raises(TimeoutError):
+        node.send_action(object, "cobot/move_to_pose", object(), timeout=0.5)
+
+    assert goal_handle.cancel_calls == 1

src/iiwa_web/test/test_runner_upload_security.py

@@ -0,0 +1,130 @@
+import asyncio
+from pathlib import Path
+
+import pytest
+
+from iiwa_web import runner
+
+
+class DummyProcess:
+    def __init__(self, cmd):
+        self.cmd = cmd
+        self.pid = 4321
+        self.stdout = iter(())
+        self.returncode = None
+
+    def poll(self):
+        return None
+
+
+class DummyThread:
+    def __init__(self, target=None, args=(), daemon=False):
+        self.target = target
+        self.args = args
+        self.daemon = daemon
+
+    def start(self):
+        return None
+
+
+class UploadStub:
+    def __init__(self, filename: str, content: bytes):
+        self.filename = filename
+        self._content = content
+
+    async def read(self) -> bytes:
+        return self._content
+
+
+@pytest.fixture
+def runner_module(monkeypatch, tmp_path):
+    upload_dir = tmp_path / "uploads"
+    upload_dir.mkdir()
+    bag_root = tmp_path / "bags"
+    bag_root.mkdir()
+
+    monkeypatch.setattr(runner, "_UPLOAD_DIR", upload_dir)
+    monkeypatch.setattr(runner, "_BAG_ROOT", bag_root, raising=False)
+    monkeypatch.setattr(runner, "MAX_UPLOAD_BYTES", 8, raising=False)
+    monkeypatch.setattr(runner, "_process", None)
+
+    started = {}
+
+    def fake_popen(cmd, **kwargs):
+        started["cmd"] = cmd
+        proc = DummyProcess(cmd)
+        started["proc"] = proc
+        return proc
+
+    monkeypatch.setattr(runner.subprocess, "Popen", fake_popen)
+    monkeypatch.setattr(runner.threading, "Thread", DummyThread)
+
+    return runner, upload_dir, bag_root, started
+
+
+def _upload_file(name: str, content: bytes) -> UploadStub:
+    return UploadStub(filename=name, content=content)
+
+
+def test_start_runner_sanitizes_uploaded_filename(runner_module):
+    runner, upload_dir, _bag_root, started = runner_module
+
+    response = asyncio.run(
+        runner.start_runner(
+            config=_upload_file("../escape.json", b"{}"),
+            n_iterations=3,
+            delay_between_iterations=5.0,
+            bag_path="",
+            topics="",
+            joints_action="cobot/move_to_joints",
+            pose_action="cobot/move_to_pose",
+        )
+    )
+
+    assert response["config"] == "escape.json"
+    assert (upload_dir / "escape.json").exists()
+    assert not (upload_dir.parent / "escape.json").exists()
+    config_arg = next(part for part in started["cmd"] if part.startswith("config_path:="))
+    assert Path(config_arg.split(":=", 1)[1]).parent == upload_dir
+
+
+def test_start_runner_rejects_oversized_upload(runner_module):
+    runner, _upload_dir, _bag_root, started = runner_module
+
+    with pytest.raises(runner.HTTPException) as excinfo:
+        asyncio.run(
+            runner.start_runner(
+                config=_upload_file("config.json", b"123456789"),
+                n_iterations=3,
+                delay_between_iterations=5.0,
+                bag_path="",
+                topics="",
+                joints_action="cobot/move_to_joints",
+                pose_action="cobot/move_to_pose",
+            )
+        )
+
+    assert excinfo.value.status_code == 413
+    assert "слишком" in excinfo.value.detail.lower()
+    assert "cmd" not in started
+
+
+def test_start_runner_rejects_bag_path_outside_allowed_root(runner_module):
+    runner, _upload_dir, _bag_root, started = runner_module
+
+    with pytest.raises(runner.HTTPException) as excinfo:
+        asyncio.run(
+            runner.start_runner(
+                config=_upload_file("config.json", b"{}"),
+                n_iterations=3,
+                delay_between_iterations=5.0,
+                bag_path="/tmp/escape",
+                topics="",
+                joints_action="cobot/move_to_joints",
+                pose_action="cobot/move_to_pose",
+            )
+        )
+
+    assert excinfo.value.status_code == 422
+    assert "bag_path" in excinfo.value.detail
+    assert "cmd" not in started

src/iiwa_web/test/test_trajectory_validation.py

@@ -0,0 +1,118 @@
+import importlib
+import pytest
+from fastapi import HTTPException
+from pydantic import ValidationError
+
+from iiwa_web import config_loader
+
+
+class RecordingBridge:
+    def __init__(self):
+        self.messages = []
+
+    def publish(self, topic_name, message_type, msg):
+        self.messages.append((topic_name, message_type, msg))
+
+    def get_latest(self, topic_name):
+        return None
+
+
+@pytest.fixture
+def trajectory_module(monkeypatch):
+    monkeypatch.setattr(
+        config_loader,
+        "load_joint_names",
+        lambda *args, **kwargs: [f"joint{i}" for i in range(1, 8)],
+    )
+    monkeypatch.setattr(
+        config_loader,
+        "load_joint_limits",
+        lambda *args, **kwargs: [(-1.0, 1.0)] * 7,
+    )
+
+    trajectory = importlib.import_module("iiwa_web.trajectory")
+    trajectory = importlib.reload(trajectory)
+    bridge = RecordingBridge()
+    monkeypatch.setattr(trajectory, "get_bridge", lambda: bridge)
+    return trajectory, bridge
+
+
+@pytest.mark.parametrize(
+    ("payload", "detail"),
+    [
+        (
+            {
+                "points": [
+                    {
+                        "positions": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+                        "time_from_start": 0.0,
+                    }
+                ]
+            },
+            "positions",
+        ),
+        (
+            {
+                "points": [
+                    {
+                        "positions": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, float("nan")],
+                        "time_from_start": 0.0,
+                    }
+                ]
+            },
+            "конеч",
+        ),
+    ],
+)
+def test_send_request_rejects_invalid_payloads(trajectory_module, payload, detail):
+    trajectory, bridge = trajectory_module
+
+    with pytest.raises((ValidationError, HTTPException)) as excinfo:
+        request = trajectory.SendRequest.model_validate(payload)
+        trajectory.send_trajectory(request)
+
+    assert detail.lower() in str(excinfo.value).lower()
+    assert bridge.messages == []
+
+
+def test_send_trajectory_rejects_non_monotonic_time(trajectory_module):
+    trajectory, bridge = trajectory_module
+    payload = {
+        "points": [
+            {
+                "positions": [0.0] * trajectory.N_JOINTS,
+                "time_from_start": 0.5,
+            },
+            {
+                "positions": [0.1] * trajectory.N_JOINTS,
+                "time_from_start": 0.4,
+            },
+        ]
+    }
+
+    with pytest.raises(ValidationError) as excinfo:
+        request = trajectory.SendRequest.model_validate(payload)
+        trajectory.send_trajectory(request)
+
+    assert "монотон" in str(excinfo.value).lower()
+    assert bridge.messages == []
+
+
+def test_send_trajectory_enforces_joint_limits_even_when_bypass_requested(trajectory_module):
+    trajectory, bridge = trajectory_module
+    payload = {
+        "points": [
+            {
+                "positions": [100.0] * trajectory.N_JOINTS,
+                "time_from_start": 0.0,
+            }
+        ],
+        "validate_limits": False,
+    }
+
+    with pytest.raises(HTTPException) as excinfo:
+        request = trajectory.SendRequest.model_validate(payload)
+        trajectory.send_trajectory(request)
+
+    assert "вне диапазона" in excinfo.value.detail.lower()
+    assert bridge.messages == []

src/iiwa_web/test/test_web_auth.py

@@ -0,0 +1,137 @@
+import pytest
+
+from iiwa_web import config_loader, ros_node
+
+
+def test_web_node_defaults_to_loopback_host():
+    node = ros_node.CobotWebNode()
+
+    assert node.get_parameter("host").value == "127.0.0.1"
+
+
+def test_external_bind_requires_token():
+    import importlib
+
+    monkeypatchable_main = _import_main_with_stubbed_config()
+
+    with pytest.raises(RuntimeError):
+        monkeypatchable_main.ensure_safe_bind("0.0.0.0", token=None)
+
+    monkeypatchable_main.ensure_safe_bind("0.0.0.0", token="secret-token")
+
+
+def test_web_settings_load_token():
+    import sys
+    from pathlib import Path
+
+    utils_root = Path(__file__).resolve().parents[2] / "iiwa_utils"
+    if str(utils_root) not in sys.path:
+        sys.path.insert(0, str(utils_root))
+
+    from iiwa_utils.setting_loader import _parse_web
+
+    settings = _parse_web(
+        {"enabled": True, "host": "0.0.0.0", "token": "configured-secret"},
+        "/tmp",
+    )
+
+    assert settings.token == "configured-secret"
+
+
+def test_web_server_passes_configured_token_to_process_environment(monkeypatch):
+    import sys
+    import types
+    from pathlib import Path
+    from types import SimpleNamespace
+
+    class FakeNode:
+        def __init__(self, *args, **kwargs):
+            self.additional_env = kwargs.get("additional_env")
+
+    launch_ros = types.ModuleType("launch_ros")
+    launch_ros_actions = types.ModuleType("launch_ros.actions")
+    launch_ros_actions.Node = FakeNode
+    launch_ros.actions = launch_ros_actions
+    monkeypatch.setitem(sys.modules, "launch_ros", launch_ros)
+    monkeypatch.setitem(sys.modules, "launch_ros.actions", launch_ros_actions)
+
+    launch_root = str(Path(__file__).resolve().parents[2] / "iiwa_bringup" / "launch")
+    if launch_root not in sys.path:
+        sys.path.insert(0, launch_root)
+
+    from supported.optional_nodes import make_web_server_node
+
+    settings = SimpleNamespace(
+        web=SimpleNamespace(
+            host="0.0.0.0",
+            port=8007,
+            endpoints="/tmp/endpoints.yaml",
+            joint_limits="/tmp/joint_limits.yaml",
+            token="configured-secret",
+        )
+    )
+
+    node = make_web_server_node(settings, use_sim_time=False)
+
+    assert node.additional_env == {"IIWA_WEB_TOKEN": "configured-secret"}
+
+
+def test_bearer_token_is_required_and_compared_in_constant_time(monkeypatch):
+    main = _import_main_with_stubbed_config()
+    compared = []
+
+    def fake_compare(left, right):
+        compared.append((left, right))
+        return left == right
+
+    monkeypatch.setattr(main.hmac, "compare_digest", fake_compare)
+
+    assert main.has_valid_bearer_token(None, "secret-token") is False
+    assert main.has_valid_bearer_token("Bearer wrong", "secret-token") is False
+    assert main.has_valid_bearer_token("Bearer secret-token", "secret-token") is True
+    assert compared == [("wrong", "secret-token"), ("secret-token", "secret-token")]
+
+
+def test_docs_paths_are_public_but_api_routes_are_not():
+    main = _import_main_with_stubbed_config()
+
+    assert main.is_public_auth_path("/docs")
+    assert main.is_public_auth_path("/docs/oauth2-redirect")
+    assert main.is_public_auth_path("/openapi.json")
+    assert main.is_public_auth_path("/redoc")
+    assert not main.is_public_auth_path("/protected")
+
+
+def test_openapi_declares_bearer_security_for_swagger_authorize_button():
+    from fastapi import FastAPI
+
+    main = _import_main_with_stubbed_config()
+    app = FastAPI()
+
+    @app.get("/protected")
+    def protected():
+        return {"ok": True}
+
+    main.install_bearer_openapi(app, "secret-token")
+    schema = app.openapi()
+
+    assert schema["components"]["securitySchemes"]["BearerAuth"] == {
+        "type": "http",
+        "scheme": "bearer",
+        "bearerFormat": "opaque-token",
+    }
+    assert schema["paths"]["/protected"]["get"]["security"] == [
+        {"BearerAuth": []}
+    ]
+
+
+def _import_main_with_stubbed_config():
+    monkeypatch_joint_names = [f"joint{i}" for i in range(1, 8)]
+    monkeypatch_joint_limits = [(-1.0, 1.0)] * 7
+    config_loader.load_joint_names = lambda *args, **kwargs: list(monkeypatch_joint_names)
+    config_loader.load_joint_limits = lambda *args, **kwargs: list(monkeypatch_joint_limits)
+
+    import importlib
+
+    main = importlib.import_module("iiwa_web.main")
+    return importlib.reload(main)