diff --git a/.gitignore b/.gitignore index a2fbf38..7ff6dad 100644 --- a/.gitignore +++ b/.gitignore @@ -10,7 +10,9 @@ venv .venv .claude .codex +.agents __pycache__ +.pytest_cache *.egg-info **.FCBak diff --git a/README.md b/README.md index 605eb59..23f6a35 100644 --- a/README.md +++ b/README.md @@ -65,7 +65,7 @@ Docker можно использовать как альтернативную ### Требования -- Ubuntu 24.04 LTS; +- Ubuntu 24.044 LTS; - доступ в интернет; - права `sudo`; - физический KUKA LBR iiwa 7 R800 либо компьютер для работы только с симулятором. diff --git a/cobot-setting.yaml b/cobot-setting.yaml index a2aef58..face0cf 100644 --- a/cobot-setting.yaml +++ b/cobot-setting.yaml @@ -50,6 +50,7 @@ web: enabled: true host: "0.0.0.0" port: 8007 + token: "ysXOyL_p3-f2YH2WqQ808KgQUA32qACziKdfqbhsJsPI0GGkoRbGn9eU22FV8mdS" endpoints: pkg://iiwa_config/config/api_endpoints.yaml joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml diff --git a/cobot/commands/robot_setup.py b/cobot/commands/robot_setup.py index c70e2a4..c252348 100644 --- a/cobot/commands/robot_setup.py +++ b/cobot/commands/robot_setup.py @@ -109,6 +109,8 @@ _BLOCKS: List[_Block] = [ options=["0.0.0.0", "127.0.0.1"]), _Field("port", "Порт HTTP:", "8007", note="Порт FastAPI-сервера (по умолчанию 8007)"), + _Field("token", "Токен Bearer для REST API и MCP:", "", + note="Длинный секрет. Хранится в cobot-setting.yaml и нужен при внешнем host; пустое значение отключает авторизацию"), ], ), _Block( diff --git a/doc/lwc-doc/docs/getting-started/configuration.en.md b/doc/lwc-doc/docs/getting-started/configuration.en.md index 17d1c86..0ca9867 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.en.md +++ b/doc/lwc-doc/docs/getting-started/configuration.en.md @@ -122,6 +122,7 @@ web: enabled: true host: "0.0.0.0" port: 8007 + token: "replace-with-a-long-secret-token" endpoints: pkg://iiwa_config/config/api_endpoints.yaml joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml ``` @@ -131,10 +132,18 @@ web: | `enabled` | Enable (`true`) or disable (`false`) the web server | | `host` | Listening address: `0.0.0.0` for all interfaces or `127.0.0.1` for local access only | | `port` | HTTP API port; default: `8007` | +| `token` | Bearer token for the REST API and MCP; required for an external `host` | | `endpoints` | Path to the REST endpoint description | | `joint_limits` | Path to joint limits used for command validation | -After startup, the REST API is available at `http://:8007`, and MCP is available at `/mcp`. +When `token` is set, every request to a protected REST or MCP route must include +the `Authorization: Bearer ` 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://:8007`, and MCP is available at `/mcp/mcp`. --- diff --git a/doc/lwc-doc/docs/getting-started/configuration.md b/doc/lwc-doc/docs/getting-started/configuration.md index b68d2ad..30ed8f8 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.md +++ b/doc/lwc-doc/docs/getting-started/configuration.md @@ -124,6 +124,7 @@ web: enabled: true host: "0.0.0.0" port: 8007 + token: "замените-на-длинный-секретный-токен" endpoints: pkg://iiwa_config/config/api_endpoints.yaml joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml ``` @@ -133,10 +134,18 @@ web: | `enabled` | Включить (`true`) или отключить (`false`) веб-сервер | | `host` | Адрес прослушивания: `0.0.0.0` — все интерфейсы, `127.0.0.1` — только локально | | `port` | Порт HTTP API (по умолчанию `8007`) | +| `token` | Bearer-токен для REST API и MCP; обязателен для внешнего `host` | | `endpoints` | Путь к описанию REST-эндпоинтов | | `joint_limits` | Путь к файлу ограничений суставов для валидации команд | -После запуска REST API доступен по адресу `http://:8007`, MCP — по пути `/mcp`. +Если `token` заполнен, каждый запрос к защищённому REST- или MCP-маршруту +должен содержать заголовок `Authorization: Bearer `. Страницы +`/docs`, `/redoc` и `/openapi.json` доступны без токена только для загрузки +Swagger UI. Пустой `token` допустим только для локального доступа +(`127.0.0.1` или `localhost`); при внешнем `host` сервер не запустится. +Храните файл настроек с ограниченными правами доступа и не публикуйте токен. + +После запуска REST API доступен по адресу `http://:8007`, MCP — по пути `/mcp/mcp`. --- diff --git a/doc/lwc-doc/docs/getting-started/control/rest-api.en.md b/doc/lwc-doc/docs/getting-started/control/rest-api.en.md index a885caf..457412a 100644 --- a/doc/lwc-doc/docs/getting-started/control/rest-api.en.md +++ b/doc/lwc-doc/docs/getting-started/control/rest-api.en.md @@ -16,6 +16,7 @@ web: enabled: true host: 0.0.0.0 port: 8007 + token: "replace-with-a-long-secret-token" endpoints: pkg://iiwa_config/config/api_endpoints.yaml joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml ~~~ @@ -28,7 +29,65 @@ After starting the stack with **cobot run**, the server is available at **http:/ There is no separate health-check endpoint. If Swagger UI opens, the HTTP server is running. Readiness of ROS components is checked when a specific endpoint is called. -By default, the server listens on all network interfaces and does not use authentication. Do not expose port 8007 to an untrusted network. For local access, set **host: 127.0.0.1**. For remote access, restrict the network with firewall rules or a VPN. +When `host` is not `localhost`, `127.0.0.1`, or another loopback address, the +`token` field is required; otherwise the server exits during startup. When +`token` is set, Bearer authentication applies to every REST route and to the +MCP route `/mcp/mcp`, including with a local `host`. + +The `/docs`, `/redoc`, and `/openapi.json` resources are available without a +header so the browser can load Swagger UI. This does not expose control +commands. Open `/docs`, click **Authorize**, paste the `web.token` value without +the word `Bearer`, and confirm. Swagger adds the header to API requests. + +The token is stored in **cobot-setting.yaml**. Do not add it to documentation, +scripts, or public repositories. If it is exposed, replace it and restart the +stack. For remote access, also restrict port 8007 with a firewall or VPN. + +### REST and MCP authentication + +Every request to a protected REST route or MCP must include the following header +when `web.token` is set: + +~~~ http +Authorization: Bearer +~~~ + +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://:8007/mcp/mcp`. If the client supports custom HTTP headers, set +`Authorization: Bearer ` in its connection settings. ## Preparing the examples @@ -648,4 +707,4 @@ When troubleshooting, proceed from simple checks to more complex ones: 3. Make sure that the complete stack is running: `controller_manager`, MoveIt, and `iiwa_motion_server`. 4. After starting a sequence, inspect **/sequences/logs**. After publishing a trajectory, inspect **/trajectory/logs**. -The MCP server runs in the same process but provides a separate interface at **http://server-address:8007/mcp/mcp**. For ordinary HTTP integrations, use the endpoints documented on this page. +The MCP server runs in the same process but provides a separate interface at **http://server-address:8007/mcp/mcp**. It uses the same Bearer token; pass the `Authorization` header when connecting an MCP client. For ordinary HTTP integrations, use the endpoints documented on this page. diff --git a/doc/lwc-doc/docs/getting-started/control/rest-api.md b/doc/lwc-doc/docs/getting-started/control/rest-api.md index 45c018e..ed89125 100644 --- a/doc/lwc-doc/docs/getting-started/control/rest-api.md +++ b/doc/lwc-doc/docs/getting-started/control/rest-api.md @@ -16,6 +16,7 @@ web: enabled: true host: 0.0.0.0 port: 8007 + token: "замените-на-длинный-секретный-токен" endpoints: pkg://iiwa_config/config/api_endpoints.yaml joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml ~~~ @@ -28,7 +29,65 @@ web: Отдельного health-check в сервере нет. Если открывается Swagger UI, HTTP-сервер запущен. Готовность ROS-компонентов проверяется при обращении к конкретному маршруту. -По умолчанию сервер слушает все сетевые интерфейсы и не использует аутентификацию. Не публикуйте порт 8007 в недоверенную сеть. Для локальной работы укажите **host: 127.0.0.1**, а для удалённого доступа ограничьте сеть правилами firewall или VPN. +Если `host` отличается от `localhost`, `127.0.0.1` или другого loopback-адреса, +поле `token` обязательно: без него сервер завершит запуск с ошибкой. Если +`token` заполнен, Bearer-аутентификация применяется ко всем REST-маршрутам и +к MCP-маршруту `/mcp/mcp`, в том числе при локальном `host`. + +Страницы `/docs`, `/redoc` и схема `/openapi.json` доступны без заголовка, +чтобы браузер мог загрузить Swagger UI. Это не открывает команды управления. +Откройте `/docs`, нажмите **Authorize**, вставьте значение `web.token` без +слова `Bearer` и подтвердите. Swagger сам добавит нужный заголовок к запросам. + +Токен хранится в **cobot-setting.yaml**. Не добавляйте его в документацию, +скрипты или публичные репозитории; после утечки замените значение и перезапустите +стек. Для удалённого доступа дополнительно ограничьте порт 8007 firewall или VPN. + +### Аутентификация REST и MCP + +Каждый запрос к защищённому REST-маршруту или MCP при заполненном `web.token` +должен содержать: + +~~~ http +Authorization: Bearer <значение-web.token> +~~~ + +Примеры подготовки клиентов: + +=== "curl" + + ~~~ bash + HOST=http://localhost:8007 + API_TOKEN='скопируйте значение web.token из cobot-setting.yaml' + AUTH_HEADER="Authorization: Bearer ${API_TOKEN}" + curl -sS -H "${AUTH_HEADER}" $HOST/robot/joint_states + ~~~ + +=== "Python" + + ~~~ python + import httpx + + HOST = "http://localhost:8007" + API_TOKEN = "скопируйте значение web.token из cobot-setting.yaml" + HEADERS = {"Authorization": f"Bearer {API_TOKEN}"} + response = httpx.get(f"{HOST}/robot/joint_states", headers=HEADERS, timeout=10) + response.raise_for_status() + ~~~ + +=== "MATLAB" + + ~~~ matlab + HOST = 'http://localhost:8007'; + API_TOKEN = 'скопируйте значение web.token из cobot-setting.yaml'; + readOpts = weboptions('Timeout', 10, ... + 'HeaderFields', {'Authorization', ['Bearer ' API_TOKEN]}); + jointState = webread([HOST '/robot/joint_states'], readOpts); + ~~~ + +Тот же заголовок передаётся MCP-клиенту при подключении к +`http://:8007/mcp/mcp`. Если клиент поддерживает пользовательские HTTP +заголовки, укажите `Authorization: Bearer <значение-web.token>` в его настройках. ## Подготовка к примерам @@ -96,7 +155,7 @@ GET **/robot/joint_states** возвращает последнее сообще === "Python" ~~~ python - response = httpx.get(f"{HOST}/robot/joint_states", timeout=T_READ) + response = httpx.get(f"{HOST}/robot/joint_states", headers=HEADERS, timeout=T_READ) response.raise_for_status() state = response.json() print(dict(zip(state["name"], state["position"]))) @@ -139,7 +198,7 @@ GET **/robot/pose** вычисляет прямую кинематику чер === "Python" ~~~ python - response = httpx.get(f"{HOST}/robot/pose", timeout=T_READ) + response = httpx.get(f"{HOST}/robot/pose", headers=HEADERS, timeout=T_READ) response.raise_for_status() pose = response.json() print(pose["position"]) @@ -172,7 +231,7 @@ GET **/robot/positions** читает положения group_state из SRDF. === "Python" ~~~ python - response = httpx.get(f"{HOST}/robot/positions", timeout=T_READ) + response = httpx.get(f"{HOST}/robot/positions", headers=HEADERS, timeout=T_READ) response.raise_for_status() for position in response.json(): print(position["name"], "—", position["description"]) @@ -223,6 +282,7 @@ POST **/robot/move/named** перемещает манипулятор в пол ~~~ python response = httpx.post( f"{HOST}/robot/move/named", + headers=HEADERS, json={"name": "home", "speed": 0.1, "accel_scale": 0.0}, timeout=T_MOVE, ) @@ -274,7 +334,7 @@ POST **/robot/move/pose** принимает положение TCP в метр "a": 0.0, "b": 3.14159, "c": 0.0, "speed": 0.1, "planner": "ptp", "frame_id": "", } - response = httpx.post(f"{HOST}/robot/move/pose", json=target, timeout=T_MOVE) + response = httpx.post(f"{HOST}/robot/move/pose", headers=HEADERS, json=target, timeout=T_MOVE) response.raise_for_status() print(response.json()) ~~~ @@ -319,6 +379,7 @@ POST **/robot/move/joints** принимает ровно семь углов в ~~~ python response = httpx.post( f"{HOST}/robot/move/joints", + headers=HEADERS, json={ "joints": [0.0, 0.5, 0.0, -1.57, 0.0, 1.57, 0.0], "speed": 0.1, @@ -382,7 +443,7 @@ POST **/trajectory/send** принимает одну или несколько ], "validate_limits": True, } - response = httpx.post(f"{HOST}/trajectory/send", json=trajectory, timeout=T_READ) + response = httpx.post(f"{HOST}/trajectory/send", headers=HEADERS, json=trajectory, timeout=T_READ) response.raise_for_status() print(response.json()) ~~~ @@ -430,6 +491,7 @@ joint1,joint2,joint3,joint4,joint5,joint6,joint7,t with open("trajectory.csv", "rb") as csv_file: response = httpx.post( f"{HOST}/trajectory/send_csv", + headers=HEADERS, params={"separator": ",", "validate_limits": True}, files={"file": ("trajectory.csv", csv_file, "text/csv")}, timeout=T_READ, @@ -468,7 +530,7 @@ GET **/trajectory/logs?n=50** возвращает до 300 последних === "Python" ~~~ python - response = httpx.get(f"{HOST}/trajectory/logs", params={"n": 20}, timeout=T_READ) + response = httpx.get(f"{HOST}/trajectory/logs", headers=HEADERS, params={"n": 20}, timeout=T_READ) response.raise_for_status() for line in response.json()["lines"]: print(line) @@ -538,6 +600,7 @@ POST **/sequences/start** запускает отдельный процесс m with open("motion_sequence_config.json", "rb") as config: response = httpx.post( f"{HOST}/sequences/start", + headers=HEADERS, files={"config": ("motion_sequence_config.json", config, "application/json")}, data={"n_iterations": "3", "delay_between_iterations": "5.0"}, timeout=T_READ, @@ -577,11 +640,11 @@ POST **/sequences/start** запускает отдельный процесс m === "Python" ~~~ python - status = httpx.get(f"{HOST}/sequences/status", timeout=T_READ) + status = httpx.get(f"{HOST}/sequences/status", headers=HEADERS, timeout=T_READ) status.raise_for_status() print(status.json()) - logs = httpx.get(f"{HOST}/sequences/logs", params={"n": 50}, timeout=T_READ) + logs = httpx.get(f"{HOST}/sequences/logs", headers=HEADERS, params={"n": 50}, timeout=T_READ) logs.raise_for_status() for line in logs.json()["lines"]: print(line) @@ -617,7 +680,7 @@ POST **/stop** останавливает запущенный runner, публ === "Python" ~~~ python - response = httpx.post(f"{HOST}/stop", timeout=T_READ) + response = httpx.post(f"{HOST}/stop", headers=HEADERS, timeout=T_READ) response.raise_for_status() print(response.json()) ~~~ @@ -648,4 +711,4 @@ POST **/stop** останавливает запущенный runner, публ 3. Убедитесь, что стек запущен полностью: controller_manager, MoveIt и iiwa_motion_server. 4. После запуска последовательности посмотрите **/sequences/logs**; после публикации траектории — **/trajectory/logs**. -MCP-сервер работает в том же процессе, но это отдельный интерфейс: его адрес — **http://адрес-сервера:8007/mcp/mcp**. Для обычных HTTP-интеграций используйте маршруты из этой страницы. +MCP-сервер работает в том же процессе, но это отдельный интерфейс: его адрес — **http://адрес-сервера:8007/mcp/mcp**. Он использует тот же Bearer-токен; передайте заголовок `Authorization` при подключении MCP-клиента. Для обычных HTTP-интеграций используйте маршруты из этой страницы. diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index 14340c0..c04e5cd 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -4,18 +4,21 @@ import sys sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare - -from iiwa_utils import converter, setting_loader - from supported.moveit_nodes import make_moveit_nodes +from supported.optional_nodes import make_foxglove_node, make_web_server_node from supported.rviz_nodes import make_rviz_nodes from supported.simulation_nodes import make_simulation_nodes -from supported.optional_nodes import make_foxglove_node, make_web_server_node + +from iiwa_utils import converter, setting_loader def _runtime_setup(context, *args, **kwargs): @@ -52,6 +55,7 @@ def _runtime_setup(context, *args, **kwargs): "fri_port": str(settings.robot.port), "simulate": "false", "joint_position_tau": str(settings.robot.joint_position_tau), + "joint_velocity_tau": str(settings.robot.joint_velocity_tau), } robot_description = converter.load_robot_description( @@ -118,6 +122,7 @@ def _runtime_setup(context, *args, **kwargs): "controller_timer": str(settings.digital_twin.webots.controller_timer), "fri_cycle_ms": str(settings.robot.fri_cycle_ms), "joint_position_tau": str(settings.robot.joint_position_tau), + "joint_velocity_tau": str(settings.robot.joint_velocity_tau), "controller": settings.robot.active_controller, } diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 4e01598..adfa806 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/controllers.launch.py @@ -1,12 +1,10 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler from launch.event_handlers import OnProcessExit -from launch.actions import RegisterEventHandler +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from webots_ros2_driver.urdf_spawner import URDFSpawner - from iiwa_utils import converter @@ -22,6 +20,7 @@ def _setup_controllers(context, *args, **kwargs): controller = LaunchConfiguration("controller").perform(context) # "jtc" | "forward" fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context)) joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context) + joint_velocity_tau = LaunchConfiguration("joint_velocity_tau").perform(context) update_rate = 1000 // fri_cycle_ms xacro_args = {"initial_positions_file": initial_positions_file} @@ -30,6 +29,7 @@ def _setup_controllers(context, *args, **kwargs): xacro_args["simulate"] = "true" else: xacro_args["joint_position_tau"] = joint_position_tau + xacro_args["joint_velocity_tau"] = joint_velocity_tau robot_description = converter.load_robot_description( model_path=description, @@ -139,6 +139,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("fri_cycle_ms", default_value="5"), DeclareLaunchArgument("joint_position_tau", default_value="0.04"), + DeclareLaunchArgument("joint_velocity_tau", default_value="0.01"), DeclareLaunchArgument("controller", default_value="jtc"), OpaqueFunction(function=_setup_controllers), ]) diff --git a/src/iiwa_bringup/launch/supported/optional_nodes.py b/src/iiwa_bringup/launch/supported/optional_nodes.py index 8f93d94..3d3fc7e 100644 --- a/src/iiwa_bringup/launch/supported/optional_nodes.py +++ b/src/iiwa_bringup/launch/supported/optional_nodes.py @@ -18,11 +18,15 @@ def make_foxglove_node(settings, use_sim_time: bool) -> Node: def make_web_server_node(settings, use_sim_time: bool) -> Node: + token = getattr(settings.web, "token", None) + additional_env = {"IIWA_WEB_TOKEN": str(token)} if token else None + return Node( package="iiwa_web", executable="iiwa_web_server", output="screen", name="iiwa_web_server", + additional_env=additional_env, parameters=[{ "host": settings.web.host, "port": settings.web.port, diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 2352374..4d38743 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -48,8 +48,9 @@ planning: web: enabled: true - host: "0.0.0.0" + host: "127.0.0.1" port: 8007 + token: "ysXOyL_p3-f2YH2WqQ808KgQUA32qACziKdfqbhsJsPI0GGkoRbGn9eU22FV8mdS" endpoints: pkg://iiwa_config/config/api_endpoints.yaml joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml @@ -79,4 +80,4 @@ foxglove: - assets include_hidden: false # Показывать клиенту скрытые топики и сервисы (начинаются с _) asset_uri_allowlist: ['^package://(?:[-\w%]+/)*[-\w%.]+\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$'] # Regex-список URI вида package://..., из которых bridge разрешает отдавать файлы-ассеты (URDF, mesh и т.п.) - ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) \ No newline at end of file + ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) diff --git a/src/iiwa_controller/CMakeLists.txt b/src/iiwa_controller/CMakeLists.txt index 63af7d4..5208586 100644 --- a/src/iiwa_controller/CMakeLists.txt +++ b/src/iiwa_controller/CMakeLists.txt @@ -16,6 +16,11 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) +endif() + # FRI SDK set(FRI_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/libFRI) @@ -75,6 +80,28 @@ pluginlib_export_plugin_description_file( iiwa_hardware_interface_plugin.xml ) +if(BUILD_TESTING) + ament_add_gtest( + test_guards + test/test_guards.cpp + ) + target_include_directories(test_guards PRIVATE + include + ${FRI_SDK_DIR}/include + ${FRI_SDK_DIR}/src/nanopb-0.2.8 + ${FRI_SDK_DIR}/src/protobuf + ${FRI_SDK_DIR}/src/protobuf_gen + ${FRI_SDK_DIR}/src/base + ${FRI_SDK_DIR}/src/client_lbr + ${FRI_SDK_DIR}/src/connection + ) + + ament_add_pytest_test( + test_safety_contract + test/test_safety_contract.py + ) +endif() + # Установка — только библиотека и заголовки, без config/launch/urdf install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} @@ -87,10 +114,14 @@ install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY external/libFRI/include/ + DESTINATION include +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs) -ament_package() \ No newline at end of file +ament_package() diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index fa112b5..8a560c4 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -2,26 +2,35 @@ #include #include -#include +#include +#include #include "friClientApplication.h" #include "friLBRClient.h" #include "friUdpConnection.h" +#include "iiwa_controller/filters.hpp" + namespace iiwa_controller { -// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке -// и так же за один lock читается из read() в потоке управления. +// Снимок состояния робота публикуется через короткий seqlock: FRI-поток не +// блокируется на mutex, а ros2_control читает только согласованный снимок. struct IIWAStateSnapshot { - std::array measured_pos{}; // в Commanding = filtered_pos_ (open-loop) + std::array measured_pos{}; // фактическая позиция из FRI std::array measured_tau{}; // измеренные моменты [Нм] std::array external_tau{}; // внешние моменты без компенсации модели [Нм] std::array ipo_pos{}; // позиция интерполятора [рад], только в Commanding double sample_time{0.005}; // период цикла FRI [с] KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR}; + KUKA::FRI::ESafetyState safety_state{KUKA::FRI::SAFETY_STOP_LEVEL_2}; + KUKA::FRI::EOperationMode operation_mode{KUKA::FRI::TEST_MODE_1}; + KUKA::FRI::EDriveState drive_state{KUKA::FRI::OFF}; + KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::NO_COMMAND_MODE}; + KUKA::FRI::EControlMode control_mode{KUKA::FRI::NO_CONTROL}; + double tracking_performance{0.0}; bool ipo_valid{false}; // в Monitor-режиме IPO недоступна unsigned int time_stamp_sec{0}; // Unix-время пакета [с] unsigned int time_stamp_nano_sec{0}; // наносекундная часть [нс] @@ -45,22 +54,39 @@ public: void onStateChange( KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override; - // Потокобезопасное API для ros2_control, вызывается из read() и write() + // API для ros2_control. Обмен данными не использует mutex в RT-пути. void setTargetJointPositions(const std::array & q); IIWAStateSnapshot getStateSnapshot() const; bool isCommandingActive() const; KUKA::FRI::ESessionState getSessionState() const; + uint64_t getSnapshotGeneration() const; private: double joint_position_tau_; std::atomic session_state_{KUKA::FRI::IDLE}; - mutable std::mutex data_mutex_; - std::array target_pos_{}; + // Цель публикуется поэлементно атомарно, чтобы read()/write() не + // блокировали FRI callback и не создавали data race. + std::array, N_JOINTS> target_pos_atomic_{}; // Сглаженная позиция, которую реально отправляем роботу. // Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте. std::array filtered_pos_{}; - IIWAStateSnapshot snapshot_{}; + std::array, N_JOINTS> measured_pos_{}; + std::array, N_JOINTS> measured_tau_{}; + std::array, N_JOINTS> external_tau_{}; + std::array, N_JOINTS> ipo_pos_{}; + std::atomic sample_time_{0.005}; + std::atomic quality_{KUKA::FRI::POOR}; + std::atomic safety_state_{KUKA::FRI::SAFETY_STOP_LEVEL_2}; + std::atomic operation_mode_{KUKA::FRI::TEST_MODE_1}; + std::atomic drive_state_{KUKA::FRI::OFF}; + std::atomic client_command_mode_{KUKA::FRI::NO_COMMAND_MODE}; + std::atomic control_mode_{KUKA::FRI::NO_CONTROL}; + std::atomic tracking_performance_{0.0}; + std::atomic ipo_valid_{false}; + std::atomic time_stamp_sec_{0}; + std::atomic time_stamp_nano_sec_{0}; + std::atomic snapshot_generation_{0}; // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна) void captureMonitoringData(); diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 79f231e..e0ab588 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -22,7 +22,9 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "iiwa_controller/CommandGuard.hpp" #include "iiwa_controller/FRIClient.h" +#include "iiwa_controller/StateGuard.hpp" namespace iiwa_controller { @@ -63,6 +65,11 @@ private: // EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей. // joint_velocity_tau = 0 отключает фильтр (raw finite difference). double joint_velocity_tau_{0.01}; + std::array command_min_{}; + std::array command_max_{}; + std::array command_max_velocity_{}; + CommandGuard command_guard_; + StateGuard state_guard_; // Объекты FRI SDK std::unique_ptr fri_client_; @@ -73,6 +80,7 @@ private: // read() лишь читает готовый снимок — без блокировки RT-потока. std::thread fri_thread_; std::atomic fri_running_{false}; + std::atomic communication_fault_{false}; void friThreadFunc(); // Хэндлы интерфейсов состояния, заполняются в on_activate @@ -90,7 +98,11 @@ private: unsigned int last_ts_sec_{0}; unsigned int last_ts_nsec_{0}; bool velocity_initialized_{false}; + uint64_t last_snapshot_generation_{0}; + unsigned int stale_snapshot_cycles_{0}; void compute_velocity_(const IIWAStateSnapshot & snap); + std::array last_command_pos_{}; + bool command_initialized_{false}; // Отслеживание сессии FRI для обнаружения потери управления KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE}; diff --git a/src/iiwa_controller/package.xml b/src/iiwa_controller/package.xml index 479aa24..108d7bb 100644 --- a/src/iiwa_controller/package.xml +++ b/src/iiwa_controller/package.xml @@ -19,6 +19,9 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest + ament_cmake_pytest + python3-pytest ament_cmake diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index bb856f5..be39bfc 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/src/iiwa_controller/src/FRIClient.cpp @@ -23,7 +23,13 @@ static const char * friStateName(KUKA::FRI::ESessionState s) FRIClient::FRIClient(double joint_position_tau) : joint_position_tau_(joint_position_tau) { - target_pos_.fill(0.0); + for (size_t i = 0; i < N_JOINTS; ++i) { + target_pos_atomic_[i].store(0.0, std::memory_order_relaxed); + measured_pos_[i].store(0.0, std::memory_order_relaxed); + measured_tau_[i].store(0.0, std::memory_order_relaxed); + external_tau_[i].store(0.0, std::memory_order_relaxed); + ipo_pos_[i].store(0.0, std::memory_order_relaxed); + } filtered_pos_.fill(0.0); } @@ -31,20 +37,26 @@ FRIClient::FRIClient(double joint_position_tau) // В Monitor-режиме getIpoJointPosition() бросает FRIException, поэтому здесь не зовём. void FRIClient::captureMonitoringData() { - std::memcpy( - snapshot_.measured_pos.data(), - robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double)); - std::memcpy( - snapshot_.measured_tau.data(), - robotState().getMeasuredTorque(), N_JOINTS * sizeof(double)); - std::memcpy( - snapshot_.external_tau.data(), - robotState().getExternalTorque(), N_JOINTS * sizeof(double)); - snapshot_.sample_time = robotState().getSampleTime(); - snapshot_.quality = robotState().getConnectionQuality(); - snapshot_.ipo_valid = false; - snapshot_.time_stamp_sec = robotState().getTimestampSec(); - snapshot_.time_stamp_nano_sec = robotState().getTimestampNanoSec(); + const auto & state = robotState(); + const auto * measured_pos = state.getMeasuredJointPosition(); + const auto * measured_tau = state.getMeasuredTorque(); + const auto * external_tau = state.getExternalTorque(); + for (size_t i = 0; i < N_JOINTS; ++i) { + measured_pos_[i].store(measured_pos[i], std::memory_order_relaxed); + measured_tau_[i].store(measured_tau[i], std::memory_order_relaxed); + external_tau_[i].store(external_tau[i], std::memory_order_relaxed); + } + sample_time_.store(state.getSampleTime(), std::memory_order_relaxed); + quality_.store(state.getConnectionQuality(), std::memory_order_relaxed); + safety_state_.store(state.getSafetyState(), std::memory_order_relaxed); + operation_mode_.store(state.getOperationMode(), std::memory_order_relaxed); + drive_state_.store(state.getDriveState(), std::memory_order_relaxed); + client_command_mode_.store(state.getClientCommandMode(), std::memory_order_relaxed); + control_mode_.store(state.getControlMode(), std::memory_order_relaxed); + tracking_performance_.store(state.getTrackingPerformance(), std::memory_order_relaxed); + ipo_valid_.store(false, std::memory_order_relaxed); + time_stamp_sec_.store(state.getTimestampSec(), std::memory_order_relaxed); + time_stamp_nano_sec_.store(state.getTimestampNanoSec(), std::memory_order_relaxed); } // Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE). @@ -52,20 +64,18 @@ void FRIClient::captureMonitoringData() void FRIClient::captureCommandingData() { captureMonitoringData(); - std::memcpy( - snapshot_.ipo_pos.data(), - robotState().getIpoJointPosition(), N_JOINTS * sizeof(double)); - snapshot_.ipo_valid = true; - // Open-loop: JTC видит filtered_pos_ как «измеренную» позицию — как в lbr_fri_ros2_stack. - // Благодаря этому JTC не видит расхождения и не генерирует коррекций. - snapshot_.measured_pos = filtered_pos_; + const auto * ipo_pos = robotState().getIpoJointPosition(); + for (size_t i = 0; i < N_JOINTS; ++i) { + ipo_pos_[i].store(ipo_pos[i], std::memory_order_relaxed); + } + ipo_valid_.store(true, std::memory_order_relaxed); } // Вызывается в MONITORING_WAIT и MONITORING_READY void FRIClient::monitor() { - std::lock_guard lock(data_mutex_); captureMonitoringData(); + snapshot_generation_.fetch_add(1, std::memory_order_release); } // Вызывается в COMMANDING_WAIT. @@ -76,35 +86,39 @@ void FRIClient::monitor() // статическое отклонение не даст выполниться этому условию. void FRIClient::waitForCommand() { - std::lock_guard lock(data_mutex_); captureCommandingData(); // Инициализируем цель и фильтр IPO-позицией. // Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE. - std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double)); - std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double)); + for (size_t i = 0; i < N_JOINTS; ++i) { + const double ipo = ipo_pos_[i].load(std::memory_order_relaxed); + target_pos_atomic_[i].store(ipo, std::memory_order_relaxed); + filtered_pos_[i] = ipo; + } robotCommand().setJointPosition(filtered_pos_.data()); + snapshot_generation_.fetch_add(1, std::memory_order_release); } // Вызывается в COMMANDING_ACTIVE, основной цикл управления void FRIClient::command() { - std::lock_guard lock(data_mutex_); - - // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_ - // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом). - // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA. - const double dt = robotState().getSampleTime(); - const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0; + std::array target{}; for (size_t i = 0; i < N_JOINTS; ++i) { - filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i]; + target[i] = target_pos_atomic_[i].load(std::memory_order_relaxed); + } + + const double dt = robotState().getSampleTime(); + const double alpha = exponentialFilterAlpha(joint_position_tau_, dt); + for (size_t i = 0; i < N_JOINTS; ++i) { + filtered_pos_[i] = alpha * target[i] + (1.0 - alpha) * filtered_pos_[i]; } robotCommand().setJointPosition(filtered_pos_.data()); - // Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил. + // Захватываем фактическое состояние FRI после отправки команды. captureCommandingData(); + snapshot_generation_.fetch_add(1, std::memory_order_release); } void FRIClient::onStateChange( @@ -132,14 +146,32 @@ void FRIClient::setTargetJointPositions(const std::array & q) return; } } - std::lock_guard lock(data_mutex_); - target_pos_ = q; + for (size_t i = 0; i < N_JOINTS; ++i) { + target_pos_atomic_[i].store(q[i], std::memory_order_relaxed); + } } IIWAStateSnapshot FRIClient::getStateSnapshot() const { - std::lock_guard lock(data_mutex_); - return snapshot_; + IIWAStateSnapshot result{}; + for (size_t i = 0; i < N_JOINTS; ++i) { + result.measured_pos[i] = measured_pos_[i].load(std::memory_order_relaxed); + result.measured_tau[i] = measured_tau_[i].load(std::memory_order_relaxed); + result.external_tau[i] = external_tau_[i].load(std::memory_order_relaxed); + result.ipo_pos[i] = ipo_pos_[i].load(std::memory_order_relaxed); + } + result.sample_time = sample_time_.load(std::memory_order_relaxed); + result.quality = quality_.load(std::memory_order_relaxed); + result.safety_state = safety_state_.load(std::memory_order_relaxed); + result.operation_mode = operation_mode_.load(std::memory_order_relaxed); + result.drive_state = drive_state_.load(std::memory_order_relaxed); + result.client_command_mode = client_command_mode_.load(std::memory_order_relaxed); + result.control_mode = control_mode_.load(std::memory_order_relaxed); + result.tracking_performance = tracking_performance_.load(std::memory_order_relaxed); + result.ipo_valid = ipo_valid_.load(std::memory_order_relaxed); + result.time_stamp_sec = time_stamp_sec_.load(std::memory_order_relaxed); + result.time_stamp_nano_sec = time_stamp_nano_sec_.load(std::memory_order_relaxed); + return result; } bool FRIClient::isCommandingActive() const @@ -152,4 +184,9 @@ KUKA::FRI::ESessionState FRIClient::getSessionState() const return session_state_.load(std::memory_order_relaxed); } +uint64_t FRIClient::getSnapshotGeneration() const +{ + return snapshot_generation_.load(std::memory_order_acquire); +} + } // namespace iiwa_controller diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 22ba74c..9ef2207 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -1,8 +1,11 @@ #include "iiwa_controller/IIWAHardwareInterface.hpp" +#include #include #include #include +#include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -10,6 +13,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" +#include "friException.h" PLUGINLIB_EXPORT_CLASS( iiwa_controller::IIWAHardwareInterface, @@ -43,11 +47,24 @@ CallbackReturn IIWAHardwareInterface::on_init( const auto & info = params.hardware_info; - robot_ip_ = getParam(info, "robot_ip", "192.170.10.2"); - fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); - simulate_ = (getParam(info, "simulate", "false") == "true"); - joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04")); - joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01")); + try { + robot_ip_ = getParam(info, "robot_ip", "192.170.10.2"); + fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); + const auto simulate = getParam(info, "simulate", "false"); + simulate_ = simulate == "true" || simulate == "1" || simulate == "yes"; + joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04")); + joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01")); + } catch (const std::exception & ex) { + RCLCPP_FATAL(rclcpp::get_logger(LOG), "Некорректные параметры FRI: %s", ex.what()); + return CallbackReturn::ERROR; + } + + if (robot_ip_.empty() || fri_port_ < 1 || fri_port_ > 65535 || + !std::isfinite(joint_position_tau_) || joint_position_tau_ < 0.0 || + !std::isfinite(joint_velocity_tau_) || joint_velocity_tau_ < 0.0) { + RCLCPP_FATAL(rclcpp::get_logger(LOG), "Параметры FRI вне допустимого диапазона"); + return CallbackReturn::ERROR; + } RCLCPP_INFO(rclcpp::get_logger(LOG), "on_init: ip=%s port=%d simulate=%s pos_tau=%.3f vel_tau=%.3f", @@ -62,6 +79,84 @@ CallbackReturn IIWAHardwareInterface::on_init( return CallbackReturn::ERROR; } + command_min_.fill(-std::numeric_limits::infinity()); + command_max_.fill(std::numeric_limits::infinity()); + for (size_t i = 0; i < N_JOINTS; ++i) { + const std::string expected_name = "joint" + std::to_string(i + 1); + if (info.joints[i].name != expected_name) { + RCLCPP_FATAL(rclcpp::get_logger(LOG), + "Сустав %zu имеет имя '%s', ожидается '%s'", i, + info.joints[i].name.c_str(), expected_name.c_str()); + return CallbackReturn::ERROR; + } + + bool position_interface_found = false; + for (const auto & interface : info.joints[i].command_interfaces) { + if (interface.name != hardware_interface::HW_IF_POSITION) { + continue; + } + position_interface_found = true; + try { + auto min_value = interface.min; + auto max_value = interface.max; + if (min_value.empty()) { + const auto it = interface.parameters.find("min"); + if (it != interface.parameters.end()) { + min_value = it->second; + } + } + if (max_value.empty()) { + const auto it = interface.parameters.find("max"); + if (it != interface.parameters.end()) { + max_value = it->second; + } + } + if (!min_value.empty()) { + command_min_[i] = std::stod(min_value); + } + if (!max_value.empty()) { + command_max_[i] = std::stod(max_value); + } + } catch (const std::exception & ex) { + RCLCPP_FATAL(rclcpp::get_logger(LOG), + "Некорректные ограничения позиции для %s: %s", + expected_name.c_str(), ex.what()); + return CallbackReturn::ERROR; + } + const auto limits_it = info.limits.find(expected_name); + if (limits_it != info.limits.end() && limits_it->second.has_position_limits) { + if (!std::isfinite(command_min_[i])) { + command_min_[i] = limits_it->second.min_position; + } + if (!std::isfinite(command_max_[i])) { + command_max_[i] = limits_it->second.max_position; + } + } + } + if (!position_interface_found || !std::isfinite(command_min_[i]) || + !std::isfinite(command_max_[i]) || command_min_[i] > command_max_[i]) { + RCLCPP_FATAL(rclcpp::get_logger(LOG), + "Для %s отсутствует корректный position command interface", expected_name.c_str()); + return CallbackReturn::ERROR; + } + + const auto limits_it = info.limits.find(expected_name); + if (limits_it == info.limits.end() || !limits_it->second.has_velocity_limits || + !std::isfinite(limits_it->second.max_velocity) || + limits_it->second.max_velocity <= 0.0) { + RCLCPP_FATAL(rclcpp::get_logger(LOG), + "Для %s отсутствует корректное ограничение скорости", expected_name.c_str()); + return CallbackReturn::ERROR; + } + command_max_velocity_[i] = limits_it->second.max_velocity; + } + + CommandGuardLimits guard_limits; + guard_limits.min_position = command_min_; + guard_limits.max_position = command_max_; + guard_limits.max_velocity = command_max_velocity_; + command_guard_ = CommandGuard(guard_limits); + prev_pos_.fill(0.0); velocity_.fill(0.0); velocity_raw_.fill(0.0); @@ -105,6 +200,9 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State if (!app_->connect(fri_port_, robot_ip_.c_str())) { RCLCPP_FATAL(rclcpp::get_logger(LOG), "Не удалось открыть UDP-сокет на порту %d (робот: %s)", fri_port_, robot_ip_.c_str()); + app_.reset(); + connection_.reset(); + fri_client_.reset(); return CallbackReturn::ERROR; } @@ -145,13 +243,20 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State return CallbackReturn::SUCCESS; } + if (!fri_client_ || !connection_ || !app_) { + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI не сконфигурирован перед активацией"); + return CallbackReturn::ERROR; + } + + communication_fault_.store(false, std::memory_order_release); fri_running_.store(true, std::memory_order_relaxed); fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this); constexpr int kTimeoutMs = 15000; constexpr int kPollMs = 200; for (int elapsed = 0; - fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs; + fri_client_->getSessionState() < KUKA::FRI::MONITORING_READY && + !communication_fault_.load(std::memory_order_acquire) && elapsed < kTimeoutMs; elapsed += kPollMs) { RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000, @@ -159,10 +264,17 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs)); } - if (fri_client_->getSessionState() == KUKA::FRI::IDLE) { + if (fri_client_->getSessionState() < KUKA::FRI::MONITORING_READY || + communication_fault_.load(std::memory_order_acquire)) { RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s", kTimeoutMs / 1000, robot_ip_.c_str()); + fri_running_.store(false, std::memory_order_release); + if (fri_thread_.joinable()) { + fri_thread_.join(); + } + app_->disconnect(); + return CallbackReturn::ERROR; } else { RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI сессия установлена!"); const auto snap = fri_client_->getStateSnapshot(); @@ -171,6 +283,10 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State last_ts_nsec_ = snap.time_stamp_nano_sec; velocity_.fill(0.0); velocity_initialized_ = true; + last_snapshot_generation_ = fri_client_->getSnapshotGeneration(); + stale_snapshot_cycles_ = 0; + last_command_pos_ = snap.ipo_valid ? snap.ipo_pos : snap.measured_pos; + command_initialized_ = snap.ipo_valid; } previous_session_state_ = fri_client_->getSessionState(); @@ -184,12 +300,12 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat { RCLCPP_INFO(rclcpp::get_logger(LOG), "Деактивация..."); - if (!simulate_ && fri_running_.load()) { - fri_running_.store(false, std::memory_order_relaxed); - // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке. - // Только потом join(), иначе он зависнет навсегда. - if (app_) { app_->disconnect(); } + if (!simulate_) { + fri_running_.store(false, std::memory_order_release); + // UdpConnection не обещает потокобезопасный disconnect(). Сначала ждём + // завершения step() (таймаут сокета 100 мс), затем закрываем приложение. if (fri_thread_.joinable()) { fri_thread_.join(); } + if (app_) { app_->disconnect(); } RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток остановлен"); } @@ -199,6 +315,7 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat } velocity_initialized_ = false; + command_initialized_ = false; return CallbackReturn::SUCCESS; } @@ -207,9 +324,13 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat CallbackReturn IIWAHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &) { - fri_client_.reset(); - connection_.reset(); + fri_running_.store(false, std::memory_order_release); + if (fri_thread_.joinable()) { + fri_thread_.join(); + } app_.reset(); + connection_.reset(); + fri_client_.reset(); return CallbackReturn::SUCCESS; } @@ -219,11 +340,28 @@ void IIWAHardwareInterface::friThreadFunc() { RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен"); - while (fri_running_.load(std::memory_order_relaxed)) { - if (!app_->step()) { - RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000, - "FRI: step() вернул false, возможно потеряли соединение"); + try { + while (fri_running_.load(std::memory_order_acquire)) { + if (!app_->step()) { + communication_fault_.store(true, std::memory_order_release); + fri_running_.store(false, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), + "FRI: step() вернул false, соединение потеряно"); + break; + } } + } catch (const KUKA::FRI::FRIException & ex) { + communication_fault_.store(true, std::memory_order_release); + fri_running_.store(false, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Исключение FRI: %s", ex.getErrorMessage()); + } catch (const std::exception & ex) { + communication_fault_.store(true, std::memory_order_release); + fri_running_.store(false, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Ошибка FRI-потока: %s", ex.what()); + } catch (...) { + communication_fault_.store(true, std::memory_order_release); + fri_running_.store(false, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "Неизвестная ошибка FRI-потока"); } RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток завершён"); @@ -262,11 +400,15 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap) {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14}; static constexpr double kVelDeadband = 1e-4; + if (dt < 0.0) { + communication_fault_.store(true, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI timestamp пошёл назад"); + return; + } + if (dt > 0.0) { // EMA alpha для фильтра скорости: tau=0 → alpha=1 (без фильтра) - const double vel_alpha = (joint_velocity_tau_ > 0.0) - ? dt / (joint_velocity_tau_ + dt) - : 1.0; + const double vel_alpha = exponentialFilterAlpha(joint_velocity_tau_, dt); for (size_t i = 0; i < N_JOINTS; ++i) { const double raw = (snap.measured_pos[i] - prev_pos_[i]) / dt; @@ -300,7 +442,29 @@ hardware_interface::return_type IIWAHardwareInterface::read( return hardware_interface::return_type::OK; } + if (communication_fault_.load(std::memory_order_acquire)) { + return hardware_interface::return_type::ERROR; + } + const auto snap = fri_client_->getStateSnapshot(); + if (state_guard_.validateSnapshot(snap) != StateGuardResult::OK) { + communication_fault_.store(true, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI передал некорректный снимок состояния"); + return hardware_interface::return_type::ERROR; + } + + const auto generation = fri_client_->getSnapshotGeneration(); + if (generation == last_snapshot_generation_) { + ++stale_snapshot_cycles_; + if (stale_snapshot_cycles_ > 20U) { + communication_fault_.store(true, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI не передаёт новые пакеты состояния"); + return hardware_interface::return_type::ERROR; + } + } else { + last_snapshot_generation_ = generation; + stale_snapshot_cycles_ = 0; + } // Обнаружение потери управления: неожиданный выход из COMMANDING_ACTIVE. const auto current_state = fri_client_->getSessionState(); @@ -313,6 +477,12 @@ hardware_interface::return_type IIWAHardwareInterface::read( } previous_session_state_ = current_state; + if (snap.safety_state != KUKA::FRI::NORMAL_OPERATION) { + communication_fault_.store(true, std::memory_order_release); + RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI сообщил safety stop (%d)", snap.safety_state); + return hardware_interface::return_type::ERROR; + } + compute_velocity_(snap); for (size_t i = 0; i < N_JOINTS; ++i) { @@ -334,16 +504,51 @@ hardware_interface::return_type IIWAHardwareInterface::write( return hardware_interface::return_type::OK; } - if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) { + if (communication_fault_.load(std::memory_order_acquire)) { + return hardware_interface::return_type::ERROR; + } + + const auto current_state = fri_client_->getSessionState(); + if (current_state != KUKA::FRI::COMMANDING_ACTIVE) { return hardware_interface::return_type::OK; } + const auto snap = fri_client_->getStateSnapshot(); + const auto state_guard_result = + state_guard_.validatePositionState(snap, current_state); + if (state_guard_result != StateGuardResult::OK) { + communication_fault_.store(true, std::memory_order_release); + RCLCPP_ERROR_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 1000, + "StateGuard запретил position-команду: %s", + stateGuardResultName(state_guard_result)); + return hardware_interface::return_type::ERROR; + } + std::array 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; } diff --git a/src/iiwa_planning/CMakeLists.txt b/src/iiwa_planning/CMakeLists.txt index 8a4ad33..e0106ea 100644 --- a/src/iiwa_planning/CMakeLists.txt +++ b/src/iiwa_planning/CMakeLists.txt @@ -54,5 +54,20 @@ install(FILES DESTINATION share/${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test( + test_move_to_pose_server + test/test_move_to_pose_server.py + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} + ) + + ament_add_pytest_test( + test_motion_sequence_runner + test/test_motion_sequence_runner.py + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} + ) +endif() ament_package() diff --git a/src/iiwa_planning/package.xml b/src/iiwa_planning/package.xml index 0be8b87..7573448 100644 --- a/src/iiwa_planning/package.xml +++ b/src/iiwa_planning/package.xml @@ -28,6 +28,8 @@ ament_lint_auto ament_lint_common + ament_cmake_pytest + python3-pytest ament_cmake diff --git a/src/iiwa_planning/scripts/motion_sequence_runner.py b/src/iiwa_planning/scripts/motion_sequence_runner.py index 50fc822..5d3f9b3 100644 --- a/src/iiwa_planning/scripts/motion_sequence_runner.py +++ b/src/iiwa_planning/scripts/motion_sequence_runner.py @@ -14,6 +14,7 @@ import json import shutil import threading +import tempfile import time from pathlib import Path @@ -30,6 +31,9 @@ from rosidl_runtime_py.utilities import get_message from iiwa_msgs.action import MoveToJoints, MoveToPose +ACTION_RESULT_TIMEOUT_SEC = 300.0 + + def _is_joints_waypoint(wp: dict) -> bool: return "joints" in wp @@ -92,8 +96,15 @@ class MotionSequenceRunner(Node): def _init_bag(self): - bag_dir = Path(self._bag_path) + bag_dir = Path(self._bag_path).expanduser().resolve(strict=False) + tmp_root = Path(tempfile.gettempdir()).resolve(strict=False) + if bag_dir == tmp_root or tmp_root not in bag_dir.parents: + raise ValueError(f'bag_path must stay under temporary root {tmp_root}') + + self._bag_path = str(bag_dir) if bag_dir.exists(): + if not bag_dir.is_dir(): + raise ValueError(f'bag_path already exists and is not a directory: {bag_dir}') shutil.rmtree(bag_dir) self.get_logger().info(f'Removed existing bag at {self._bag_path}') @@ -171,6 +182,7 @@ class MotionSequenceRunner(Node): return False done = threading.Event() + goal_handle_holder = [None] result_holder: list[bool] = [False] def _on_result(future): @@ -180,6 +192,7 @@ class MotionSequenceRunner(Node): def _on_goal(future): gh = future.result() + goal_handle_holder[0] = gh if not gh.accepted: self.get_logger().error('MoveToJoints goal rejected') done.set() @@ -187,7 +200,12 @@ class MotionSequenceRunner(Node): gh.get_result_async().add_done_callback(_on_result) self._joints_client.send_goal_async(goal).add_done_callback(_on_goal) - done.wait() + if not done.wait(timeout=ACTION_RESULT_TIMEOUT_SEC): + gh = goal_handle_holder[0] + if gh is not None: + gh.cancel_goal_async() + self.get_logger().error('MoveToJoints action timed out and was canceled') + return False return result_holder[0] def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool: @@ -206,6 +224,7 @@ class MotionSequenceRunner(Node): return False done = threading.Event() + goal_handle_holder = [None] result_holder: list[bool] = [False] def _on_result(future): @@ -215,6 +234,7 @@ class MotionSequenceRunner(Node): def _on_goal(future): gh = future.result() + goal_handle_holder[0] = gh if not gh.accepted: self.get_logger().error('MoveToPose goal rejected') done.set() @@ -222,7 +242,12 @@ class MotionSequenceRunner(Node): gh.get_result_async().add_done_callback(_on_result) self._pose_client.send_goal_async(goal).add_done_callback(_on_goal) - done.wait() + if not done.wait(timeout=ACTION_RESULT_TIMEOUT_SEC): + gh = goal_handle_holder[0] + if gh is not None: + gh.cancel_goal_async() + self.get_logger().error('MoveToPose action timed out and was canceled') + return False return result_holder[0] def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool: @@ -236,12 +261,18 @@ class MotionSequenceRunner(Node): for i in range(self._n_iter): self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======') - self._send_waypoint(self._home) + if not self._send_waypoint(self._home): + self.get_logger().info('Home waypoint failed, stopping sequence') + return for idx, wp in enumerate(self._waypoints): - self._send_waypoint(wp, idx=idx) + if not self._send_waypoint(wp, idx=idx): + self.get_logger().info(f'Waypoint #{idx} failed, stopping sequence') + return - self._send_waypoint(self._home) + if not self._send_waypoint(self._home): + self.get_logger().info('Final home waypoint failed, stopping sequence') + return time.sleep(self._delay) self.get_logger().info('======= Sequence complete =======') diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index 1f33f09..1f22b49 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/src/iiwa_planning/scripts/move_to_pose_server.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 +import math import threading +import time import rclpy from rclpy.node import Node @@ -28,6 +30,10 @@ PLANNERS = { "chomp": ("chomp", "", 10.0), } +DEFAULT_EXECUTION_TIMEOUT_SEC = 30.0 +EXECUTION_POLL_PERIOD_SEC = 0.05 +STOP_WAIT_TIMEOUT_SEC = 0.1 + def _abc_to_quaternion(a: float, b: float, c: float): """ZYX Euler (радианы, конвенция KUKA ABC) → (qx, qy, qz, qw).""" @@ -59,12 +65,14 @@ class IiwaMotionServer(Node): self.declare_parameter("default_frame", "base_link") self.declare_parameter("default_planner", "ompl") self.declare_parameter("planning_attempts", 3) + self.declare_parameter("execution_timeout_sec", DEFAULT_EXECUTION_TIMEOUT_SEC) self._pose_link = self.get_parameter("pose_link").value self._planning_group = self.get_parameter("planning_group").value self._default_frame = self.get_parameter("default_frame").value self._default_planner = self.get_parameter("default_planner").value self._planning_attempts = self.get_parameter("planning_attempts").value + self._execution_timeout_sec = float(self.get_parameter("execution_timeout_sec").value) def _setup_moveit(self): self._moveit = MoveItPy(node_name="iiwa_motion_server") @@ -73,17 +81,19 @@ class IiwaMotionServer(Node): def _setup_servers(self): cb = ReentrantCallbackGroup() + self._motion_lock = threading.Lock() + self._active_motion_kind: str | None = None ActionServer( self, MoveToPose, "cobot/move_to_pose", self._execute_pose, callback_group=cb, - goal_callback=lambda _: GoalResponse.ACCEPT, + goal_callback=self._handle_pose_goal, cancel_callback=lambda _: CancelResponse.ACCEPT, ) ActionServer( self, MoveToJoints, "cobot/move_to_joints", self._execute_joints, callback_group=cb, - goal_callback=lambda _: GoalResponse.ACCEPT, + goal_callback=self._handle_joints_goal, cancel_callback=lambda _: CancelResponse.ACCEPT, ) self.create_service(MoveToNamedPose, "cobot/move_to_named", self._handle_named, callback_group=cb) @@ -99,6 +109,148 @@ class IiwaMotionServer(Node): params.max_acceleration_scaling_factor = accel_scale if accel_scale is not None else velocity_scale return params + def _busy_motion_message(self) -> str: + current = self._active_motion_kind or "motion" + return f"Another {current} is already in progress; parallel motions are not allowed" + + def _reserve_motion(self, motion_kind: str) -> bool: + if not self._motion_lock.acquire(blocking=False): + return False + self._active_motion_kind = motion_kind + return True + + def _release_motion(self): + if self._motion_lock.locked(): + self._active_motion_kind = None + self._motion_lock.release() + + def _handle_pose_goal(self, _goal_request): + if self._reserve_motion("pose"): + return GoalResponse.ACCEPT + self.get_logger().error(self._busy_motion_message()) + return GoalResponse.REJECT + + def _handle_joints_goal(self, _goal_request): + if self._reserve_motion("joints"): + return GoalResponse.ACCEPT + self.get_logger().error(self._busy_motion_message()) + return GoalResponse.REJECT + + def _ensure_motion_reservation(self, motion_kind: str) -> bool: + if self._motion_lock.locked(): + return False + if self._reserve_motion(motion_kind): + return True + raise RuntimeError(self._busy_motion_message()) + + def _normalize_scale(self, raw_value, label: str, minimum: float = 0.01) -> tuple[float | None, str | None]: + try: + value = float(raw_value) + except (TypeError, ValueError): + return None, f"Invalid {label}: expected a finite number" + + if not math.isfinite(value): + return None, f"Invalid {label}: expected a finite number" + + return max(minimum, min(1.0, value)), None + + def _normalize_named_accel(self, raw_value, velocity_scale: float) -> tuple[float | None, str | None]: + try: + value = float(raw_value) + except (TypeError, ValueError): + return None, "Invalid accel_scale: expected a finite number" + + if not math.isfinite(value): + return None, "Invalid accel_scale: expected a finite number" + + if value <= 0.0: + return velocity_scale, None + + return max(0.01, min(1.0, value)), None + + def _validate_pose_goal(self, request) -> str | None: + values = (request.x, request.y, request.z, request.a, request.b, request.c) + try: + finite = all(math.isfinite(float(value)) for value in values) + except (TypeError, ValueError): + finite = False + if not finite: + return "Invalid pose: all coordinates and ABC angles must be finite" + return None + + def _joint_bounds_for_group(self) -> list[tuple[float | None, float | None]] | None: + if hasattr(self._robot_model, "get_joint_bounds"): + return list(self._robot_model.get_joint_bounds(self._planning_group)) + return None + + def _validate_joint_goal(self, raw_joints) -> tuple[list[float] | None, str | None]: + try: + joints = [float(value) for value in raw_joints] + except (TypeError, ValueError): + return None, "Invalid joints: expected numeric joint targets" + + if not all(math.isfinite(value) for value in joints): + return None, "Invalid joints: expected finite joint targets" + + bounds = self._joint_bounds_for_group() + expected_count = len(bounds) if bounds is not None else 7 + if len(joints) != expected_count: + return None, f"Invalid joints: expected {expected_count} values" + + if bounds is None: + return joints, None + + for index, (value, bound_pair) in enumerate(zip(joints, bounds)): + lower, upper = bound_pair + if lower is not None and value < lower: + return None, f"Joint {index} violates limit [{lower}, {upper}]" + if upper is not None and value > upper: + return None, f"Joint {index} violates limit [{lower}, {upper}]" + + return joints, None + + def _plan_result_valid(self, plan_result) -> bool: + if not plan_result: + return False + + trajectory = getattr(plan_result, "trajectory", None) + if trajectory is None: + return False + + error_code = getattr(plan_result, "error_code", None) + if error_code is not None and getattr(error_code, "val", error_code) != 1: + return False + + success = getattr(plan_result, "success", None) + if success is not None and not bool(success): + return False + + return True + + def _execution_result_successful(self, execution_result) -> bool: + if execution_result is None: + return False + + success = getattr(execution_result, "success", None) + if success is not None: + return bool(success) + + error_code = getattr(execution_result, "error_code", None) + if error_code is not None: + return getattr(error_code, "val", error_code) == 1 + + status = getattr(execution_result, "status", None) + if status is not None: + return status == 1 + + return bool(execution_result) + + def _stop_execution(self): + try: + self._moveit.get_trajectory_execution_manager().stop_execution() + except Exception: + pass + def _plan_and_execute(self, plan_params: PlanRequestParameters, goal_handle): """Планирует траекторию и выполняет её с поддержкой отмены. @@ -106,7 +258,7 @@ class IiwaMotionServer(Node): (None, 'canceled') если цель была отменена. """ plan_result = self._arm.plan(single_plan_parameters=plan_params) - if not plan_result: + if not self._plan_result_valid(plan_result): return False, "Планирование не удалось: поза недостижима или в столкновении" if goal_handle.is_cancel_requested: @@ -114,10 +266,14 @@ class IiwaMotionServer(Node): done = threading.Event() failed = threading.Event() + execution_success = {"ok": False} def do_execute(): try: - self._moveit.execute(plan_result.trajectory, controllers=[]) + execution_result = self._moveit.execute(plan_result.trajectory, controllers=[]) + execution_success["ok"] = self._execution_result_successful(execution_result) + if not execution_success["ok"]: + failed.set() except Exception as exc: self.get_logger().error(f"Ошибка выполнения траектории: {exc}") failed.set() @@ -125,15 +281,17 @@ class IiwaMotionServer(Node): done.set() threading.Thread(target=do_execute, daemon=True).start() + started_at = time.monotonic() - while not done.wait(timeout=0.05): + while not done.wait(timeout=EXECUTION_POLL_PERIOD_SEC): if goal_handle.is_cancel_requested: - try: - self._moveit.get_trajectory_execution_manager().stop_execution() - except Exception: - pass - done.wait() + self._stop_execution() + done.wait(timeout=STOP_WAIT_TIMEOUT_SEC) return None, "canceled" + if time.monotonic() - started_at >= self._execution_timeout_sec: + self._stop_execution() + done.wait(timeout=STOP_WAIT_TIMEOUT_SEC) + return False, f"Trajectory execution timeout after {self._execution_timeout_sec:.2f} s" if failed.is_set(): return False, "Выполнение траектории завершилось ошибкой" @@ -154,123 +312,164 @@ class IiwaMotionServer(Node): return result def _execute_pose(self, goal_handle): + reserved_here = False req = goal_handle.request feedback = MoveToPose.Feedback() result = MoveToPose.Result() - velocity_scale = max(0.01, min(1.0, float(req.speed))) - planner_key = (req.planner or self._default_planner).lower() + try: + reserved_here = self._ensure_motion_reservation("pose") + velocity_scale, error = self._normalize_scale(req.speed, "speed") + if error is not None: + return self._finish_action(goal_handle, result, False, error) + error = self._validate_pose_goal(req) + if error is not None: + return self._finish_action(goal_handle, result, False, error) - if planner_key not in PLANNERS: - result.success = False - result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}" - self.get_logger().error(result.message) - goal_handle.abort() - return result + planner_key = (req.planner or self._default_planner).lower() + if planner_key not in PLANNERS: + return self._finish_action( + goal_handle, + result, + False, + f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}", + ) - pipeline, planner_id, plan_time = PLANNERS[planner_key] + pipeline, planner_id, plan_time = PLANNERS[planner_key] - pose = PoseStamped() - pose.header.frame_id = req.frame_id or self._default_frame - pose.pose.position.x = req.x - pose.pose.position.y = req.y - pose.pose.position.z = req.z - qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c) - pose.pose.orientation.x = qx - pose.pose.orientation.y = qy - pose.pose.orientation.z = qz - pose.pose.orientation.w = qw + pose = PoseStamped() + pose.header.frame_id = req.frame_id or self._default_frame + pose.pose.position.x = req.x + pose.pose.position.y = req.y + pose.pose.position.z = req.z + qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c) + pose.pose.orientation.x = qx + pose.pose.orientation.y = qy + pose.pose.orientation.z = qz + pose.pose.orientation.w = qw - self.get_logger().info( - f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f}) " - f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад " - f"speed={velocity_scale:.2f} planner={planner_key}" - ) + self.get_logger().info( + f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f}) " + f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад " + f"speed={velocity_scale:.2f} planner={planner_key}" + ) - feedback.state = "planning" - goal_handle.publish_feedback(feedback) + feedback.state = "planning" + goal_handle.publish_feedback(feedback) - self._arm.set_start_state_to_current_state() - self._arm.set_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link) + self._arm.set_start_state_to_current_state() + self._arm.set_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link) - feedback.state = "executing" - goal_handle.publish_feedback(feedback) + feedback.state = "executing" + goal_handle.publish_feedback(feedback) - plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale) - ok, msg = self._plan_and_execute(plan_params, goal_handle) - return self._finish_action(goal_handle, result, ok, msg) + plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale) + ok, msg = self._plan_and_execute(plan_params, goal_handle) + return self._finish_action(goal_handle, result, ok, msg) + finally: + if reserved_here or self._motion_lock.locked(): + self._release_motion() def _execute_joints(self, goal_handle): + reserved_here = False req = goal_handle.request feedback = MoveToJoints.Feedback() result = MoveToJoints.Result() - velocity_scale = max(0.01, min(1.0, float(req.speed))) - joints = list(req.joints) + try: + reserved_here = self._ensure_motion_reservation("joints") + velocity_scale, error = self._normalize_scale(req.speed, "speed") + if error is not None: + return self._finish_action(goal_handle, result, False, error) - self.get_logger().info( - f"[joints] {[f'{v:.3f}' for v in joints]} speed={velocity_scale:.2f}" - ) + joints, error = self._validate_joint_goal(req.joints) + if error is not None: + return self._finish_action(goal_handle, result, False, error) - feedback.state = "planning" - goal_handle.publish_feedback(feedback) + self.get_logger().info( + f"[joints] {[f'{v:.3f}' for v in joints]} speed={velocity_scale:.2f}" + ) - # Формируем целевое состояние по суставным координатам - goal_state = RobotState(self._robot_model) - goal_state.set_joint_group_positions(self._planning_group, joints) - goal_state.update() + feedback.state = "planning" + goal_handle.publish_feedback(feedback) - self._arm.set_start_state_to_current_state() - self._arm.set_goal_state(robot_state=goal_state) + goal_state = RobotState(self._robot_model) + goal_state.set_joint_group_positions(self._planning_group, joints) + goal_state.update() - feedback.state = "executing" - goal_handle.publish_feedback(feedback) + self._arm.set_start_state_to_current_state() + self._arm.set_goal_state(robot_state=goal_state) - plan_params = self._make_plan_params( - "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale - ) - ok, msg = self._plan_and_execute(plan_params, goal_handle) - return self._finish_action(goal_handle, result, ok, msg) + feedback.state = "executing" + goal_handle.publish_feedback(feedback) + + plan_params = self._make_plan_params( + "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale + ) + ok, msg = self._plan_and_execute(plan_params, goal_handle) + return self._finish_action(goal_handle, result, ok, msg) + finally: + if reserved_here or self._motion_lock.locked(): + self._release_motion() def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response): - name = request.name.strip() - velocity_scale = max(0.01, min(1.0, float(request.speed))) - raw_accel = float(request.accel_scale) - accel_scale = max(0.01, min(1.0, raw_accel)) if raw_accel > 0.0 else velocity_scale + if not self._reserve_motion("named"): + response.success = False + response.message = self._busy_motion_message() + self.get_logger().error(response.message) + return response - self.get_logger().info( - f"[named] name='{name}' speed={velocity_scale:.2f} accel={accel_scale:.2f}" - ) - - self._arm.set_start_state_to_current_state() try: - self._arm.set_goal_state(configuration_name=name) - except Exception as exc: - response.success = False - response.message = f"Неизвестное состояние '{name}': {exc}" - self.get_logger().error(response.message) - return response + name = request.name.strip() + velocity_scale, error = self._normalize_scale(request.speed, "speed") + if error is not None: + response.success = False + response.message = error + self.get_logger().error(response.message) + return response - plan_params = self._make_plan_params( - "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale - ) - plan_result = self._arm.plan(single_plan_parameters=plan_params) - if not plan_result: - response.success = False - response.message = f"Не удалось построить траекторию для '{name}'" - self.get_logger().error(response.message) - return response + accel_scale, error = self._normalize_named_accel(request.accel_scale, velocity_scale) + if error is not None: + response.success = False + response.message = error + self.get_logger().error(response.message) + return response - exec_result = self._moveit.execute(plan_result.trajectory, controllers=[]) - if exec_result: - response.success = True - response.message = f"Переместился в '{name}'" - self.get_logger().info(response.message) - else: - response.success = False - response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)" - self.get_logger().error(response.message) - return response + self.get_logger().info( + f"[named] name='{name}' speed={velocity_scale:.2f} accel={accel_scale:.2f}" + ) + + self._arm.set_start_state_to_current_state() + try: + self._arm.set_goal_state(configuration_name=name) + except Exception as exc: + response.success = False + response.message = f"Неизвестное состояние '{name}': {exc}" + self.get_logger().error(response.message) + return response + + plan_params = self._make_plan_params( + "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale + ) + plan_result = self._arm.plan(single_plan_parameters=plan_params) + if not self._plan_result_valid(plan_result): + response.success = False + response.message = f"Не удалось построить траекторию для '{name}'" + self.get_logger().error(response.message) + return response + + exec_result = self._moveit.execute(plan_result.trajectory, controllers=[]) + if self._execution_result_successful(exec_result): + response.success = True + response.message = f"Переместился в '{name}'" + self.get_logger().info(response.message) + else: + response.success = False + response.message = f"Выполнение траектории для '{name}' завершилось ошибкой" + self.get_logger().error(response.message) + return response + finally: + self._release_motion() def _handle_stop(self, request: Trigger.Request, response: Trigger.Response): try: diff --git a/src/iiwa_sunrise/src/ServerFriRos2.java b/src/iiwa_sunrise/src/ServerFriRos2.java index 819dfed..2e94f1f 100644 --- a/src/iiwa_sunrise/src/ServerFriRos2.java +++ b/src/iiwa_sunrise/src/ServerFriRos2.java @@ -362,9 +362,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication { try { _lbr.move(posHold.addMotionOverlay(_friOverlay)); } catch (Exception e) { - // Normal exit path when the ROS 2 client closes the FRI session. - // Штатный путь выхода при закрытии FRI-сессии со стороны ROS 2. - getLogger().info("FRI сеанс закрыт."); + getLogger().error("Ошибка Position режима [" + e.getClass().getSimpleName() + + "]: " + String.valueOf(e.getMessage())); } closeFriSession(); @@ -414,7 +413,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication { try { _lbr.move(posHold); } catch (Exception e) { - getLogger().info("FRI сеанс закрыт."); + getLogger().error("Ошибка Monitor режима [" + e.getClass().getSimpleName() + + "]: " + String.valueOf(e.getMessage())); } closeFriSession(); diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index 0161ab4..03b3de4 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -75,6 +75,7 @@ class WebCfg: enabled: bool host: str port: int + token: Optional[str] endpoints: str # resolved absolute path to api_endpoints.yaml joint_limits: str # resolved absolute path to joint_limits.yaml @@ -189,6 +190,7 @@ _WEB_DEFAULTS: Dict[str, Any] = { "enabled": False, "host": "0.0.0.0", "port": 8007, + "token": None, "endpoints": "pkg://iiwa_config/config/api_endpoints.yaml", "joint_limits": "pkg://iiwa_config/config/moveit/joint_limits.yaml", } @@ -201,10 +203,14 @@ def _parse_web(raw: Optional[Dict[str, Any]], settings_dir: str) -> WebCfg: def get(key: str) -> Any: return raw.get(key, _WEB_DEFAULTS[key]) + raw_token = get("token") + token = str(raw_token).strip() if raw_token is not None else "" + return WebCfg( enabled=bool(get("enabled")), host=str(get("host")), port=int(get("port")), + token=token or None, endpoints=resolve_path(str(get("endpoints")), settings_dir), joint_limits=resolve_path(str(get("joint_limits")), settings_dir), ) diff --git a/src/iiwa_web/iiwa_web/main.py b/src/iiwa_web/iiwa_web/main.py index e12f9e5..d2311cc 100644 --- a/src/iiwa_web/iiwa_web/main.py +++ b/src/iiwa_web/iiwa_web/main.py @@ -1,9 +1,14 @@ +import hmac +import ipaddress +import os import threading from contextlib import asynccontextmanager import rclpy import uvicorn -from fastapi import FastAPI +from fastapi import FastAPI, Request +from fastapi.openapi.utils import get_openapi +from fastapi.responses import JSONResponse from fastmcp import FastMCP from sensor_msgs.msg import JointState from std_srvs.srv import Trigger @@ -13,11 +18,100 @@ from .ros_node import CobotWebNode, get_bridge, set_bridge from . import runner, trajectory, positions +_PUBLIC_AUTH_PATHS = frozenset({ + "/docs", + "/docs/oauth2-redirect", + "/openapi.json", + "/redoc", +}) + + +def is_public_auth_path(path: str) -> bool: + return path in _PUBLIC_AUTH_PATHS + + +def install_bearer_openapi(app: FastAPI, token: str | None) -> None: + """Expose the Bearer scheme so Swagger UI can configure authenticated calls.""" + if not token: + return + + def custom_openapi(): + if app.openapi_schema: + return app.openapi_schema + + schema = get_openapi( + title=app.title, + version=app.version, + description=app.description, + routes=app.routes, + ) + schema.setdefault("components", {}).setdefault("securitySchemes", {})[ + "BearerAuth" + ] = { + "type": "http", + "scheme": "bearer", + "bearerFormat": "opaque-token", + } + + for path_item in schema.get("paths", {}).values(): + for operation in path_item.values(): + if isinstance(operation, dict): + operation["security"] = [{"BearerAuth": []}] + + app.openapi_schema = schema + return app.openapi_schema + + app.openapi = custom_openapi + + +def ensure_safe_bind(host: str, token: str | None) -> None: + normalized = host.strip().lower() + if normalized == "localhost": + return + + try: + if ipaddress.ip_address(normalized).is_loopback: + return + except ValueError: + pass + + if not token: + raise RuntimeError("Внешний bind Web API разрешён только при заданном IIWA_WEB_TOKEN") + + +def has_valid_bearer_token(auth_header: str | None, token: str | None) -> bool: + if not token: + return True + if not auth_header: + return False + + scheme, _, provided_token = auth_header.partition(" ") + if scheme.lower() != "bearer" or not provided_token: + return False + return hmac.compare_digest(provided_token, token) + + +def install_token_auth(app: FastAPI, token: str | None) -> None: + if not token: + return + + @app.middleware("http") + async def require_bearer_token(request: Request, call_next): + if is_public_auth_path(request.url.path): + return await call_next(request) + + if not has_valid_bearer_token(request.headers.get("authorization"), token): + return JSONResponse( + status_code=401, + content={"detail": "Bearer token required"}, + headers={"WWW-Authenticate": "Bearer"}, + ) + return await call_next(request) + + def main(): rclpy.init() node = CobotWebNode() - set_bridge(node) - threading.Thread(target=rclpy.spin, args=(node,), daemon=True).start() host = node.get_parameter('host').value port = node.get_parameter('port').value @@ -25,6 +119,16 @@ def main(): joint_limits_path = node.get_parameter('joint_limits_path').value or None positions.init() + token = os.getenv("IIWA_WEB_TOKEN") or None + try: + ensure_safe_bind(host, token) + except Exception: + node.destroy_node() + rclpy.shutdown() + raise + + set_bridge(node) + threading.Thread(target=rclpy.spin, args=(node,), daemon=True).start() _schema_app = FastAPI() _schema_app.include_router(build_dynamic_router(endpoints_path, joint_limits_path)) @@ -58,6 +162,8 @@ def main(): except RuntimeError: return {"status": "stopped", "success": True, "message": "Планировщик не запущен"} + install_bearer_openapi(app, token) + install_token_auth(app, token) uvicorn.run(app, host=host, port=port) diff --git a/src/iiwa_web/iiwa_web/ros_node.py b/src/iiwa_web/iiwa_web/ros_node.py index c4eb960..b99e014 100644 --- a/src/iiwa_web/iiwa_web/ros_node.py +++ b/src/iiwa_web/iiwa_web/ros_node.py @@ -11,7 +11,7 @@ class CobotWebNode(Node): def __init__(self): super().__init__('cobot_web_node') - self.declare_parameter('host', '0.0.0.0') + self.declare_parameter('host', '127.0.0.1') self.declare_parameter('port', 8007) self.declare_parameter('endpoints_path', '') self.declare_parameter('joint_limits_path', '') @@ -102,7 +102,11 @@ class CobotWebNode(Node): deadline = time.monotonic() + timeout while not result_future.done(): if time.monotonic() > deadline: - raise TimeoutError(f"Таймаут выполнения action '{action_name}'") + cancel_future = goal_handle.cancel_goal_async() + cancel_deadline = time.monotonic() + min(2.0, max(timeout, 0.1)) + while not cancel_future.done() and time.monotonic() <= cancel_deadline: + time.sleep(0.01) + raise TimeoutError(f"Таймаут выполнения action '{action_name}', отправлена отмена goal") time.sleep(0.05) return result_future.result() diff --git a/src/iiwa_web/iiwa_web/runner.py b/src/iiwa_web/iiwa_web/runner.py index 8a80949..d4d2161 100644 --- a/src/iiwa_web/iiwa_web/runner.py +++ b/src/iiwa_web/iiwa_web/runner.py @@ -1,4 +1,5 @@ import os +import re import signal import subprocess import threading @@ -13,8 +14,12 @@ from .ros_node import get_bridge router = APIRouter(prefix="/sequences", tags=["sequences"]) -_UPLOAD_DIR = Path(tempfile.gettempdir()) / "iiwa_configs" -_UPLOAD_DIR.mkdir(exist_ok=True) +_TEMP_ROOT = Path(tempfile.gettempdir()) / "iiwa_web" +_UPLOAD_DIR = _TEMP_ROOT / "uploads" +_BAG_ROOT = _TEMP_ROOT / "bags" +_UPLOAD_DIR.mkdir(parents=True, exist_ok=True) +_BAG_ROOT.mkdir(parents=True, exist_ok=True) +MAX_UPLOAD_BYTES = 1_048_576 _LOG_BUFFER = 300 @@ -22,12 +27,43 @@ _process: Optional[subprocess.Popen] = None _log_lines: deque[str] = deque(maxlen=_LOG_BUFFER) _lock = threading.Lock() +_SAFE_NAME_RE = re.compile(r"[^A-Za-z0-9._-]+") + def _stream_output(proc: subprocess.Popen) -> None: for line in proc.stdout: _log_lines.append(line.rstrip("\n")) +def _sanitize_upload_name(filename: str | None) -> str: + candidate = Path(filename or "config.json").name + if candidate in {"", ".", ".."}: + raise HTTPException(422, "Имя файла конфигурации недопустимо") + + sanitized = _SAFE_NAME_RE.sub("_", candidate) + if sanitized in {"", ".", ".."}: + raise HTTPException(422, "Имя файла конфигурации недопустимо") + return sanitized + + +def _resolve_bag_path(raw_bag_path: str) -> str: + if not raw_bag_path: + return "" + + candidate = Path(raw_bag_path) + resolved = candidate.resolve(strict=False) if candidate.is_absolute() else (_BAG_ROOT / candidate).resolve(strict=False) + allowed_root = _BAG_ROOT.resolve(strict=False) + if resolved == allowed_root: + raise HTTPException(422, "bag_path должен указывать на каталог внутри разрешённого временного каталога") + try: + resolved.relative_to(allowed_root) + except ValueError as exc: + raise HTTPException(422, "bag_path должен находиться внутри разрешённого временного каталога") from exc + + resolved.parent.mkdir(parents=True, exist_ok=True) + return str(resolved) + + def _build_cmd(config_path: str, n_iterations: int, delay: float, bag_path: str, topics: list[str], joints_action: str, pose_action: str) -> list[str]: @@ -63,9 +99,14 @@ async def start_runner( if _process and _process.poll() is None: raise HTTPException(409, f"Runner уже запущен (pid={_process.pid})") - filename = config.filename or "config.json" + filename = _sanitize_upload_name(config.filename) dest = _UPLOAD_DIR / filename - dest.write_bytes(await config.read()) + content = await config.read() + if len(content) > MAX_UPLOAD_BYTES: + raise HTTPException(413, f"Файл конфигурации слишком большой: максимум {MAX_UPLOAD_BYTES} байт") + dest.write_bytes(content) + + safe_bag_path = _resolve_bag_path(bag_path) topics_list = [t.strip() for t in topics.split(",") if t.strip()] @@ -74,7 +115,7 @@ async def start_runner( config_path=str(dest), n_iterations=n_iterations, delay=delay_between_iterations, - bag_path=bag_path, + bag_path=safe_bag_path, topics=topics_list, joints_action=joints_action, pose_action=pose_action, diff --git a/src/iiwa_web/iiwa_web/trajectory.py b/src/iiwa_web/iiwa_web/trajectory.py index c4693c5..5549e55 100644 --- a/src/iiwa_web/iiwa_web/trajectory.py +++ b/src/iiwa_web/iiwa_web/trajectory.py @@ -1,10 +1,11 @@ import csv import io +import math from collections import deque from datetime import datetime from builtin_interfaces.msg import Duration from fastapi import APIRouter, File, HTTPException, Query, UploadFile -from pydantic import BaseModel, Field +from pydantic import BaseModel, Field, field_validator, model_validator from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from .config_loader import load_joint_limits, load_joint_names @@ -41,6 +42,32 @@ def _validate_limits(points: list[list[float]]) -> None: ) +def _validate_rows(rows: list[tuple[list[float], float]]) -> None: + previous_time = -math.inf + for row_idx, (positions, point_time) in enumerate(rows): + if len(positions) != N_JOINTS: + raise HTTPException( + 422, + f"Точка {row_idx + 1}: ожидалось {N_JOINTS} суставов, получено {len(positions)}", + ) + for joint_idx, position in enumerate(positions): + if not math.isfinite(position): + raise HTTPException( + 422, + f"Точка {row_idx + 1}, сустав {joint_idx + 1}: позиция должна быть конечным числом", + ) + if not math.isfinite(point_time): + raise HTTPException(422, f"Точка {row_idx + 1}: time_from_start должен быть конечным числом") + if point_time < previous_time: + raise HTTPException( + 422, + "Время точек траектории должно быть монотонно неубывающим", + ) + previous_time = point_time + + _validate_limits([positions for positions, _time in rows]) + + def _build_msg(rows: list[tuple[list[float], float]]) -> JointTrajectory: msg = JointTrajectory() msg.joint_names = JOINT_NAMES @@ -63,11 +90,35 @@ class Waypoint(BaseModel): ) time_from_start: float = Field(..., ge=0.0, description="Время от начала траектории [с]") + @field_validator("positions") + @classmethod + def validate_positions(cls, value: list[float]) -> list[float]: + for joint_idx, position in enumerate(value): + if not math.isfinite(position): + raise ValueError(f"Позиция сустава {joint_idx + 1} должна быть конечным числом") + return value + + @field_validator("time_from_start") + @classmethod + def validate_time_from_start(cls, value: float) -> float: + if not math.isfinite(value): + raise ValueError("time_from_start должен быть конечным числом") + return value + class SendRequest(BaseModel): points: list[Waypoint] = Field(..., min_length=1, description="Точки траектории") validate_limits: bool = Field(True, description="Проверять лимиты суставов") + @model_validator(mode="after") + def validate_time_monotonicity(self) -> "SendRequest": + previous_time = -math.inf + for waypoint in self.points: + if waypoint.time_from_start < previous_time: + raise ValueError("Время точек траектории должно быть монотонно неубывающим") + previous_time = waypoint.time_from_start + return self + @router.post("/send", summary="Отправить траекторию вручную (JSON)") def send_trajectory(req: SendRequest): @@ -76,9 +127,7 @@ def send_trajectory(req: SendRequest): Публикует `JointTrajectory` в `/iiwa_arm_controller/joint_trajectory`. """ rows = [(wp.positions, wp.time_from_start) for wp in req.points] - - if req.validate_limits: - _validate_limits([r[0] for r in rows]) + _validate_rows(rows) _publish(_build_msg(rows)) _log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с") @@ -163,8 +212,7 @@ async def send_csv_trajectory( if not rows: raise HTTPException(422, "CSV не содержит точек траектории") - if validate_limits: - _validate_limits([r[0] for r in rows]) + _validate_rows(rows) _publish(_build_msg(rows)) _log(f"[csv] {file.filename} → {len(rows)} точек, t_end={rows[-1][1]:.2f}с") diff --git a/src/iiwa_web/package.xml b/src/iiwa_web/package.xml index 45dab2e..4cc0395 100644 --- a/src/iiwa_web/package.xml +++ b/src/iiwa_web/package.xml @@ -11,8 +11,18 @@ python3-uvicorn python3-multipart python3-fastmcp + python3-pydantic + python3-yaml + rclpy + builtin_interfaces + rosidl_runtime_py + rosbag2_py + iiwa_msgs + sensor_msgs + std_srvs tf2_ros tf2_py + trajectory_msgs moveit_msgs ament_copyright diff --git a/src/iiwa_web/setup.py b/src/iiwa_web/setup.py index 3db5655..a297997 100644 --- a/src/iiwa_web/setup.py +++ b/src/iiwa_web/setup.py @@ -14,11 +14,14 @@ setup( install_requires=[ 'setuptools', 'fastapi>=0.100.0', + 'pydantic>=2.0.0', + 'PyYAML', 'starlette>=0.27.0', 'uvicorn[standard]', 'python-multipart', 'fastmcp', ], + tests_require=['pytest'], zip_safe=True, maintainer='daniel', maintainer_email='grabardm@ml-dev.ru',