feat: implement ROS node and API for robot control with FastAPI

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

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()

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}

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

src/iiwa_web/setup.py

@@ -25,6 +25,7 @@ setup(
     },
     entry_points={
         'console_scripts': [
+            'iiwa_web_server = iiwa_web.main:main',
         ],
     },
 )