feat: enhance API with new trajectory and sequence handling, add TF support and improve endpoint definitions
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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}'")
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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}'")
|
||||
|
||||
@@ -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)}
|
||||
@@ -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)}
|
||||
@@ -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>
|
||||
|
||||
Reference in New Issue
Block a user