From a84cfde96c015d38b881d6bfb2cc9e8b06d98f2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Tue, 26 May 2026 08:39:45 +0300 Subject: [PATCH] feat: implement ROS node and API for robot control with FastAPI --- src/iiwa_web/iiwa_web/deps.py | 26 ++++++ src/iiwa_web/iiwa_web/main.py | 22 +++++ src/iiwa_web/iiwa_web/robot.py | 145 ++++++++++++++++++++++++++++++ src/iiwa_web/iiwa_web/ros_node.py | 107 ++++++++++++++++++++++ src/iiwa_web/setup.py | 1 + 5 files changed, 301 insertions(+) create mode 100644 src/iiwa_web/iiwa_web/deps.py create mode 100644 src/iiwa_web/iiwa_web/main.py create mode 100644 src/iiwa_web/iiwa_web/robot.py create mode 100644 src/iiwa_web/iiwa_web/ros_node.py diff --git a/src/iiwa_web/iiwa_web/deps.py b/src/iiwa_web/iiwa_web/deps.py new file mode 100644 index 0000000..879ed2b --- /dev/null +++ b/src/iiwa_web/iiwa_web/deps.py @@ -0,0 +1,26 @@ +import time +from fastapi import HTTPException +from .ros_node import get_bridge + +_TIMEOUT = 2.0 +_POLL_INTERVAL = 0.05 + +def ros_topic(topic_name: str, msg_type): + + def dependency(): + bridge = get_bridge() + bridge.subscribe(topic_name, msg_type) + + deadline = time.monotonic() + _TIMEOUT + while time.monotonic() < deadline: + msg = bridge.get_latest(topic_name) + if msg is not None: + return msg + time.sleep(_POLL_INTERVAL) + + raise HTTPException( + status_code=503, + detail=f"Timeout waiting for message on topic '{topic_name}'.", + ) + + return dependency \ No newline at end of file diff --git a/src/iiwa_web/iiwa_web/main.py b/src/iiwa_web/iiwa_web/main.py new file mode 100644 index 0000000..e9fee45 --- /dev/null +++ b/src/iiwa_web/iiwa_web/main.py @@ -0,0 +1,22 @@ +import uvicorn +from contextlib import asynccontextmanager +from fastapi import FastAPI +from .ros_node import init_ros_node +from . import robot + + +@asynccontextmanager +async def lifespan(_: FastAPI): + init_ros_node() + yield + + +app = FastAPI(lifespan=lifespan) +app.include_router(robot.router) + + +def main(): + uvicorn.run(app, host="0.0.0.0", port=8007) + +if __name__ == "__main__": + main() diff --git a/src/iiwa_web/iiwa_web/robot.py b/src/iiwa_web/iiwa_web/robot.py new file mode 100644 index 0000000..e3f4395 --- /dev/null +++ b/src/iiwa_web/iiwa_web/robot.py @@ -0,0 +1,145 @@ +import math + +from fastapi import APIRouter, Depends, HTTPException +from fastapi.responses import JSONResponse +from pydantic import BaseModel, Field, field_validator +from sensor_msgs.msg import JointState +from std_srvs.srv import Trigger +from iiwa_msgs.action import MoveToPose, MoveToJoints +from iiwa_msgs.srv import MoveToNamedPose + +from .deps import ros_topic +from .ros_node import get_bridge + +router = APIRouter(prefix="/robot", tags=["robot"]) + +JOINT_LIMITS = [ + (-2.967, 2.967), + (-2.094, 2.094), + (-2.967, 2.967), + (-2.094, 2.094), + (-2.967, 2.967), + (-2.094, 2.094), + (-3.054, 3.054), +] + +PLANNERS = ["ompl", "ptp", "lin", "circ", "chomp"] + + +def _clean(values): + return [None if math.isnan(v) else v for v in values] + + +def _action_error(e: Exception): + return HTTPException(status_code=503, detail=str(e)) + + + +class MoveToPoseRequest(BaseModel): + x: float = Field(..., description="Позиция X в метрах") + y: float = Field(..., description="Позиция Y в метрах") + z: float = Field(..., description="Позиция Z в метрах") + a: float = Field(0.0, description="Угол A (ZYX Эйлер, KUKA ABC) в радианах") + b: float = Field(0.0, description="Угол B в радианах") + c: float = Field(0.0, description="Угол C в радианах") + speed: float = Field(0.1, ge=0.01, le=1.0, description="Скорость [0.01 – 1.0]") + planner: str = Field("ompl", description=f"Планировщик: {', '.join(PLANNERS)}") + frame_id: str = Field("", description="Целевой фрейм (пусто = default_frame)") + + @field_validator("planner") + @classmethod + def check_planner(cls, v: str) -> str: + if v.lower() not in PLANNERS: + raise ValueError(f"Неизвестный планировщик '{v}'. Доступные: {', '.join(PLANNERS)}") + return v.lower() + + +class MoveToJointsRequest(BaseModel): + joints: list[float] = Field( + ..., min_length=7, max_length=7, + description="Позиции суставов в радианах [j1..j7]", + ) + speed: float = Field(0.1, ge=0.01, le=1.0, description="Скорость [0.01 – 1.0]") + + @field_validator("joints") + @classmethod + def check_limits(cls, joints: list[float]) -> list[float]: + for i, (pos, (lo, hi)) in enumerate(zip(joints, JOINT_LIMITS)): + if not (lo <= pos <= hi): + raise ValueError( + f"Сустав {i + 1}: {pos:.4f} рад вне диапазона [{lo:.3f}, {hi:.3f}]" + ) + return joints + + +class MoveToNamedRequest(BaseModel): + name: str = Field(..., description="Имя позиции из SRDF") + speed: float = Field(0.1, ge=0.01, le=1.0, description="Скорость [0.01 – 1.0]") + accel_scale: float = Field(0.0, ge=0.0, le=1.0, description="Ускорение (0 = равно speed)") + + + +@router.get("/joint_states", summary="Текущее состояние суставов") +def get_joint_states(msg: JointState = Depends(ros_topic("/joint_states", JointState))): + return JSONResponse({ + "name": list(msg.name), + "position": _clean(msg.position), + "velocity": _clean(msg.velocity), + "effort": _clean(msg.effort), + }) + + +@router.post("/move/pose", summary="[Action] Переместить в декартову позу") +def move_to_pose(req: MoveToPoseRequest): + goal = MoveToPose.Goal() + goal.x, goal.y, goal.z = req.x, req.y, req.z + goal.a, goal.b, goal.c = req.a, req.b, req.c + goal.speed = req.speed + goal.planner = req.planner + goal.frame_id = req.frame_id + + try: + result = get_bridge().send_action(MoveToPose, "cobot/move_to_pose", goal) + except (RuntimeError, TimeoutError) as e: + raise _action_error(e) + + return {"success": result.result.success, "message": result.result.message} + + +@router.post("/move/joints", summary="[Action] Переместить в позиции суставов") +def move_to_joints(req: MoveToJointsRequest): + goal = MoveToJoints.Goal() + goal.joints = req.joints + goal.speed = req.speed + + try: + result = get_bridge().send_action(MoveToJoints, "cobot/move_to_joints", goal) + except (RuntimeError, TimeoutError) as e: + raise _action_error(e) + + return {"success": result.result.success, "message": result.result.message} + + +@router.post("/move/named", summary="[Service] Переместить в именованную позу из SRDF") +def move_to_named(req: MoveToNamedRequest): + request = MoveToNamedPose.Request() + request.name = req.name + request.speed = req.speed + request.accel_scale = req.accel_scale + + try: + response = get_bridge().call_service(MoveToNamedPose, "cobot/move_to_named", request) + except (RuntimeError, TimeoutError) as e: + raise HTTPException(status_code=503, detail=str(e)) + + return {"success": response.success, "message": response.message} + + +@router.post("/stop", summary="[Service] Немедленно остановить движение") +def stop(): + try: + response = get_bridge().call_service(Trigger, "cobot/stop", Trigger.Request()) + except (RuntimeError, TimeoutError) as e: + raise HTTPException(status_code=503, detail=str(e)) + + return {"success": response.success, "message": response.message} diff --git a/src/iiwa_web/iiwa_web/ros_node.py b/src/iiwa_web/iiwa_web/ros_node.py new file mode 100644 index 0000000..9c11275 --- /dev/null +++ b/src/iiwa_web/iiwa_web/ros_node.py @@ -0,0 +1,107 @@ +import time +import rclpy +import threading + +from rclpy.node import Node +from rclpy.action import ActionClient + + +class CobotWebNode(Node): + def __init__(self): + super().__init__('cobot_web_node') + + self._topic_cache: dict = {} + self._publishers: dict = {} + self._service_clients: dict = {} + self._action_clients: dict = {} + self._lock = threading.Lock() + + def subscribe(self, topic_name: str, msg_type): + if topic_name not in self._topic_cache: + self._topic_cache[topic_name] = None + self.create_subscription( + msg_type, + topic_name, + lambda msg, t=topic_name: self._handle_message(t, msg), + 10 + ) + self.get_logger().info(f'Subscribed to topic: {topic_name}') + + def publish(self, topic_name: str, message_type, msg): + if topic_name not in self._publishers: + self._publishers[topic_name] = self.create_publisher(message_type, topic_name, 10) + self.get_logger().info(f'Created publisher for topic: {topic_name}') + + self._publishers[topic_name].publish(msg) + + def get_latest(self, topic_name: str): + with self._lock: + return self._topic_cache.get(topic_name) + + def _handle_message(self, topic_name: str, msg): + with self._lock: + self._topic_cache[topic_name] = msg + + def call_service(self, srv_type, srv_name: str, request, timeout: float = 5.0): + if srv_name not in self._service_clients: + self._service_clients[srv_name] = self.create_client(srv_type, srv_name) + + client = self._service_clients[srv_name] + if not client.wait_for_service(timeout_sec=timeout): + raise RuntimeError(f"Сервис '{srv_name}' недоступен") + + future = client.call_async(request) + deadline = time.monotonic() + timeout + while not future.done(): + if time.monotonic() > deadline: + raise TimeoutError(f"Таймаут вызова сервиса '{srv_name}'") + time.sleep(0.01) + + return future.result() + + def send_action(self, action_type, action_name: str, goal, timeout: float = 30.0): + if action_name not in self._action_clients: + self._action_clients[action_name] = ActionClient(self, action_type, action_name) + + client = self._action_clients[action_name] + if not client.wait_for_server(timeout_sec=5.0): + raise RuntimeError(f"Action сервер '{action_name}' недоступен") + + goal_future = client.send_goal_async(goal) + deadline = time.monotonic() + 5.0 + while not goal_future.done(): + if time.monotonic() > deadline: + raise TimeoutError(f"Таймаут принятия goal '{action_name}'") + time.sleep(0.01) + + goal_handle = goal_future.result() + if not goal_handle.accepted: + raise RuntimeError(f"Goal отклонён сервером '{action_name}'") + + result_future = goal_handle.get_result_async() + deadline = time.monotonic() + timeout + while not result_future.done(): + if time.monotonic() > deadline: + raise TimeoutError(f"Таймаут выполнения action '{action_name}'") + time.sleep(0.05) + + return result_future.result() + + + +_bridge: CobotWebNode = None + +def init_ros_node() -> None: + global _bridge + rclpy.init() + _bridge = CobotWebNode() + + thread = threading.Thread(target=rclpy.spin, args=(_bridge,), daemon=True) + thread.start() + + +def get_bridge() -> CobotWebNode: + global _bridge + if _bridge is None: + raise RuntimeError("ROS node not initialized. Call init_ros_node() first.") + return _bridge diff --git a/src/iiwa_web/setup.py b/src/iiwa_web/setup.py index 5a3b24d..1899902 100644 --- a/src/iiwa_web/setup.py +++ b/src/iiwa_web/setup.py @@ -25,6 +25,7 @@ setup( }, entry_points={ 'console_scripts': [ + 'iiwa_web_server = iiwa_web.main:main', ], }, )