diff --git a/src/iiwa_config/config/api_endpoints.yaml b/src/iiwa_config/config/api_endpoints.yaml new file mode 100644 index 0000000..228871a --- /dev/null +++ b/src/iiwa_config/config/api_endpoints.yaml @@ -0,0 +1,132 @@ +endpoints: + + - path: /robot/joint_states + method: GET + type: topic + ros_name: /joint_states + msg_type: sensor_msgs/msg/JointState + summary: "Текущее состояние суставов" + description: "Возвращает имена, позиции, скорости и усилия всех суставов." + tags: [robot] + fields: [name, position, velocity, effort] + timeout: 2.0 + enabled: true + + - path: /robot/stop + method: POST + type: service + ros_name: cobot/stop + msg_type: std_srvs/srv/Trigger + summary: "[Service] Немедленно остановить движение" + description: "Вызывает сервис экстренной остановки — движение прерывается немедленно." + tags: [robot] + response_fields: [success, message] + timeout: 5.0 + enabled: true + + - path: /robot/move/named + method: POST + type: service + ros_name: cobot/move_to_named + msg_type: iiwa_msgs/srv/MoveToNamedPose + summary: "[Action] Переместить в именованную позу из SRDF" + description: "Перемещает робота в позу, определённую по имени в SRDF-файле (например, home, work)." + tags: [motion] + timeout: 30.0 + request_fields: + - name: name + type: string + required: true + description: "Имя позиции из SRDF" + - name: speed + type: float + default: 0.1 + min: 0.01 + max: 1.0 + description: "Скорость [0.01–1.0]" + - name: accel_scale + type: float + default: 0.0 + min: 0.0 + max: 1.0 + description: "Масштаб ускорения (0 = равно speed)" + response_fields: [success, message] + enabled: true + + - path: /robot/move/pose + method: POST + type: action + ros_name: cobot/move_to_pose + msg_type: iiwa_msgs/action/MoveToPose + summary: "[Action] Переместить в декартову позу" + description: "Перемещает TCP робота в заданную декартову позицию и ориентацию." + tags: [motion] + timeout: 30.0 + request_fields: + - name: x + type: float + required: true + description: "Позиция X в метрах" + - name: y + type: float + required: true + description: "Позиция Y в метрах" + - name: z + type: float + required: true + description: "Позиция Z в метрах" + - name: a + type: float + default: 0.0 + description: "Угол A (ZYX Эйлер, KUKA ABC) в радианах" + - name: b + type: float + default: 0.0 + description: "Угол B в радианах" + - name: c + type: float + default: 0.0 + description: "Угол C в радианах" + - name: speed + type: float + default: 0.1 + min: 0.01 + max: 1.0 + description: "Скорость [0.01–1.0]" + - name: planner + type: string + default: "ptp" + choices: [ompl, ptp, lin, circ, chomp] + normalize: lower + description: "Планировщик движения: ompl, ptp, lin, circ, chomp" + - name: frame_id + type: string + default: "" + description: "Целевой фрейм (пусто = default_frame)" + response_fields: [success, message] + enabled: true + + - path: /robot/move/joints + method: POST + type: action + ros_name: cobot/move_to_joints + msg_type: iiwa_msgs/action/MoveToJoints + summary: "[Action] Переместить в позиции суставов" + description: "Перемещает все суставы робота в заданные угловые позиции (радианы). Лимиты читаются из joint_limits.yaml." + tags: [motion] + timeout: 30.0 + request_fields: + - name: joints + type: float_array + required: true + length: 7 + joint_limits: true + description: "Позиции суставов в радианах [j1..j7]" + - name: speed + type: float + default: 0.1 + min: 0.01 + max: 1.0 + description: "Скорость [0.01–1.0]" + response_fields: [success, message] + enabled: true diff --git a/src/iiwa_web/iiwa_web/config_loader.py b/src/iiwa_web/iiwa_web/config_loader.py new file mode 100644 index 0000000..c044ae3 --- /dev/null +++ b/src/iiwa_web/iiwa_web/config_loader.py @@ -0,0 +1,117 @@ +from __future__ import annotations + +import yaml +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any, Optional + + +@dataclass +class FieldDef: + name: str + type: str # string | float | int | bool | float_array + required: bool = False + default: Any = None + description: str = "" + min: Optional[float] = None + max: Optional[float] = None + length: Optional[int] = None + choices: Optional[list] = None + normalize: Optional[str] = None # "lower" | "upper" + joint_limits: bool = False + + +@dataclass +class EndpointDef: + path: str + method: str # GET | POST + type: str # topic | service | action + ros_name: str + msg_type: str + summary: str = "" + description: str = "" + tags: list = field(default_factory=list) + fields: list = field(default_factory=list) # topic: поля ответа + response_fields: list = field(default_factory=list) # service/action: поля ответа + request_fields: list[FieldDef] = field(default_factory=list) + timeout: float = 5.0 + deprecated: bool = False + enabled: bool = True + + +def _resolve_path(package: str, relative: str) -> Path: + """Ищет файл сначала через ament_index, затем по пути относительно src/.""" + try: + from ament_index_python.packages import get_package_share_directory + return Path(get_package_share_directory(package)) / relative + except Exception: + src = Path(__file__).parents[3] # .../src/iiwa_web/iiwa_web/ -> .../src/ + return src / package / relative + + +def load_joint_limits( + package: str = "iiwa_config", + relative: str = "config/moveit/joint_limits.yaml", +) -> list[tuple[float, float]]: + """Возвращает список (min, max) для каждого сустава по порядку joint1..jointN.""" + path = _resolve_path(package, relative) + with open(path) as f: + data = yaml.safe_load(f) + joints = data["joint_limits"] + limits: list[tuple[float, float]] = [] + i = 1 + while f"joint{i}" in joints: + j = joints[f"joint{i}"] + limits.append((j["min_position"], j["max_position"])) + i += 1 + return limits + + +def load_api_config( + package: str = "iiwa_config", + relative: str = "config/api_endpoints.yaml", +) -> list[EndpointDef]: + """Загружает описания эндпоинтов из YAML и возвращает список EndpointDef.""" + path = _resolve_path(package, relative) + with open(path) as f: + data = yaml.safe_load(f) + + endpoints: list[EndpointDef] = [] + for ep in data.get("endpoints", []): + if not ep.get("enabled", True): + continue + + request_fields = [ + FieldDef( + name=rf["name"], + type=rf["type"], + required=rf.get("required", False), + default=rf.get("default"), + description=rf.get("description", ""), + min=rf.get("min"), + max=rf.get("max"), + length=rf.get("length"), + choices=rf.get("choices"), + normalize=rf.get("normalize"), + joint_limits=rf.get("joint_limits", False), + ) + for rf in ep.get("request_fields", []) + ] + + endpoints.append(EndpointDef( + path=ep["path"], + method=ep["method"].upper(), + type=ep["type"], + ros_name=ep["ros_name"], + msg_type=ep["msg_type"], + summary=ep.get("summary", ""), + description=ep.get("description", ""), + tags=ep.get("tags", []), + fields=ep.get("fields", []), + response_fields=ep.get("response_fields", []), + request_fields=request_fields, + timeout=ep.get("timeout", 5.0), + deprecated=ep.get("deprecated", False), + )) + + return endpoints diff --git a/src/iiwa_web/iiwa_web/deps.py b/src/iiwa_web/iiwa_web/deps.py deleted file mode 100644 index 879ed2b..0000000 --- a/src/iiwa_web/iiwa_web/deps.py +++ /dev/null @@ -1,26 +0,0 @@ -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/dynamic_router.py b/src/iiwa_web/iiwa_web/dynamic_router.py new file mode 100644 index 0000000..4743826 --- /dev/null +++ b/src/iiwa_web/iiwa_web/dynamic_router.py @@ -0,0 +1,199 @@ +import importlib +import math +import time +from typing import Any + +from fastapi import APIRouter, HTTPException +from fastapi.responses import JSONResponse +from pydantic import Field, create_model + +from .config_loader import EndpointDef, FieldDef, load_api_config, load_joint_limits +from .ros_node import get_bridge + +_POLL_INTERVAL = 0.05 + +_TYPE_MAP: dict[str, type] = { + "string": str, + "float": float, + "int": int, + "bool": bool, + "float_array": list[float], +} + + +def _import_ros_type(type_str: str): + """'sensor_msgs/msg/JointState' → класс JointState.""" + parts = type_str.split("/") + module = importlib.import_module(".".join(parts[:-1])) + return getattr(module, parts[-1]) + + +def _to_python(val: Any) -> Any: + """Конвертирует ROS-значение в JSON-сериализуемый Python-тип.""" + if isinstance(val, float): + return None if math.isnan(val) else val + if hasattr(val, "__iter__") and not isinstance(val, (str, bytes)): + return [None if (isinstance(v, float) and math.isnan(v)) else v for v in val] + return val + + +def _extract(msg, fields: list[str]) -> dict: + return {f: _to_python(getattr(msg, f)) for f in fields} + + +def _build_model(name: str, field_defs: list[FieldDef]) -> type: + """Динамически создаёт Pydantic-модель из списка FieldDef.""" + definitions: dict[str, tuple] = {} + for fd in field_defs: + py_type = _TYPE_MAP[fd.type] + kwargs: dict[str, Any] = {"description": fd.description} + if fd.min is not None: + kwargs["ge"] = fd.min + if fd.max is not None: + kwargs["le"] = fd.max + if fd.length is not None: + kwargs["min_length"] = fd.length + kwargs["max_length"] = fd.length + kwargs["default"] = ... if fd.required else fd.default + definitions[fd.name] = (py_type, Field(**kwargs)) + return create_model(name, **definitions) + + +def _make_topic_handler(ep: EndpointDef): + ros_type = _import_ros_type(ep.msg_type) + ros_name = ep.ros_name + fields = ep.fields + timeout = ep.timeout + + def handler(): + bridge = get_bridge() + bridge.subscribe(ros_name, ros_type) + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + msg = bridge.get_latest(ros_name) + if msg is not None: + return JSONResponse(_extract(msg, fields)) + time.sleep(_POLL_INTERVAL) + raise HTTPException(503, f"Timeout waiting for topic '{ros_name}'") + + return handler + + +def _make_service_handler(ep: EndpointDef, Body: type | None): + ros_type = _import_ros_type(ep.msg_type) + ros_name = ep.ros_name + response_fields = ep.response_fields + timeout = ep.timeout + field_defs = ep.request_fields + + if Body is None: + def handler(): + try: + resp = get_bridge().call_service(ros_type, ros_name, ros_type.Request(), timeout) + except (RuntimeError, TimeoutError) as e: + raise HTTPException(503, str(e)) + return _extract(resp, response_fields) if response_fields else {"success": resp.success} + else: + def handler(body: Body): # type: ignore[valid-type] + req = ros_type.Request() + for fd in field_defs: + setattr(req, fd.name, getattr(body, fd.name)) + try: + resp = get_bridge().call_service(ros_type, ros_name, req, timeout) + except (RuntimeError, TimeoutError) as e: + raise HTTPException(503, str(e)) + return _extract(resp, response_fields) if response_fields else {"success": resp.success} + + return handler + + +def _make_action_handler(ep: EndpointDef, Body: type, joint_limits: list[tuple[float, float]]): + ros_type = _import_ros_type(ep.msg_type) + ros_name = ep.ros_name + response_fields = ep.response_fields + timeout = ep.timeout + field_defs = ep.request_fields + + choice_fields = [(fd.name, fd.choices, fd.normalize) for fd in field_defs if fd.choices] + jl_fields = [fd.name for fd in field_defs if fd.joint_limits] + + def handler(body: Body): # type: ignore[valid-type] + values: dict[str, Any] = {fd.name: getattr(body, fd.name) for fd in field_defs} + + # Нормализация и валидация choices + for fname, choices, normalize in choice_fields: + val = values[fname] + if normalize == "lower" and isinstance(val, str): + val = val.lower() + elif normalize == "upper" and isinstance(val, str): + val = val.upper() + if val not in choices: + raise HTTPException(422, f"Поле '{fname}' должно быть одним из {choices}, получено '{val}'") + values[fname] = val + + # Валидация лимитов суставов из joint_limits.yaml + for fname in jl_fields: + joints = values[fname] + if len(joints) != len(joint_limits): + raise HTTPException( + 422, + f"Ожидалось {len(joint_limits)} суставов, получено {len(joints)}", + ) + for i, (pos, (lo, hi)) in enumerate(zip(joints, joint_limits)): + if not (lo <= pos <= hi): + raise HTTPException( + 422, + f"Сустав {i + 1}: {pos:.4f} рад вне диапазона [{lo:.3f}, {hi:.3f}]", + ) + + goal = ros_type.Goal() + for fd in field_defs: + setattr(goal, fd.name, values[fd.name]) + + try: + result = get_bridge().send_action(ros_type, ros_name, goal, timeout) + except (RuntimeError, TimeoutError) as e: + raise HTTPException(503, str(e)) + + return _extract(result.result, response_fields) if response_fields else {} + + return handler + + +def build_dynamic_router() -> APIRouter: + """Читает api_endpoints.yaml и joint_limits.yaml, возвращает готовый APIRouter.""" + endpoints = load_api_config() + joint_limits = load_joint_limits() + + router = APIRouter() + + for ep in endpoints: + # Имя модели — CamelCase из пути (/robot/move/joints → RobotMoveJoints) + model_name = "".join(p.title() for p in ep.path.strip("/").split("/")) + + Body: type | None = None + if ep.request_fields: + Body = _build_model(f"{model_name}Request", ep.request_fields) + + if ep.type == "topic": + handler = _make_topic_handler(ep) + elif ep.type == "service": + handler = _make_service_handler(ep, Body) + elif ep.type == "action": + if Body is None: + raise ValueError(f"Action-эндпоинт '{ep.path}' не имеет request_fields") + handler = _make_action_handler(ep, Body, joint_limits) + else: + raise ValueError(f"Неизвестный тип эндпоинта: '{ep.type}'") + + router.add_api_route( + ep.path, + handler, + methods=[ep.method], + summary=ep.summary or ep.description, + description=ep.description, + tags=ep.tags, + deprecated=ep.deprecated, + ) + + return router diff --git a/src/iiwa_web/iiwa_web/main.py b/src/iiwa_web/iiwa_web/main.py index e9fee45..b1870e3 100644 --- a/src/iiwa_web/iiwa_web/main.py +++ b/src/iiwa_web/iiwa_web/main.py @@ -2,7 +2,7 @@ import uvicorn from contextlib import asynccontextmanager from fastapi import FastAPI from .ros_node import init_ros_node -from . import robot +from .dynamic_router import build_dynamic_router @asynccontextmanager @@ -12,7 +12,7 @@ async def lifespan(_: FastAPI): app = FastAPI(lifespan=lifespan) -app.include_router(robot.router) +app.include_router(build_dynamic_router()) def main(): diff --git a/src/iiwa_web/iiwa_web/robot.py b/src/iiwa_web/iiwa_web/robot.py deleted file mode 100644 index e3f4395..0000000 --- a/src/iiwa_web/iiwa_web/robot.py +++ /dev/null @@ -1,145 +0,0 @@ -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}