Merge branch 'dev'

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

.gitignore

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

README.md

@@ -65,7 +65,7 @@ Docker можно использовать как альтернативную
 
 ### Требования
 
-- Ubuntu 24.044 LTS;
+- Ubuntu 24.04 LTS;
 - доступ в интернет;
 - права `sudo`;
 - физический KUKA LBR iiwa 7 R800 либо компьютер для работы только с симулятором.
@@ -188,165 +188,4 @@ Java-программы для KUKA Sunrise Cabinet находятся отде
 | Организация | Примечание |
 |---|---|
 | [Комсомольский-на-Амуре государственный университет](https://knastu.ru/) | Исследования проводились на базе КнАГУ |
-| [Российский научный фонд](https://rscf.ru/) | Работа выполнена при поддержке Российского научного фонда |
+| [Российский научный фонд](https://rscf.ru/) | Работа выполнена при поддержке Российского научного фонда |
-
----
-
-<!-- ============================================================ -->
-<!-- Черновые команды — будут удалены в следующих версиях         -->
-<!-- ============================================================ -->
-
-## Черновые команды
-
-> Вроде уже не обязательно
-Подмена файла по пути обязательна: `/opt/ros/rolling/lib/webots_ros2_driver/ros2_supervisor.py`
-необходимо `warn` заменить на `warning` в логере
-
-```bash
-
-sudo apt install -y ros-${ROS_DISTRO}-webots-ros2 \
-                    ros-${ROS_DISTRO}-ros2-control \ 
-                    ros-${ROS_DISTRO}-ros2-controllers \
-                    ros-${ROS_DISTRO}-moveit \
-                    ros-${ROS_DISTRO}-moveit-py \
-                    ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
-                    ros-${ROS_DISTRO}-ament-cmake-clang-format \
-                    ros-${ROS_DISTRO}-rosbag2-storage-mcap \
-                    ros-${ROS_DISTRO}-librealsense2 \
-                    ros-${ROS_DISTRO}-realsense2* \
-                    libportaudio2 \
-
-```
-
-Установка moveit2 (внимательно проверяй)
-```bash
-sudo apt install -y build-essential \
-                    cmake \
-                    git \
-                    python3-colcon-common-extensions \
-                    python3-flake8 \
-                    python3-rosdep \
-                    python3-setuptools \
-                    python3-vcstool \
-                    wget
-
-# Not Using
-LC_ALL=C ros2 launch iiwa_moveit demo...
-
-sudo pip3 install transforms3d --break-system-packages
-```
-Отправка робота в точку:
-```bash
-ros2 action send_goal /iiwa/move_to_pose iiwa_msgs/action/MoveToPose \
-  "{x: 0.5, y: 0.0, z: 0.5, a: 3.14, b: 0, c: 0, speed: 0.1, planner: 'ptp'}"
-
-ros2 action send_goal --feedback /iiwa/move_to_joints iiwa_msgs/action/MoveToJoints \
-  "{joints: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0], speed: 0.4}"
-
-ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
-  "{name: 'home', speed: 0.5}"
-
-ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
-  "{name: 'work', speed: 0.3}"
-
-ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"
-```
-
-Примеры использования `test_motion_sequence`:
-```bash
-ros2 run iiwa_planning motion_sequence_runner \
-  --ros-args \
-  -p config_path:=/path/to/config.json \
-  -p n_iterations:=3 \
-  -p delay_between_iterations:=5.0 \
-  -p bag_path:=/tmp/my_bag \
-  -p joints_action:=my_ns/move_to_joints \
-  -p pose_action:=my_ns/move_to_pose
-
-```
-
-Спавн объекта:
-```bash
-{
-    "data": "Solid { name \"test_box2\" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }"
-}
-```
-
-Спавн `.proto`:
-
-
-Можно заготовить готовые `.proto` файлы, и потом случайно спавнить объект по такому принципу + создать Node который будет вызываться и спавнить этот объекты. Может быть шаблон куда потом подставятся данные через `.format()`. По такому же принципу спавн человека. Остается понять, только задать область спавна относительно робта
-
-Установка зависимостей для сборки пакетов:
-
-```bash
-rosdep install --from-paths src --ignore-src -r -y --os=ubuntu:jammy
-```
-
-Запись данных в rosbag:
-
-```bash
-$ sudo apt-get install ros-$ROS_DISTRO-rosbag2-storage-mcap
-
-# Record all topics
-ros2 bag record -a --storage mcap -o my_session
-
-# Record specific topics
-ros2 bag record \
-  /camera/image_raw \
-  /lidar/points \
-  /imu/data \
-  --storage mcap \
-  -o robot_drive_session
-
-
-# LZ4 = faster write, moderate compression (good for real-time recording)
-ros2 bag record -a --storage mcap \
-  --compression-mode file \
-  --compression-format lz4 \
-  -o compressed_session
-
-# Zstandard = slower write, better compression (good for post-processing)
-ros2 bag record -a --storage mcap \
-  --compression-mode file \
-  --compression-format zstd \
-  -o compressed_zstd_session
-
-# convert existing rosbag2 to mcap format
-ros2 bag convert \
-  -i robot_drive_old/ \
-  -o robot_drive_mcap/ \
-  --output-options '{"storage_id": "mcap"}'
-```
-
-
-<!-- Generate Doc -->
-```bash
-chmod +x doc/build_docs.sh
-
-./build_docs.sh #Собирает статику в mkdocs/site/
-./build_docs.sh build # То же самое явно
-./build_docs.sh serve # Live-preview с авто-перезагрузкой
-```
-
-<!-- iiwa controller - docker -->
-<!-- Lightweight cobot -->
-```bash
-docker build -t evilfisru/lwc:... -f docker/jazzy/.../Dockerfile .
-
-docker run -it --rm --network host evilfisru/lwa:jazzy-lwa7-noble
-```
-
-```bash
-# release - src/build/log удаляются (по умолчанию)
-`docker build -t my-image .`
-
-# dev - src остаётся для отладки
-`docker build --build-arg BUILD_TYPE=dev -t my-image .`
-```
-
-```
-# URL для доступа к MCP LLM
-http://localhost:8007/mcp/mcp
-
-```

cobot-setting.yaml

@@ -50,7 +50,6 @@ 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,8 +109,6 @@ _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,7 +122,6 @@ 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
 ```
@@ -132,18 +131,10 @@ 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 |
 
-When `token` is set, every request to a protected REST or MCP route must include
+After startup, the REST API is available at `http://<host>:8007`, and MCP is available at `/mcp`.
-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,7 +124,6 @@ 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
 ```
@@ -134,18 +133,10 @@ 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` | Путь к файлу ограничений суставов для валидации команд |
 
-Если `token` заполнен, каждый запрос к защищённому REST- или MCP-маршруту
+После запуска REST API доступен по адресу `http://<host>:8007`, MCP — по пути `/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,7 +16,6 @@ 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
 ~~~
@@ -29,65 +28,7 @@ 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.
 
-When `host` is not `localhost`, `127.0.0.1`, or another loopback address, the
+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.
-`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
 
@@ -707,4 +648,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**. 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.
+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.

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

@@ -16,7 +16,6 @@ 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
 ~~~
@@ -29,65 +28,7 @@ web:
 
 Отдельного health-check в сервере нет. Если открывается Swagger UI, HTTP-сервер запущен. Готовность ROS-компонентов проверяется при обращении к конкретному маршруту.
 
-Если `host` отличается от `localhost`, `127.0.0.1` или другого loopback-адреса,
+По умолчанию сервер слушает все сетевые интерфейсы и не использует аутентификацию. Не публикуйте порт 8007 в недоверенную сеть. Для локальной работы укажите **host: 127.0.0.1**, а для удалённого доступа ограничьте сеть правилами firewall или VPN.
-поле `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>` в его настройках.
 
 ## Подготовка к примерам
 
@@ -155,7 +96,7 @@ GET **/robot/joint_states** возвращает последнее сообще
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/robot/joint_states", headers=HEADERS, timeout=T_READ)
+    response = httpx.get(f"{HOST}/robot/joint_states", timeout=T_READ)
     response.raise_for_status()
     state = response.json()
     print(dict(zip(state["name"], state["position"])))
@@ -198,7 +139,7 @@ GET **/robot/pose** вычисляет прямую кинематику чер
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/robot/pose", headers=HEADERS, timeout=T_READ)
+    response = httpx.get(f"{HOST}/robot/pose", timeout=T_READ)
     response.raise_for_status()
     pose = response.json()
     print(pose["position"])
@@ -231,7 +172,7 @@ GET **/robot/positions** читает положения group_state из SRDF.
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/robot/positions", headers=HEADERS, timeout=T_READ)
+    response = httpx.get(f"{HOST}/robot/positions", timeout=T_READ)
     response.raise_for_status()
     for position in response.json():
         print(position["name"], "—", position["description"])
@@ -282,7 +223,6 @@ 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,
     )
@@ -334,7 +274,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", headers=HEADERS, json=target, timeout=T_MOVE)
+    response = httpx.post(f"{HOST}/robot/move/pose", json=target, timeout=T_MOVE)
     response.raise_for_status()
     print(response.json())
     ~~~
@@ -379,7 +319,6 @@ 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,
@@ -443,7 +382,7 @@ POST **/trajectory/send** принимает одну или несколько
         ],
         "validate_limits": True,
     }
-    response = httpx.post(f"{HOST}/trajectory/send", headers=HEADERS, json=trajectory, timeout=T_READ)
+    response = httpx.post(f"{HOST}/trajectory/send", json=trajectory, timeout=T_READ)
     response.raise_for_status()
     print(response.json())
     ~~~
@@ -491,7 +430,6 @@ 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,
@@ -530,7 +468,7 @@ GET **/trajectory/logs?n=50** возвращает до 300 последних
 === "Python"
 
     ~~~ python
-    response = httpx.get(f"{HOST}/trajectory/logs", headers=HEADERS, params={"n": 20}, timeout=T_READ)
+    response = httpx.get(f"{HOST}/trajectory/logs", params={"n": 20}, timeout=T_READ)
     response.raise_for_status()
     for line in response.json()["lines"]:
         print(line)
@@ -600,7 +538,6 @@ 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,
@@ -640,11 +577,11 @@ POST **/sequences/start** запускает отдельный процесс m
 === "Python"
 
     ~~~ python
-    status = httpx.get(f"{HOST}/sequences/status", headers=HEADERS, timeout=T_READ)
+    status = httpx.get(f"{HOST}/sequences/status", timeout=T_READ)
     status.raise_for_status()
     print(status.json())
 
-    logs = httpx.get(f"{HOST}/sequences/logs", headers=HEADERS, params={"n": 50}, timeout=T_READ)
+    logs = httpx.get(f"{HOST}/sequences/logs", params={"n": 50}, timeout=T_READ)
     logs.raise_for_status()
     for line in logs.json()["lines"]:
         print(line)
@@ -680,7 +617,7 @@ POST **/stop** останавливает запущенный runner, публ
 === "Python"
 
     ~~~ python
-    response = httpx.post(f"{HOST}/stop", headers=HEADERS, timeout=T_READ)
+    response = httpx.post(f"{HOST}/stop", timeout=T_READ)
     response.raise_for_status()
     print(response.json())
     ~~~
@@ -711,4 +648,4 @@ POST **/stop** останавливает запущенный runner, публ
 3. Убедитесь, что стек запущен полностью: controller_manager, MoveIt и iiwa_motion_server.
 4. После запуска последовательности посмотрите **/sequences/logs**; после публикации траектории — **/trajectory/logs**.
 
-MCP-сервер работает в том же процессе, но это отдельный интерфейс: его адрес — **http://адрес-сервера:8007/mcp/mcp**. Он использует тот же Bearer-токен; передайте заголовок `Authorization` при подключении MCP-клиента. Для обычных HTTP-интеграций используйте маршруты из этой страницы.
+MCP-сервер работает в том же процессе, но это отдельный интерфейс: его адрес — **http://адрес-сервера:8007/mcp/mcp**. Для обычных HTTP-интеграций используйте маршруты из этой страницы.

src/iiwa_bringup/launch/iiwa.launch.py

@@ -4,22 +4,19 @@ import sys
 sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
 
 from launch import LaunchDescription
-from launch.actions import (
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
-    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 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 iiwa_utils import converter, setting_loader
 
+from supported.moveit_nodes import make_moveit_nodes
+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
+
 
 def _runtime_setup(context, *args, **kwargs):
     setup = []
@@ -55,7 +52,6 @@ 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(
@@ -122,7 +118,6 @@ 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,10 +1,12 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.event_handlers import OnProcessExit
 from launch.substitutions import LaunchConfiguration
+from launch.event_handlers import OnProcessExit
+from launch.actions import RegisterEventHandler
 from launch_ros.actions import Node
 from webots_ros2_driver.urdf_spawner import URDFSpawner
 
+
 from iiwa_utils import converter
 
 
@@ -20,7 +22,6 @@ 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}
@@ -29,7 +30,6 @@ 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,7 +139,6 @@ 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,15 +18,11 @@ 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,9 +48,8 @@ planning:
 
 web:
   enabled: true
-  host: "127.0.0.1"
+  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
 
@@ -80,4 +79,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,11 +16,6 @@ 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)
 
@@ -80,28 +75,6 @@ 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}
@@ -114,14 +87,10 @@ install(DIRECTORY include/
   DESTINATION include
 )
 
-install(DIRECTORY external/libFRI/include/
-  DESTINATION include
-)
-
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(export_${PROJECT_NAME})
 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

@@ -1,89 +0,0 @@
-#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,35 +2,26 @@
 
 #include <array>
 #include <atomic>
-#include <cstddef>
+#include <mutex>
-#include <cstdint>
 
 #include "friClientApplication.h"
 #include "friLBRClient.h"
 #include "friUdpConnection.h"
 
-#include "iiwa_controller/filters.hpp"
-
 namespace iiwa_controller
 {
 
 
-// Снимок состояния робота публикуется через короткий seqlock: FRI-поток не
+// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке
-// блокируется на mutex, а ros2_control читает только согласованный снимок.
+// и так же за один lock читается из read() в потоке управления.
 struct IIWAStateSnapshot
 {
-  std::array<double, 7> measured_pos{}; // фактическая позиция из FRI
+  std::array<double, 7> measured_pos{}; // в Commanding = filtered_pos_ (open-loop)
   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};  // наносекундная часть [нс]
@@ -54,39 +45,22 @@ public:
   void onStateChange(
     KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
 
-  // API для ros2_control. Обмен данными не использует mutex в RT-пути.
+  // Потокобезопасное API для ros2_control, вызывается из read() и write()
   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};
 
-  // Цель публикуется поэлементно атомарно, чтобы read()/write() не
+  mutable std::mutex data_mutex_;
-  // блокировали FRI callback и не создавали data race.
+  std::array<double, N_JOINTS> target_pos_{};
-  std::array<std::atomic<double>, N_JOINTS> target_pos_atomic_{};
   // Сглаженная позиция, которую реально отправляем роботу.
   // Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
   std::array<double, N_JOINTS> filtered_pos_{};
-  std::array<std::atomic<double>, N_JOINTS> measured_pos_{};
+  IIWAStateSnapshot snapshot_{};
-  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,9 +22,7 @@
 #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
 {
@@ -65,11 +63,6 @@ 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_;
@@ -80,7 +73,6 @@ private:
   // read() лишь читает готовый снимок — без блокировки RT-потока.
   std::thread fri_thread_;
   std::atomic<bool> fri_running_{false};
-  std::atomic<bool> communication_fault_{false};
   void friThreadFunc();
 
   // Хэндлы интерфейсов состояния, заполняются в on_activate
@@ -98,11 +90,7 @@ 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

@@ -1,99 +0,0 @@
-#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

@@ -1,18 +0,0 @@
-#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,9 +19,6 @@
 
   <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,13 +23,7 @@ static const char * friStateName(KUKA::FRI::ESessionState s)
 FRIClient::FRIClient(double joint_position_tau)
 : joint_position_tau_(joint_position_tau)
 {
-  for (size_t i = 0; i < N_JOINTS; ++i) {
+  target_pos_.fill(0.0);
-    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);
 }
 
@@ -37,26 +31,20 @@ FRIClient::FRIClient(double joint_position_tau)
 // В Monitor-режиме getIpoJointPosition() бросает FRIException, поэтому здесь не зовём.
 void FRIClient::captureMonitoringData()
 {
-  const auto & state = robotState();
+  std::memcpy(
-  const auto * measured_pos = state.getMeasuredJointPosition();
+    snapshot_.measured_pos.data(),
-  const auto * measured_tau = state.getMeasuredTorque();
+    robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double));
-  const auto * external_tau = state.getExternalTorque();
+  std::memcpy(
-  for (size_t i = 0; i < N_JOINTS; ++i) {
+    snapshot_.measured_tau.data(),
-    measured_pos_[i].store(measured_pos[i], std::memory_order_relaxed);
+    robotState().getMeasuredTorque(), N_JOINTS * sizeof(double));
-    measured_tau_[i].store(measured_tau[i], std::memory_order_relaxed);
+  std::memcpy(
-    external_tau_[i].store(external_tau[i], std::memory_order_relaxed);
+    snapshot_.external_tau.data(),
-  }
+    robotState().getExternalTorque(), N_JOINTS * sizeof(double));
-  sample_time_.store(state.getSampleTime(), std::memory_order_relaxed);
+  snapshot_.sample_time = robotState().getSampleTime();
-  quality_.store(state.getConnectionQuality(), std::memory_order_relaxed);
+  snapshot_.quality = robotState().getConnectionQuality();
-  safety_state_.store(state.getSafetyState(), std::memory_order_relaxed);
+  snapshot_.ipo_valid = false;
-  operation_mode_.store(state.getOperationMode(), std::memory_order_relaxed);
+  snapshot_.time_stamp_sec = robotState().getTimestampSec();
-  drive_state_.store(state.getDriveState(), std::memory_order_relaxed);
+  snapshot_.time_stamp_nano_sec = robotState().getTimestampNanoSec();
-  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).
@@ -64,18 +52,20 @@ void FRIClient::captureMonitoringData()
 void FRIClient::captureCommandingData()
 {
   captureMonitoringData();
-  const auto * ipo_pos = robotState().getIpoJointPosition();
+  std::memcpy(
-  for (size_t i = 0; i < N_JOINTS; ++i) {
+    snapshot_.ipo_pos.data(),
-    ipo_pos_[i].store(ipo_pos[i], std::memory_order_relaxed);
+    robotState().getIpoJointPosition(), N_JOINTS * sizeof(double));
-  }
+  snapshot_.ipo_valid = true;
-  ipo_valid_.store(true, std::memory_order_relaxed);
+  // Open-loop: JTC видит filtered_pos_ как «измеренную» позицию — как в lbr_fri_ros2_stack.
+  // Благодаря этому JTC не видит расхождения и не генерирует коррекций.
+  snapshot_.measured_pos = filtered_pos_;
 }
 
 // Вызывается в 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.
@@ -86,39 +76,35 @@ void FRIClient::monitor()
 // статическое отклонение не даст выполниться этому условию.
 void FRIClient::waitForCommand()
 {
+  std::lock_guard<std::mutex> lock(data_mutex_);
   captureCommandingData();
 
   // Инициализируем цель и фильтр IPO-позицией.
   // Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE.
-  for (size_t i = 0; i < N_JOINTS; ++i) {
+  std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
-    const double ipo = ipo_pos_[i].load(std::memory_order_relaxed);
+  std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
-    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::array<double, N_JOINTS> target{};
+  std::lock_guard<std::mutex> lock(data_mutex_);
-  for (size_t i = 0; i < N_JOINTS; ++i) {
-    target[i] = target_pos_atomic_[i].load(std::memory_order_relaxed);
-  }
 
+  // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_
+  // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом).
+  // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA.
   const double dt = robotState().getSampleTime();
-  const double alpha = exponentialFilterAlpha(joint_position_tau_, dt);
+  const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0;
   for (size_t i = 0; i < N_JOINTS; ++i) {
-    filtered_pos_[i] = alpha * target[i] + (1.0 - alpha) * filtered_pos_[i];
+    filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
   }
 
   robotCommand().setJointPosition(filtered_pos_.data());
 
-  // Захватываем фактическое состояние FRI после отправки команды.
+  // Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил.
   captureCommandingData();
-  snapshot_generation_.fetch_add(1, std::memory_order_release);
 }
 
 void FRIClient::onStateChange(
@@ -146,32 +132,14 @@ void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
       return;
     }
   }
-  for (size_t i = 0; i < N_JOINTS; ++i) {
+  std::lock_guard<std::mutex> lock(data_mutex_);
-    target_pos_atomic_[i].store(q[i], std::memory_order_relaxed);
+  target_pos_ = q;
-  }
 }
 
 IIWAStateSnapshot FRIClient::getStateSnapshot() const
 {
-  IIWAStateSnapshot result{};
+  std::lock_guard<std::mutex> lock(data_mutex_);
-  for (size_t i = 0; i < N_JOINTS; ++i) {
+  return snapshot_;
-    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
@@ -184,9 +152,4 @@ 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,11 +1,8 @@
 #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"
@@ -13,7 +10,6 @@
 #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,
@@ -47,24 +43,11 @@ CallbackReturn IIWAHardwareInterface::on_init(
 
   const auto & info = params.hardware_info;
 
-  try {
+  robot_ip_            = getParam(info, "robot_ip", "192.170.10.2");
-    robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
+  fri_port_            = std::stoi(getParam(info, "fri_port", "30200"));
-    fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
+  simulate_            = (getParam(info, "simulate", "false") == "true");
-    const auto simulate = getParam(info, "simulate", "false");
+  joint_position_tau_  = std::stod(getParam(info, "joint_position_tau", "0.04"));
-    simulate_ = simulate == "true" || simulate == "1" || simulate == "yes";
+  joint_velocity_tau_  = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
-    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",
@@ -79,84 +62,6 @@ 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);
@@ -200,9 +105,6 @@ 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;
   }
 
@@ -243,20 +145,13 @@ 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::MONITORING_READY &&
+       fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs;
-       !communication_fault_.load(std::memory_order_acquire) && elapsed < kTimeoutMs;
        elapsed += kPollMs)
   {
     RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
@@ -264,17 +159,10 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs));
   }
 
-  if (fri_client_->getSessionState() < KUKA::FRI::MONITORING_READY ||
+  if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
-      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();
@@ -283,10 +171,6 @@ 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();
@@ -300,12 +184,12 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
 {
   RCLCPP_INFO(rclcpp::get_logger(LOG), "Деактивация...");
 
-  if (!simulate_) {
+  if (!simulate_ && fri_running_.load()) {
-    fri_running_.store(false, std::memory_order_release);
+    fri_running_.store(false, std::memory_order_relaxed);
-    // UdpConnection не обещает потокобезопасный disconnect(). Сначала ждём
+    // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке.
-    // завершения step() (таймаут сокета 100 мс), затем закрываем приложение.
+    // Только потом join(), иначе он зависнет навсегда.
-    if (fri_thread_.joinable()) { fri_thread_.join(); }
     if (app_) { app_->disconnect(); }
+    if (fri_thread_.joinable()) { fri_thread_.join(); }
     RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток остановлен");
   }
 
@@ -315,7 +199,6 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
   }
 
   velocity_initialized_ = false;
-  command_initialized_ = false;
   return CallbackReturn::SUCCESS;
 }
 
@@ -324,13 +207,9 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
 
 CallbackReturn IIWAHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &)
 {
-  fri_running_.store(false, std::memory_order_release);
-  if (fri_thread_.joinable()) {
-    fri_thread_.join();
-  }
-  app_.reset();
-  connection_.reset();
   fri_client_.reset();
+  connection_.reset();
+  app_.reset();
   return CallbackReturn::SUCCESS;
 }
 
@@ -340,28 +219,11 @@ void IIWAHardwareInterface::friThreadFunc()
 {
   RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен");
 
-  try {
+  while (fri_running_.load(std::memory_order_relaxed)) {
-    while (fri_running_.load(std::memory_order_acquire)) {
+    if (!app_->step()) {
-      if (!app_->step()) {
+      RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-        communication_fault_.store(true, std::memory_order_release);
+        "FRI: step() вернул false, возможно потеряли соединение");
-        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 поток завершён");
@@ -400,15 +262,11 @@ 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 = exponentialFilterAlpha(joint_velocity_tau_, dt);
+    const double vel_alpha = (joint_velocity_tau_ > 0.0)
+      ? dt / (joint_velocity_tau_ + dt)
+      : 1.0;
 
     for (size_t i = 0; i < N_JOINTS; ++i) {
       const double raw     = (snap.measured_pos[i] - prev_pos_[i]) / dt;
@@ -442,29 +300,7 @@ 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();
@@ -477,12 +313,6 @@ 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) {
@@ -504,51 +334,16 @@ hardware_interface::return_type IIWAHardwareInterface::write(
     return hardware_interface::return_type::OK;
   }
 
-  if (communication_fault_.load(std::memory_order_acquire)) {
+  if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) {
-    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

@@ -1,120 +0,0 @@
-#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

@@ -1,34 +0,0 @@
-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,20 +54,5 @@ 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,8 +28,6 @@
 
   <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,7 +14,6 @@
 import json
 import shutil
 import threading
-import tempfile
 import time
 from pathlib import Path
 
@@ -31,9 +30,6 @@ 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
 
@@ -96,15 +92,8 @@ class MotionSequenceRunner(Node):
 
 
     def _init_bag(self):
-        bag_dir = Path(self._bag_path).expanduser().resolve(strict=False)
+        bag_dir = Path(self._bag_path)
-        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}')
 
@@ -182,7 +171,6 @@ class MotionSequenceRunner(Node):
             return False
 
         done = threading.Event()
-        goal_handle_holder = [None]
         result_holder: list[bool] = [False]
 
         def _on_result(future):
@@ -192,7 +180,6 @@ 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()
@@ -200,12 +187,7 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
-        if not done.wait(timeout=ACTION_RESULT_TIMEOUT_SEC):
+        done.wait()
-            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:
@@ -224,7 +206,6 @@ class MotionSequenceRunner(Node):
             return False
 
         done = threading.Event()
-        goal_handle_holder = [None]
         result_holder: list[bool] = [False]
 
         def _on_result(future):
@@ -234,7 +215,6 @@ 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()
@@ -242,12 +222,7 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
-        if not done.wait(timeout=ACTION_RESULT_TIMEOUT_SEC):
+        done.wait()
-            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:
@@ -261,18 +236,12 @@ class MotionSequenceRunner(Node):
             for i in range(self._n_iter):
                 self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
 
-                if not self._send_waypoint(self._home):
+                self._send_waypoint(self._home)
-                    self.get_logger().info('Home waypoint failed, stopping sequence')
-                    return
 
                 for idx, wp in enumerate(self._waypoints):
-                    if not self._send_waypoint(wp, idx=idx):
+                    self._send_waypoint(wp, idx=idx)
-                        self.get_logger().info(f'Waypoint #{idx} failed, stopping sequence')
-                        return
 
-                if not self._send_waypoint(self._home):
+                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,8 +1,6 @@
 #!/usr/bin/env python3
 
-import math
 import threading
-import time
 
 import rclpy
 from rclpy.node import Node
@@ -30,10 +28,6 @@ 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)."""
@@ -65,14 +59,12 @@ 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")
@@ -81,19 +73,17 @@ 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=self._handle_pose_goal,
+            goal_callback=lambda _: GoalResponse.ACCEPT,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
         ActionServer(
             self, MoveToJoints, "cobot/move_to_joints", self._execute_joints,
             callback_group=cb,
-            goal_callback=self._handle_joints_goal,
+            goal_callback=lambda _: GoalResponse.ACCEPT,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
         self.create_service(MoveToNamedPose, "cobot/move_to_named", self._handle_named, callback_group=cb)
@@ -109,148 +99,6 @@ 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):
         """Планирует траекторию и выполняет её с поддержкой отмены.
 
@@ -258,7 +106,7 @@ class IiwaMotionServer(Node):
         (None, 'canceled') если цель была отменена.
         """
         plan_result = self._arm.plan(single_plan_parameters=plan_params)
-        if not self._plan_result_valid(plan_result):
+        if not plan_result:
             return False, "Планирование не удалось: поза недостижима или в столкновении"
 
         if goal_handle.is_cancel_requested:
@@ -266,14 +114,10 @@ class IiwaMotionServer(Node):
 
         done = threading.Event()
         failed = threading.Event()
-        execution_success = {"ok": False}
 
         def do_execute():
             try:
-                execution_result = self._moveit.execute(plan_result.trajectory, controllers=[])
+                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()
@@ -281,17 +125,15 @@ class IiwaMotionServer(Node):
                 done.set()
 
         threading.Thread(target=do_execute, daemon=True).start()
-        started_at = time.monotonic()
 
-        while not done.wait(timeout=EXECUTION_POLL_PERIOD_SEC):
+        while not done.wait(timeout=0.05):
             if goal_handle.is_cancel_requested:
-                self._stop_execution()
+                try:
-                done.wait(timeout=STOP_WAIT_TIMEOUT_SEC)
+                    self._moveit.get_trajectory_execution_manager().stop_execution()
+                except Exception:
+                    pass
+                done.wait()
                 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, "Выполнение траектории завершилось ошибкой"
@@ -312,164 +154,123 @@ class IiwaMotionServer(Node):
         return result
 
     def _execute_pose(self, goal_handle):
-        reserved_here = False
         req = goal_handle.request
         feedback = MoveToPose.Feedback()
         result = MoveToPose.Result()
 
-        try:
+        velocity_scale = max(0.01, min(1.0, float(req.speed)))
-            reserved_here = self._ensure_motion_reservation("pose")
+        planner_key = (req.planner or self._default_planner).lower()
-            velocity_scale, error = self._normalize_scale(req.speed, "speed")
-            if error is not None:
-                return self._finish_action(goal_handle, result, False, error)
-            error = self._validate_pose_goal(req)
-            if error is not None:
-                return self._finish_action(goal_handle, result, False, error)
 
-            planner_key = (req.planner or self._default_planner).lower()
+        if planner_key not in PLANNERS:
-            if planner_key not in PLANNERS:
+            result.success = False
-                return self._finish_action(
+            result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
-                    goal_handle,
+            self.get_logger().error(result.message)
-                    result,
+            goal_handle.abort()
-                    False,
+            return result
-                    f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}",
-                )
 
-            pipeline, planner_id, plan_time = PLANNERS[planner_key]
+        pipeline, planner_id, plan_time = PLANNERS[planner_key]
 
-            pose = PoseStamped()
+        pose = PoseStamped()
-            pose.header.frame_id = req.frame_id or self._default_frame
+        pose.header.frame_id = req.frame_id or self._default_frame
-            pose.pose.position.x = req.x
+        pose.pose.position.x = req.x
-            pose.pose.position.y = req.y
+        pose.pose.position.y = req.y
-            pose.pose.position.z = req.z
+        pose.pose.position.z = req.z
-            qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c)
+        qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c)
-            pose.pose.orientation.x = qx
+        pose.pose.orientation.x = qx
-            pose.pose.orientation.y = qy
+        pose.pose.orientation.y = qy
-            pose.pose.orientation.z = qz
+        pose.pose.orientation.z = qz
-            pose.pose.orientation.w = qw
+        pose.pose.orientation.w = qw
 
-            self.get_logger().info(
+        self.get_logger().info(
-                f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f})  "
+            f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f})  "
-                f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад  "
+            f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад  "
-                f"speed={velocity_scale:.2f}  planner={planner_key}"
+            f"speed={velocity_scale:.2f}  planner={planner_key}"
-            )
+        )
 
-            feedback.state = "planning"
+        feedback.state = "planning"
-            goal_handle.publish_feedback(feedback)
+        goal_handle.publish_feedback(feedback)
 
-            self._arm.set_start_state_to_current_state()
+        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_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link)
 
-            feedback.state = "executing"
+        feedback.state = "executing"
-            goal_handle.publish_feedback(feedback)
+        goal_handle.publish_feedback(feedback)
 
-            plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
+        plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
-            ok, msg = self._plan_and_execute(plan_params, goal_handle)
+        ok, msg = self._plan_and_execute(plan_params, goal_handle)
-            return self._finish_action(goal_handle, result, ok, msg)
+        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()
 
-        try:
+        velocity_scale = max(0.01, min(1.0, float(req.speed)))
-            reserved_here = self._ensure_motion_reservation("joints")
+        joints = list(req.joints)
-            velocity_scale, error = self._normalize_scale(req.speed, "speed")
-            if error is not None:
-                return self._finish_action(goal_handle, result, False, error)
 
-            joints, error = self._validate_joint_goal(req.joints)
+        self.get_logger().info(
-            if error is not None:
+            f"[joints] {[f'{v:.3f}' for v in joints]}  speed={velocity_scale:.2f}"
-                return self._finish_action(goal_handle, result, False, error)
+        )
 
-            self.get_logger().info(
+        feedback.state = "planning"
-                f"[joints] {[f'{v:.3f}' for v in joints]}  speed={velocity_scale:.2f}"
+        goal_handle.publish_feedback(feedback)
-            )
 
-            feedback.state = "planning"
+        # Формируем целевое состояние по суставным координатам
-            goal_handle.publish_feedback(feedback)
+        goal_state = RobotState(self._robot_model)
+        goal_state.set_joint_group_positions(self._planning_group, joints)
+        goal_state.update()
 
-            goal_state = RobotState(self._robot_model)
+        self._arm.set_start_state_to_current_state()
-            goal_state.set_joint_group_positions(self._planning_group, joints)
+        self._arm.set_goal_state(robot_state=goal_state)
-            goal_state.update()
 
-            self._arm.set_start_state_to_current_state()
+        feedback.state = "executing"
-            self._arm.set_goal_state(robot_state=goal_state)
+        goal_handle.publish_feedback(feedback)
 
-            feedback.state = "executing"
+        plan_params = self._make_plan_params(
-            goal_handle.publish_feedback(feedback)
+            "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
-
+        )
-            plan_params = self._make_plan_params(
+        ok, msg = self._plan_and_execute(plan_params, goal_handle)
-                "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+        return self._finish_action(goal_handle, result, ok, msg)
-            )
-            ok, msg = self._plan_and_execute(plan_params, goal_handle)
-            return self._finish_action(goal_handle, result, ok, msg)
-        finally:
-            if reserved_here or self._motion_lock.locked():
-                self._release_motion()
 
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
-        if not self._reserve_motion("named"):
+        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
+
+        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 = self._busy_motion_message()
+            response.message = f"Неизвестное состояние '{name}': {exc}"
             self.get_logger().error(response.message)
             return response
 
-        try:
+        plan_params = self._make_plan_params(
-            name = request.name.strip()
+            "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
-            velocity_scale, error = self._normalize_scale(request.speed, "speed")
+        )
-            if error is not None:
+        plan_result = self._arm.plan(single_plan_parameters=plan_params)
-                response.success = False
+        if not plan_result:
-                response.message = error
+            response.success = False
-                self.get_logger().error(response.message)
+            response.message = f"Не удалось построить траекторию для '{name}'"
-                return response
+            self.get_logger().error(response.message)
-
-            accel_scale, error = self._normalize_named_accel(request.accel_scale, velocity_scale)
-            if error is not None:
-                response.success = False
-                response.message = error
-                self.get_logger().error(response.message)
-                return response
-
-            self.get_logger().info(
-                f"[named] name='{name}'  speed={velocity_scale:.2f}  accel={accel_scale:.2f}"
-            )
-
-            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()
+        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
 
     def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
         try:

src/iiwa_planning/test/conftest.py

@@ -1,232 +0,0 @@
-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

@@ -1,73 +0,0 @@
-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

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

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -75,7 +75,6 @@ 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
 
@@ -190,7 +189,6 @@ _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",
 }
@@ -203,14 +201,10 @@ 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,14 +1,9 @@
-import hmac
-import ipaddress
-import os
 import threading
 from contextlib import asynccontextmanager
 
 import rclpy
 import uvicorn
-from fastapi import FastAPI, Request
+from fastapi import FastAPI
-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
@@ -18,100 +13,11 @@ 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
@@ -119,16 +25,6 @@ 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))
@@ -162,8 +58,6 @@ 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', '127.0.0.1')
+        self.declare_parameter('host', '0.0.0.0')
         self.declare_parameter('port', 8007)
         self.declare_parameter('endpoints_path', '')
         self.declare_parameter('joint_limits_path', '')
@@ -102,11 +102,7 @@ class CobotWebNode(Node):
         deadline = time.monotonic() + timeout
         while not result_future.done():
             if time.monotonic() > deadline:
-                cancel_future = goal_handle.cancel_goal_async()
+                raise TimeoutError(f"Таймаут выполнения action '{action_name}'")
-                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,5 +1,4 @@
 import os
-import re
 import signal
 import subprocess
 import threading
@@ -14,12 +13,8 @@ from .ros_node import get_bridge
 
 router = APIRouter(prefix="/sequences", tags=["sequences"])
 
-_TEMP_ROOT = Path(tempfile.gettempdir()) / "iiwa_web"
+_UPLOAD_DIR = Path(tempfile.gettempdir()) / "iiwa_configs"
-_UPLOAD_DIR = _TEMP_ROOT / "uploads"
+_UPLOAD_DIR.mkdir(exist_ok=True)
-_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
 
@@ -27,43 +22,12 @@ _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]:
@@ -99,14 +63,9 @@ async def start_runner(
         if _process and _process.poll() is None:
             raise HTTPException(409, f"Runner уже запущен (pid={_process.pid})")
 
-        filename = _sanitize_upload_name(config.filename)
+        filename = config.filename or "config.json"
         dest = _UPLOAD_DIR / filename
-        content = await config.read()
+        dest.write_bytes(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()]
 
@@ -115,7 +74,7 @@ async def start_runner(
             config_path=str(dest),
             n_iterations=n_iterations,
             delay=delay_between_iterations,
-            bag_path=safe_bag_path,
+            bag_path=bag_path,
             topics=topics_list,
             joints_action=joints_action,
             pose_action=pose_action,

src/iiwa_web/iiwa_web/trajectory.py

@@ -1,11 +1,10 @@
 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, field_validator, model_validator
+from pydantic import BaseModel, Field
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
 from .config_loader import load_joint_limits, load_joint_names
@@ -42,32 +41,6 @@ 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
@@ -90,35 +63,11 @@ 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):
@@ -127,7 +76,9 @@ def send_trajectory(req: SendRequest):
     Публикует `JointTrajectory` в `/iiwa_arm_controller/joint_trajectory`.
     """
     rows = [(wp.positions, wp.time_from_start) for wp in req.points]
-    _validate_rows(rows)
+
+    if req.validate_limits:
+        _validate_limits([r[0] for r in rows])
 
     _publish(_build_msg(rows))
     _log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
@@ -212,7 +163,8 @@ async def send_csv_trajectory(
     if not rows:
         raise HTTPException(422, "CSV не содержит точек траектории")
 
-    _validate_rows(rows)
+    if validate_limits:
+        _validate_limits([r[0] for r in rows])
 
     _publish(_build_msg(rows))
     _log(f"[csv] {file.filename} → {len(rows)} точек, t_end={rows[-1][1]:.2f}с")

src/iiwa_web/package.xml

@@ -11,18 +11,8 @@
   <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,14 +14,11 @@ 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

@@ -1,198 +0,0 @@
-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

@@ -1,65 +0,0 @@
-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

@@ -1,130 +0,0 @@
-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

@@ -1,118 +0,0 @@
-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

@@ -1,137 +0,0 @@
-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)