feat: enhance API with new trajectory and sequence handling, add TF support and improve endpoint definitions

This commit is contained in:
Даниил Грабарь
2026-05-26 20:36:10 +10:00
parent 30d7785d8c
commit d3912a5a12
8 changed files with 464 additions and 25 deletions
+15 -4
View File
@@ -12,12 +12,23 @@ endpoints:
timeout: 2.0
enabled: true
- path: /robot/pose
method: GET
type: tf
parent_frame: base_link
child_frame: tcp
summary: "Текущая декартова поза TCP"
description: "Позиция (м) и ориентация TCP относительно base_link через TF2. Углы Эйлера в конвенции KUKA ABC (ZYX): A=рыскание, B=тангаж, C=крен."
tags: [robot]
timeout: 1.0
enabled: true
- path: /robot/stop
method: POST
type: service
ros_name: cobot/stop
msg_type: std_srvs/srv/Trigger
summary: "[Service] Немедленно остановить движение"
summary: "Немедленно остановить движение"
description: "Вызывает сервис экстренной остановки — движение прерывается немедленно."
tags: [robot]
response_fields: [success, message]
@@ -29,7 +40,7 @@ endpoints:
type: service
ros_name: cobot/move_to_named
msg_type: iiwa_msgs/srv/MoveToNamedPose
summary: "[Action] Переместить в именованную позу из SRDF"
summary: "Переместить в именованную позу из SRDF"
description: "Перемещает робота в позу, определённую по имени в SRDF-файле (например, home, work)."
tags: [motion]
timeout: 30.0
@@ -58,7 +69,7 @@ endpoints:
type: action
ros_name: cobot/move_to_pose
msg_type: iiwa_msgs/action/MoveToPose
summary: "[Action] Переместить в декартову позу"
summary: "Переместить в декартову позу"
description: "Перемещает TCP робота в заданную декартову позицию и ориентацию."
tags: [motion]
timeout: 30.0
@@ -111,7 +122,7 @@ endpoints:
type: action
ros_name: cobot/move_to_joints
msg_type: iiwa_msgs/action/MoveToJoints
summary: "[Action] Переместить в позиции суставов"
summary: "Переместить в позиции суставов"
description: "Перемещает все суставы робота в заданные угловые позиции (радианы). Лимиты читаются из joint_limits.yaml."
tags: [motion]
timeout: 30.0
+35 -12
View File
@@ -25,9 +25,9 @@ class FieldDef:
class EndpointDef:
path: str
method: str # GET | POST
type: str # topic | service | action
ros_name: str
msg_type: str
type: str # topic | service | action | tf
ros_name: str = ""
msg_type: str = ""
summary: str = ""
description: str = ""
tags: list = field(default_factory=list)
@@ -37,6 +37,8 @@ class EndpointDef:
timeout: float = 5.0
deprecated: bool = False
enabled: bool = True
parent_frame: str = "" # tf: родительский фрейм
child_frame: str = "" # tf: дочерний фрейм
def _resolve_path(package: str, relative: str) -> Path:
@@ -49,6 +51,31 @@ def _resolve_path(package: str, relative: str) -> Path:
return src / package / relative
def _parse_joint_limits_data(data: dict) -> tuple[list[str], list[tuple[float, float]]]:
joints = data["joint_limits"]
names: list[str] = []
limits: list[tuple[float, float]] = []
i = 1
while f"joint{i}" in joints:
j = joints[f"joint{i}"]
names.append(f"joint{i}")
limits.append((j["min_position"], j["max_position"]))
i += 1
return names, limits
def load_joint_names(
package: str = "iiwa_config",
relative: str = "config/moveit/joint_limits.yaml",
) -> list[str]:
"""Возвращает упорядоченный список имён суставов из joint_limits.yaml."""
path = _resolve_path(package, relative)
with open(path) as f:
data = yaml.safe_load(f)
names, _ = _parse_joint_limits_data(data)
return names
def load_joint_limits(
package: str = "iiwa_config",
relative: str = "config/moveit/joint_limits.yaml",
@@ -57,13 +84,7 @@ def load_joint_limits(
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
_, limits = _parse_joint_limits_data(data)
return limits
@@ -102,8 +123,8 @@ def load_api_config(
path=ep["path"],
method=ep["method"].upper(),
type=ep["type"],
ros_name=ep["ros_name"],
msg_type=ep["msg_type"],
ros_name=ep.get("ros_name", ""),
msg_type=ep.get("msg_type", ""),
summary=ep.get("summary", ""),
description=ep.get("description", ""),
tags=ep.get("tags", []),
@@ -112,6 +133,8 @@ def load_api_config(
request_fields=request_fields,
timeout=ep.get("timeout", 5.0),
deprecated=ep.get("deprecated", False),
parent_frame=ep.get("parent_frame", ""),
child_frame=ep.get("child_frame", ""),
))
return endpoints
+52
View File
@@ -160,6 +160,54 @@ def _make_action_handler(ep: EndpointDef, Body: type, joint_limits: list[tuple[f
return handler
def _quat_to_euler_zyx(x: float, y: float, z: float, w: float) -> tuple[float, float, float]:
"""Quaternion → ZYX Euler (KUKA ABC: A=yaw, B=pitch, C=roll)."""
sinr = 2 * (w * x + y * z)
cosr = 1 - 2 * (x * x + y * y)
roll = math.atan2(sinr, cosr)
sinp = 2 * (w * y - z * x)
pitch = math.copysign(math.pi / 2, sinp) if abs(sinp) >= 1 else math.asin(sinp)
siny = 2 * (w * z + x * y)
cosy = 1 - 2 * (y * y + z * z)
yaw = math.atan2(siny, cosy)
return roll, pitch, yaw # C, B, A
def _make_tf_handler(ep: EndpointDef):
parent = ep.parent_frame
child = ep.child_frame
timeout = ep.timeout
def handler():
try:
tf = get_bridge().lookup_transform(parent, child, timeout)
except RuntimeError as e:
raise HTTPException(503, str(e))
t = tf.transform.translation
r = tf.transform.rotation
roll, pitch, yaw = _quat_to_euler_zyx(r.x, r.y, r.z, r.w)
return {
"position": {"x": t.x, "y": t.y, "z": t.z},
"orientation": {
"quaternion": {"x": r.x, "y": r.y, "z": r.z, "w": r.w},
"euler_rad": {"a": yaw, "b": pitch, "c": roll},
"euler_deg": {
"a": math.degrees(yaw),
"b": math.degrees(pitch),
"c": math.degrees(roll),
},
},
"frame": {"parent": parent, "child": child},
}
return handler
def build_dynamic_router() -> APIRouter:
"""Читает api_endpoints.yaml и joint_limits.yaml, возвращает готовый APIRouter."""
endpoints = load_api_config()
@@ -183,6 +231,10 @@ def build_dynamic_router() -> APIRouter:
if Body is None:
raise ValueError(f"Action-эндпоинт '{ep.path}' не имеет request_fields")
handler = _make_action_handler(ep, Body, joint_limits)
elif ep.type == "tf":
if not ep.parent_frame or not ep.child_frame:
raise ValueError(f"TF-эндпоинт '{ep.path}' требует parent_frame и child_frame")
handler = _make_tf_handler(ep)
else:
raise ValueError(f"Неизвестный тип эндпоинта: '{ep.type}'")
+7 -2
View File
@@ -1,22 +1,27 @@
import uvicorn
from contextlib import asynccontextmanager
from fastapi import FastAPI
from .ros_node import init_ros_node
from sensor_msgs.msg import JointState
from .ros_node import init_ros_node, get_bridge
from .dynamic_router import build_dynamic_router
from . import runner, trajectory
@asynccontextmanager
async def lifespan(_: FastAPI):
init_ros_node()
get_bridge().subscribe("/joint_states", JointState)
yield
app = FastAPI(lifespan=lifespan)
app.include_router(build_dynamic_router())
app.include_router(runner.router)
app.include_router(trajectory.router)
def main():
uvicorn.run(app, host="0.0.0.0", port=8007)
uvicorn.run(app, host="localhost", port=8007)
if __name__ == "__main__":
main()
+22 -7
View File
@@ -4,6 +4,7 @@ import threading
from rclpy.node import Node
from rclpy.action import ActionClient
import tf2_ros
class CobotWebNode(Node):
@@ -11,11 +12,14 @@ class CobotWebNode(Node):
super().__init__('cobot_web_node')
self._topic_cache: dict = {}
self._publishers: dict = {}
self._pub_registry: dict = {}
self._service_clients: dict = {}
self._action_clients: dict = {}
self._lock = threading.Lock()
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self)
def subscribe(self, topic_name: str, msg_type):
if topic_name not in self._topic_cache:
self._topic_cache[topic_name] = None
@@ -28,11 +32,11 @@ class CobotWebNode(Node):
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)
if topic_name not in self._pub_registry:
self._pub_registry[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)
self._pub_registry[topic_name].publish(msg)
def get_latest(self, topic_name: str):
with self._lock:
@@ -59,16 +63,27 @@ class CobotWebNode(Node):
return future.result()
def lookup_transform(self, parent_frame: str, child_frame: str, timeout: float = 1.0):
try:
return self._tf_buffer.lookup_transform(
parent_frame,
child_frame,
rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=timeout),
)
except Exception as e:
raise RuntimeError(f"TF lookup {parent_frame}{child_frame}: {e}")
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):
if not client.wait_for_server(timeout_sec=10.0):
raise RuntimeError(f"Action сервер '{action_name}' недоступен")
goal_future = client.send_goal_async(goal)
deadline = time.monotonic() + 5.0
deadline = time.monotonic() + 10.0
while not goal_future.done():
if time.monotonic() > deadline:
raise TimeoutError(f"Таймаут принятия goal '{action_name}'")
+130
View File
@@ -0,0 +1,130 @@
import os
import signal
import subprocess
import threading
import tempfile
from collections import deque
from pathlib import Path
from typing import Optional
import yaml
from fastapi import APIRouter, Form, HTTPException, Query, UploadFile, File
from std_srvs.srv import Trigger
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)
_LOG_BUFFER = 300
_process: Optional[subprocess.Popen] = None
_log_lines: deque[str] = deque(maxlen=_LOG_BUFFER)
_lock = threading.Lock()
def _stream_output(proc: subprocess.Popen) -> None:
for line in proc.stdout:
_log_lines.append(line.rstrip("\n"))
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]:
cmd = [
"ros2", "run", "iiwa_planning", "motion_sequence_runner",
"--ros-args",
"-p", f"config_path:={config_path}",
"-p", f"n_iterations:={n_iterations}",
"-p", f"delay_between_iterations:={delay}",
"-p", f"joints_action:={joints_action}",
"-p", f"pose_action:={pose_action}",
]
if bag_path:
cmd += ["-p", f"bag_path:={bag_path}"]
if topics:
topics_yaml = yaml.dump(topics, default_flow_style=True).strip()
cmd += ["-p", f"topics:={topics_yaml}"]
return cmd
@router.post("/start", summary="Загрузить конфиг и запустить motion_sequence_runner")
async def start_runner(
config: UploadFile = File(..., description="JSON-файл конфигурации последовательности"),
n_iterations: int = Form(3, ge=1, description="Число повторений"),
delay_between_iterations: float = Form(5.0, ge=0.0, description="Пауза между итерациями [с]"),
bag_path: str = Form("", description="Путь для записи rosbag (пусто = не записывать)"),
topics: str = Form("", description="Топики для bag через запятую (пусто = все)"),
joints_action: str = Form("cobot/move_to_joints", description="Action для суставного движения"),
pose_action: str = Form("cobot/move_to_pose", description="Action для декартова движения"),
):
global _process
with _lock:
if _process and _process.poll() is None:
raise HTTPException(409, f"Runner уже запущен (pid={_process.pid})")
filename = config.filename or "config.json"
dest = _UPLOAD_DIR / filename
dest.write_bytes(await config.read())
topics_list = [t.strip() for t in topics.split(",") if t.strip()]
_log_lines.clear()
cmd = _build_cmd(
config_path=str(dest),
n_iterations=n_iterations,
delay=delay_between_iterations,
bag_path=bag_path,
topics=topics_list,
joints_action=joints_action,
pose_action=pose_action,
)
_process = subprocess.Popen(
cmd,
stdout=subprocess.PIPE,
stderr=subprocess.STDOUT,
text=True,
bufsize=1,
start_new_session=True,
)
threading.Thread(target=_stream_output, args=(_process,), daemon=True).start()
return {"status": "started", "pid": _process.pid, "config": filename}
@router.post("/stop", summary="Остановить motion_sequence_runner и послать cobot/stop")
def stop_runner():
global _process
with _lock:
if not _process or _process.poll() is not None:
raise HTTPException(404, "Runner не запущен")
pgid = os.getpgid(_process.pid)
os.killpg(pgid, signal.SIGTERM)
try:
_process.wait(timeout=5.0)
except subprocess.TimeoutExpired:
os.killpg(pgid, signal.SIGKILL)
_process.wait()
code = _process.returncode
result = get_bridge().call_service(Trigger, "cobot/stop", Trigger.Request())
return {"status": "stopped", "returncode": code, "success": result.success, "message": result.message}
@router.get("/status", summary="Статус motion_sequence_runner")
def runner_status():
if not _process:
return {"status": "idle"}
code = _process.poll()
if code is None:
return {"status": "running", "pid": _process.pid}
return {"status": "finished", "returncode": code}
@router.get("/logs", summary="Последние строки вывода motion_sequence_runner")
def runner_logs(n: int = Query(50, ge=1, le=_LOG_BUFFER, description="Количество последних строк")):
lines = list(_log_lines)
return {"lines": lines[-n:], "total_buffered": len(lines)}
+201
View File
@@ -0,0 +1,201 @@
import csv
import io
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 std_srvs.srv import Trigger
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from .config_loader import load_joint_limits, load_joint_names
from .ros_node import get_bridge
router = APIRouter(prefix="/trajectory", tags=["trajectory"])
TOPIC = "/iiwa_arm_controller/joint_trajectory"
JOINT_NAMES = load_joint_names()
N_JOINTS = len(JOINT_NAMES)
_log_lines: deque[str] = deque(maxlen=300)
def _log(msg: str) -> None:
_log_lines.append(f"[{datetime.now().strftime('%H:%M:%S.%f')[:-3]}] {msg}")
def _to_duration(seconds: float) -> Duration:
sec = int(seconds)
nanosec = int(round((seconds - sec) * 1e9))
return Duration(sec=sec, nanosec=nanosec)
def _validate_limits(points: list[list[float]]) -> None:
limits = load_joint_limits()
for row_idx, positions in enumerate(points):
for j, (pos, (lo, hi)) in enumerate(zip(positions, limits)):
if not (lo <= pos <= hi):
raise HTTPException(
422,
f"Точка {row_idx + 1}, сустав {j + 1}: "
f"{pos:.4f} рад вне диапазона [{lo:.3f}, {hi:.3f}]",
)
def _build_msg(rows: list[tuple[list[float], float]]) -> JointTrajectory:
msg = JointTrajectory()
msg.joint_names = JOINT_NAMES
for positions, t in rows:
pt = JointTrajectoryPoint()
pt.positions = positions
pt.time_from_start = _to_duration(t)
msg.points.append(pt)
return msg
def _publish(msg: JointTrajectory) -> None:
get_bridge().publish(TOPIC, JointTrajectory, msg)
class Waypoint(BaseModel):
positions: list[float] = Field(
..., min_length=N_JOINTS, max_length=N_JOINTS,
description="Позиции суставов [j1..j7] в радианах",
)
time_from_start: float = Field(..., ge=0.0, description="Время от начала траектории [с]")
class SendRequest(BaseModel):
points: list[Waypoint] = Field(..., min_length=1, description="Точки траектории")
validate_limits: bool = Field(True, description="Проверять лимиты суставов")
@router.post("/send", summary="Отправить траекторию вручную (JSON)")
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])
_publish(_build_msg(rows))
_log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
return {"status": "sent", "points": len(rows)}
@router.post("/send_csv", summary="Загрузить CSV и отправить траекторию")
async def send_csv_trajectory(
file: UploadFile = File(
...,
description="CSV с заголовком. Колонки суставов: joint_1..joint_7 (или joint1..joint7). Колонка времени: t.",
),
separator: str = Query(",", description="Разделитель колонок (например: ',' ';' '\\t')"),
validate_limits: bool = Query(True, description="Проверять лимиты суставов"),
):
"""
Ожидаемый формат (первая строка — обязательный заголовок):
joint1,joint2,joint3,joint4,joint5,joint6,joint7,t
-2.55,-0.71,-0.77,0.028,0.0,-2.09,-0.10,0.0
-2.54,-0.71,-0.77,0.029,0.0,-2.09,-0.10,0.01
Порядок и имена колонок произвольны — сопоставление идёт по заголовку.
Имена суставов нормализуются: `joint_1` = `joint1` = `JOINT1`.
Колонка времени определяется по заголовку `t`, `time` или `time_from_start`.
"""
sep = separator.replace("\\t", "\t")
content = (await file.read()).decode("utf-8")
reader = csv.reader(io.StringIO(content), delimiter=sep)
try:
raw_headers = next(reader)
except StopIteration:
raise HTTPException(422, "Файл пуст")
headers = [h.strip() for h in raw_headers]
def _norm(s: str) -> str:
return s.lower().replace("_", "").replace(" ", "")
TIME_ALIASES = {"t", "time", "timefromstart"}
norm_joint_to_idx = {_norm(j): i for i, j in enumerate(JOINT_NAMES)}
col_joint: dict[int, int] = {} # col_index -> joint_index
col_time: int | None = None
for col_idx, h in enumerate(headers):
n = _norm(h)
if n in TIME_ALIASES:
col_time = col_idx
elif n in norm_joint_to_idx:
col_joint[col_idx] = norm_joint_to_idx[n]
if col_time is None:
raise HTTPException(422, f"Колонка времени не найдена. Ожидалось одно из: t, time, time_from_start. Заголовки: {headers}")
missing = sorted(set(range(N_JOINTS)) - set(col_joint.values()))
if missing:
raise HTTPException(422, f"Не найдены колонки для суставов: {[JOINT_NAMES[i] for i in missing]}")
joint_to_col = {j_idx: c_idx for c_idx, j_idx in col_joint.items()}
rows: list[tuple[list[float], float]] = []
for line_no, row in enumerate(reader, start=2):
row = [c.strip() for c in row]
if not any(row):
continue
if len(row) != len(headers):
raise HTTPException(
422,
f"Строка {line_no}: ожидалось {len(headers)} столбцов, получено {len(row)}",
)
try:
positions = [float(row[joint_to_col[i]]) for i in range(N_JOINTS)]
t = float(row[col_time])
except ValueError as e:
raise HTTPException(422, f"Строка {line_no}: не удалось распарсить число — {e}")
if t < 0:
raise HTTPException(422, f"Строка {line_no}: t не может быть отрицательным")
rows.append((positions, t))
if not rows:
raise HTTPException(422, "CSV не содержит точек траектории")
if validate_limits:
_validate_limits([r[0] for r in rows])
_publish(_build_msg(rows))
_log(f"[csv] {file.filename}{len(rows)} точек, t_end={rows[-1][1]:.2f}с")
return {"status": "sent", "points": len(rows), "filename": file.filename}
@router.post("/stop", summary="Остановить выполнение траектории")
def stop_trajectory():
bridge = get_bridge()
# Replace ongoing trajectory with single point at current position
joint_states = bridge.get_latest("/joint_states")
if joint_states is not None and len(joint_states.position) >= N_JOINTS:
current_positions = list(joint_states.position[:N_JOINTS])
hold_msg = _build_msg([(current_positions, 0.5)])
_publish(hold_msg)
_log("[stop] отправлена точка удержания текущей позиции")
else:
msg = JointTrajectory()
msg.joint_names = JOINT_NAMES
_publish(msg)
_log("[stop] joint_states недоступны, отправлена пустая траектория")
# cobot/stop cancels MoveIt action-based motion
result = bridge.call_service(Trigger, "cobot/stop", Trigger.Request())
_log(f"[stop] cobot/stop -> success={result.success}, message={result.message}")
return {"status": "stopped", "success": result.success, "message": result.message}
@router.get("/logs", summary="Последние лог-записи траекторного модуля")
def trajectory_logs(n: int = Query(50, ge=1, le=300, description="Количество последних строк")):
lines = list(_log_lines)
return {"lines": lines[-n:], "total_buffered": len(lines)}
+2
View File
@@ -9,6 +9,8 @@
<exec_depend>python3-fastapi</exec_depend>
<exec_depend>python3-uvicorn</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>tf2_py</exec_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>