feat: implement ROS node and API for robot control with FastAPI
This commit is contained in:
Даниил Грабарь
@@ -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
|
||||
@@ -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()
|
||||
@@ -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}
|
||||
@@ -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
|
||||
@@ -25,6 +25,7 @@ setup(
|
||||
},
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'iiwa_web_server = iiwa_web.main:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user