机械臂控制接口说明
机械臂控制接口说明
/fd_arm/* 控制类接口 · 逐接口说明 + 独立示例
软件版本 FDRS-V04-02-00 · 核对日期 2026-09-17一、概述
本文只讲机械臂的控制类接口——即会改变机器人状态(上电、设速、末端动作、点位增删、运动执行)的接口,共 8 个:5 个服务 + 3 个动作。每个接口都配有一段可直接复制的示例(命令行 / Python / C++)。
读取类的接口(查状态、列点位、订阅运动状态)不在本文展开,只在第三章列出来划清边界,详见 《机械臂 ROS 接口说明》。
/fd_arm/ 下,由 fd_arm_server 提供。单臂与双臂共用同一套接口,用请求参数 arm_name(armleft / armright)指定侧别,与机型无关。1.1 控制接口与查询接口的边界
| 类别 | 数量 | 接口 |
|---|---|---|
| 控制接口 本文范围 |
8 | /fd_arm/power · set_speed · set_manipulator · location_save · location_delete(服务) /fd_arm/move_to_location · follow_trajectory · move_to_target(动作) |
| 查询接口 仅列出 |
2 | /fd_arm/get_state(读关节角 / 位姿 / 上电 / 运动状态) /fd_arm/location_list(列出已保存点位) |
| 状态话题 仅列出 |
1 | /fd_arm/motion_status(订阅:busy / current_action / last_result_code) |
1.2 架构位置
ros2 CLI · rclpy · rclcpp
fd_arm_server · 服务与动作 · arm_name 指定侧别
/tl_control/* · /tl_driver/* · /teleop/*
关节 · 夹爪 / 灵巧手 · 六维力
1.3 核心特性
上电、设速、末端、点位增删
运动类接口全部是 Action
result_code 判定成败,不靠延时
动作反馈含 progress 与路点数
存点 → 列点 → 运动 → 删点
left_ / right_tl_robot_jointN
二、运行前提与约定
2.1 运行前提
| 前提 | 说明 |
|---|---|
| 机械臂应用层已启动 | fd_arm_server 运行中,否则 /fd_arm/* 全部不可用 |
| 底层驱动已启动 | tl_driver、tl_control 运行中(由系统统一拉起) |
| 环境已 source | source install/setup.bash,且 ROS_DOMAIN_ID=25 与机器人一致 |
| 急停可用 | 执行任何控制接口前,必须确认急停可用、能在异常时安全终止 |
speed_percent: 10)。2.2 arm_name 与侧别
| 取值 | 含义 | 说明 |
|---|---|---|
| armleft | 左臂 | 单臂机器人的逻辑名默认仍为 armleft(配置为右臂时为 armright) |
| armright | 右臂 | 双臂配置下的右侧 |
| both | 双臂 | 仅部分接口可用;名称无效时返回 success=false |
both 不等于「双臂合并」:/fd_arm/get_state 传 both 只返回右臂;/fd_arm/set_manipulator 传 both 时必须填写 joint_names。需要双臂操作时,请分别对每一侧各调一次。2.3 关节名与自由度
双臂为 left_tl_robot_joint1…N 与 right_tl_robot_joint1…N;单臂同样带所在侧前缀(默认 left_)。N 为该侧自由度:通常为 7,机型 hh_y2_bdt 为 6。
trajectory.positions、joint_positions_deg),长度必须与该侧 DOF 一致;到位判定容差默认约 0.035 rad。2.4 单位约定
| 参数 | 单位 | 备注 |
|---|---|---|
trajectory.positions(跟随轨迹) |
弧度 | 标准 JointTrajectory 语义 |
joint_positions_deg(保存点位) |
度 | 控制类接口中唯一用「度」的关节字段 |
set_manipulator.positions |
0–1000 | 夹爪 / 手指开度原始值,不是角度 |
timeout_sec |
秒 | 须 > 0;≤ 0 时按 30 秒(轨迹 60 秒)处理 |
speed_percent |
1–100 | 百分比,默认 15 |
2.5 完成判定:不要用固定延时
控制接口下发命令后,判断是否完成只能靠两处:
- 动作返回的
result_code(0= 成功,其余见 5.4 节); - 订阅
/fd_arm/motion_status,等该臂的busy变为false。
sleep N 秒 再发下一条命令是不可靠的:负载、路径长度、限速都会影响实际耗时,容易造成指令抢占或运动中断。三、控制接口总览
8 个控制接口按「服务管配置、动作管运动」划分。点击接口名可跳转到对应详解。
3.1 服务控制接口(5 个)
| 接口 | 类型 | 作用 | 关键参数 |
|---|---|---|---|
| /fd_arm/power | 服务 | 机械臂上电 / 下电 | arm_name、power_on |
| /fd_arm/set_speed | 服务 | 设置运动速度百分比 | arm_name、speed_percent(1–100) |
| /fd_arm/set_manipulator | 服务 | 控制末端执行器(夹爪 / 灵巧手) | side、positions(0–1000)、joint_names |
| /fd_arm/location_save | 服务 | 保存预置点位 | arm_name、location_name、joint_positions_deg |
| /fd_arm/location_delete | 服务 | 删除预置点位 | arm_name、location_name |
3.2 动作控制接口(3 个)
| 接口 | 类型 | 作用 | 关键参数 |
|---|---|---|---|
| /fd_arm/move_to_location | 动作 | 运动到已保存的预置点位 | arm_name、location_name、use_joint_motion、timeout_sec |
| /fd_arm/follow_trajectory | 动作 | 跟随关节轨迹(多路点,弧度) | arm_name、trajectory、timeout_sec |
| /fd_arm/move_to_target | 动作 | 暂不可用:目标被接受但执行必定中止(逆解未实现) | target_xyz_m(相机坐标系,米) |
power(上电)→ set_speed(设速)→ location_save(存点)→ move_to_location / follow_trajectory(运动)→ 按 result_code 判定 → location_delete(清理)。set_manipulator 可在任意需要抓取 / 张开的时机调用。四、服务控制接口详解
5 个服务控制接口逐个说明。每个接口都给出三段示例:命令行、Python(rclpy)、C++(rclcpp)。
armleft 按需替换为该侧的 armright。fd_msgs/srv/... 的字段名请以本机 ros2 interface show <类型> 的输出为准。4.1 /fd_arm/power —— 上电 / 下电
对指定机械臂上电或下电。这是所有运动的前提——未上电时下发运动命令不会生效。
| 字段 | 侧 | 类型 | 取值 | 说明 |
|---|---|---|---|---|
| arm_name | 请求 | string | armleft / armright / both | 名称无效时返回 success=false |
| power_on | 请求 | bool | true / false | true 为上电,false 为下电 |
| success | 响应 | bool | 是否成功受理 | |
| message | 响应 | string | 失败原因,常见为 invalid arm_name |
get_state 返回的 powered 也可能为 true。请以机械臂能否实际运动为准,不要只依赖该字段。示例
① 命令行(ros2 CLI)
# 左臂上电
ros2 service call /fd_arm/power fd_msgs/srv/ArmPower \
"{arm_name: 'armleft', power_on: true}"
# 左臂下电
ros2 service call /fd_arm/power fd_msgs/srv/ArmPower \
"{arm_name: 'armleft', power_on: false}"
② Python(rclpy)
import rclpy
from rclpy.node import Node
from fd_msgs.srv import ArmPower
class ArmPowerClient(Node):
def __init__(self):
super().__init__('arm_power_client')
self.cli = self.create_client(ArmPower, '/fd_arm/power')
if not self.cli.wait_for_service(timeout_sec=5.0):
raise RuntimeError('/fd_arm/power 不可用')
def send(self, arm_name, power_on):
req = ArmPower.Request()
req.arm_name = arm_name
req.power_on = power_on
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return future.result()
def main():
rclpy.init()
node = ArmPowerClient()
res = node.send('armleft', True) # 上电
print('success =', res.success, '| message =', res.message)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
③ C++(rclcpp)
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/arm_power.hpp>
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("arm_power_client");
auto cli = node->create_client<fd_msgs::srv::ArmPower>("/fd_arm/power");
if (!cli->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "/fd_arm/power 不可用");
return 1;
}
auto req = std::make_shared<fd_msgs::srv::ArmPower::Request>();
req->arm_name = "armleft";
req->power_on = true; // true 上电 / false 下电
auto fut = cli->async_send_request(req);
if (rclcpp::spin_until_future_complete(node, fut, std::chrono::seconds(5))
!= rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "调用超时");
return 1;
}
auto res = fut.get();
RCLCPP_INFO(node->get_logger(), "success=%d message=%s",
res->success, res->message.c_str());
rclcpp::shutdown();
return 0;
}
4.2 /fd_arm/set_speed —— 设置运动速度
设置指定机械臂后续运动的速度百分比。该值会一直保持,直到再次调用修改。
| 字段 | 侧 | 类型 | 取值 | 说明 |
|---|---|---|---|---|
| arm_name | 请求 | string | armleft / armright / both | |
| speed_percent | 请求 | uint8 | 1–100,默认 15 | 超出范围拒绝执行(返回 must be 1..100),不做限幅。内部换算为 percent/100(0.01–1.0)后发布到 /tl_control/<臂>/speed_set |
| success / message | 响应 | bool / string | 是否成功受理 |
0、200 会被直接拒绝,命令不执行并返回错误信息。这与头颈 / 腰部「截断后继续执行」的限幅语义不同。/tl_driver/*/set_speed 这个同名话题没有订阅者,发布无效。示例
① 命令行(ros2 CLI)
# 左臂速度设为 20%
ros2 service call /fd_arm/set_speed fd_msgs/srv/ArmSetSpeed \
"{arm_name: 'armleft', speed_percent: 20}"
② Python(rclpy)
import rclpy
from rclpy.node import Node
from fd_msgs.srv import ArmSetSpeed
class ArmSpeedClient(Node):
def __init__(self):
super().__init__('arm_speed_client')
self.cli = self.create_client(ArmSetSpeed, '/fd_arm/set_speed')
if not self.cli.wait_for_service(timeout_sec=5.0):
raise RuntimeError('/fd_arm/set_speed 不可用')
def send(self, arm_name, speed_percent):
# 先做本地校验,避免被服务端拒绝
if not 1 <= speed_percent <= 100:
raise ValueError('speed_percent 必须在 1..100 之间')
req = ArmSetSpeed.Request()
req.arm_name = arm_name
req.speed_percent = speed_percent
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return future.result()
def main():
rclpy.init()
node = ArmSpeedClient()
res = node.send('armleft', 20) # 20% 速度
print('success =', res.success, '| message =', res.message)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
③ C++(rclcpp)
#include <chrono>
#include <memory>
#include <stdexcept>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/arm_set_speed.hpp>
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("arm_speed_client");
auto cli = node->create_client<fd_msgs::srv::ArmSetSpeed>("/fd_arm/set_speed");
if (!cli->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "/fd_arm/set_speed 不可用");
return 1;
}
const uint8_t speed_percent = 20; // 必须在 1..100
if (speed_percent < 1 || speed_percent > 100) {
RCLCPP_ERROR(node->get_logger(), "speed_percent 必须在 1..100 之间");
return 1;
}
auto req = std::make_shared<fd_msgs::srv::ArmSetSpeed::Request>();
req->arm_name = "armleft";
req->speed_percent = speed_percent;
auto fut = cli->async_send_request(req);
if (rclcpp::spin_until_future_complete(node, fut, std::chrono::seconds(5))
!= rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "调用超时");
return 1;
}
auto res = fut.get();
RCLCPP_INFO(node->get_logger(), "success=%d message=%s",
res->success, res->message.c_str());
rclcpp::shutdown();
return 0;
}
4.3 /fd_arm/set_manipulator —— 控制末端执行器
控制机械臂末端的夹爪 / 灵巧手开合。兼容二指夹爪与多指灵巧手,靠 joint_names 区分。
| 字段 | 侧 | 类型 | 取值 | 说明 |
|---|---|---|---|---|
| side | 请求 | string | left / right / both | 注意不是 armleft / armright |
| positions | 请求 | float64[] | 0–1000 | 二指夹爪或手指的开度原始值;长度须与 joint_names 一致 |
| joint_names | 请求 | string[] | left_jaw / right_jaw(二指) left_hand_* 等(灵巧手) |
side 为 left / right 且未填时,默认控制该侧的 *_jaw;side 为 both 时必须填写 |
| success / message | 响应 | bool / string | 当前实现只负责转发命令,大多数情况下都返回 success=true |
success=true 判断夹爪是否真的动了。本服务只做命令转发,响应不代表执行结果。如需确认,请通过相机或力传感器等外部手段核验。示例
① 命令行(ros2 CLI)
# 左手二指夹爪开到 800(0–1000)
ros2 service call /fd_arm/set_manipulator fd_msgs/srv/ArmSetManipulator \
"{side: 'left', positions: [800.0], joint_names: ['left_jaw']}"
# 双手:side=both 时必须显式给出 joint_names
ros2 service call /fd_arm/set_manipulator fd_msgs/srv/ArmSetManipulator \
"{side: 'both', positions: [800.0, 800.0], joint_names: ['left_jaw', 'right_jaw']}"
② Python(rclpy)
import rclpy
from rclpy.node import Node
from fd_msgs.srv import ArmSetManipulator
class ManipulatorClient(Node):
def __init__(self):
super().__init__('arm_manipulator_client')
self.cli = self.create_client(ArmSetManipulator, '/fd_arm/set_manipulator')
if not self.cli.wait_for_service(timeout_sec=5.0):
raise RuntimeError('/fd_arm/set_manipulator 不可用')
def send(self, side, positions, joint_names=None):
req = ArmSetManipulator.Request()
req.side = side
req.positions = [float(v) for v in positions]
# 注意:side 为 both 时必须填 joint_names
req.joint_names = list(joint_names) if joint_names else []
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return future.result()
def main():
rclpy.init()
node = ManipulatorClient()
res = node.send('left', [800.0], ['left_jaw']) # 左手夹爪开到 800
print('success =', res.success, '| message =', res.message)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
③ C++(rclcpp)
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/arm_set_manipulator.hpp>
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("arm_manipulator_client");
auto cli = node->create_client<fd_msgs::srv::ArmSetManipulator>(
"/fd_arm/set_manipulator");
if (!cli->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "/fd_arm/set_manipulator 不可用");
return 1;
}
auto req = std::make_shared<fd_msgs::srv::ArmSetManipulator::Request>();
req->side = "left";
req->positions = {800.0}; // 0–1000
req->joint_names = {"left_jaw"}; // side=both 时必须填写
auto fut = cli->async_send_request(req);
if (rclcpp::spin_until_future_complete(node, fut, std::chrono::seconds(5))
!= rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "调用超时");
return 1;
}
auto res = fut.get();
RCLCPP_INFO(node->get_logger(), "success=%d message=%s",
res->success, res->message.c_str());
rclcpp::shutdown();
return 0;
}
4.4 /fd_arm/location_save —— 保存预置点位
把当前姿态或一组指定关节角保存为命名点位,之后可用 /fd_arm/move_to_location 一键回到该位置。
| 字段 | 侧 | 类型 | 取值 | 说明 |
|---|---|---|---|---|
| arm_name | 请求 | string | armleft / armright | |
| location_name | 请求 | string | 非空 | 点位名,不能使用 after_grasp(保留名) |
| joint_positions_deg | 请求 | float64[] | 度,长度等于自由度 | 留空(空数组)= 保存当前位置 |
| success / message | 响应 | bool / string | 是否保存成功 |
follow_trajectory 的 positions)都是弧度。混用会造成数量级错误。示例
① 命令行(ros2 CLI)
# 空数组 = 保存当前位置为 sdk_demo
ros2 service call /fd_arm/location_save fd_msgs/srv/ArmLocationSave \
"{arm_name: 'armleft', location_name: 'sdk_demo', joint_positions_deg: []}"
# 也可以直接给一组关节角(单位:度,长度须等于该侧 DOF)
ros2 service call /fd_arm/location_save fd_msgs/srv/ArmLocationSave \
"{arm_name: 'armleft', location_name: 'ready_pose',
joint_positions_deg: [0.0, -30.0, 0.0, -60.0, 0.0, 45.0, 0.0]}"
② Python(rclpy)
import rclpy
from rclpy.node import Node
from fd_msgs.srv import ArmLocationSave
class LocationSaveClient(Node):
def __init__(self):
super().__init__('location_save_client')
self.cli = self.create_client(ArmLocationSave, '/fd_arm/location_save')
if not self.cli.wait_for_service(timeout_sec=5.0):
raise RuntimeError('/fd_arm/location_save 不可用')
def save_current(self, arm_name, location_name):
"""空数组 = 保存当前位置。"""
req = ArmLocationSave.Request()
req.arm_name = arm_name
req.location_name = location_name
req.joint_positions_deg = [] # 空 = 保存当前姿态
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return future.result()
def save_target(self, arm_name, location_name, joint_positions_deg):
"""指定关节角保存(单位:度)。"""
req = ArmLocationSave.Request()
req.arm_name = arm_name
req.location_name = location_name
req.joint_positions_deg = [float(v) for v in joint_positions_deg]
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return future.result()
def main():
rclpy.init()
node = LocationSaveClient()
res = node.save_current('armleft', 'sdk_demo')
print('success =', res.success, '| message =', res.message)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
③ C++(rclcpp)
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/arm_location_save.hpp>
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("location_save_client");
auto cli = node->create_client<fd_msgs::srv::ArmLocationSave>(
"/fd_arm/location_save");
if (!cli->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "/fd_arm/location_save 不可用");
return 1;
}
auto req = std::make_shared<fd_msgs::srv::ArmLocationSave::Request>();
req->arm_name = "armleft";
req->location_name = "sdk_demo";
req->joint_positions_deg = {}; // 空 = 保存当前位置;非空时单位是「度」
auto fut = cli->async_send_request(req);
if (rclcpp::spin_until_future_complete(node, fut, std::chrono::seconds(5))
!= rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "调用超时");
return 1;
}
auto res = fut.get();
RCLCPP_INFO(node->get_logger(), "success=%d message=%s",
res->success, res->message.c_str());
rclcpp::shutdown();
return 0;
}
4.5 /fd_arm/location_delete —— 删除预置点位
删除指定名称的预置点位。删除后 move_to_location 再引用该名称会失败。
| 字段 | 侧 | 类型 | 取值 | 说明 |
|---|---|---|---|---|
| arm_name | 请求 | string | armleft / armright | |
| location_name | 请求 | string | 已保存的点位名 | 点位不存在时按服务实现返回失败 |
| success / message | 响应 | bool / string | 是否删除成功 |
/fd_arm/location_list(查询接口)确认点位名,避免误删。删除是幂等操作,重复删除同一名称不会造成其他影响。示例
① 命令行(ros2 CLI)
# 删除 sdk_demo 点位
ros2 service call /fd_arm/location_delete fd_msgs/srv/ArmLocationDelete \
"{arm_name: 'armleft', location_name: 'sdk_demo'}"
# 删除前先确认有哪些点位
ros2 service call /fd_arm/location_list fd_msgs/srv/ArmLocationList \
"{arm_name: 'armleft'}"
② Python(rclpy)
import rclpy
from rclpy.node import Node
from fd_msgs.srv import ArmLocationDelete, ArmLocationList
class LocationDeleteClient(Node):
def __init__(self):
super().__init__('location_delete_client')
self.cli = self.create_client(ArmLocationDelete, '/fd_arm/location_delete')
self.list_cli = self.create_client(ArmLocationList, '/fd_arm/location_list')
if not self.cli.wait_for_service(timeout_sec=5.0):
raise RuntimeError('/fd_arm/location_delete 不可用')
def list_names(self, arm_name):
req = ArmLocationList.Request()
req.arm_name = arm_name
future = self.list_cli.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return list(future.result().location_names)
def remove(self, arm_name, location_name):
req = ArmLocationDelete.Request()
req.arm_name = arm_name
req.location_name = location_name
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return future.result()
def main():
rclpy.init()
node = LocationDeleteClient()
names = node.list_names('armleft')
print('当前点位:', names)
if 'sdk_demo' in names:
res = node.remove('armleft', 'sdk_demo')
print('success =', res.success, '| message =', res.message)
else:
print('sdk_demo 不存在,无需删除')
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
③ C++(rclcpp)
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <fd_msgs/srv/arm_location_delete.hpp>
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("location_delete_client");
auto cli = node->create_client<fd_msgs::srv::ArmLocationDelete>(
"/fd_arm/location_delete");
if (!cli->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "/fd_arm/location_delete 不可用");
return 1;
}
auto req = std::make_shared<fd_msgs::srv::ArmLocationDelete::Request>();
req->arm_name = "armleft";
req->location_name = "sdk_demo";
auto fut = cli->async_send_request(req);
if (rclcpp::spin_until_future_complete(node, fut, std::chrono::seconds(5))
!= rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "调用超时");
return 1;
}
auto res = fut.get();
RCLCPP_INFO(node->get_logger(), "success=%d message=%s",
res->success, res->message.c_str());
rclcpp::shutdown();
return 0;
}
五、动作控制接口详解
3 个动作控制接口逐个说明。运动类接口全部是 Action,因为它们耗时长、需要反馈进度、需要可取消。
请求(Goal)——下发目标;
反馈(Feedback)——执行过程中周期返回进度;
结果(Result)——结束时返回
result_code 与最终关节角。5.1 /fd_arm/move_to_location —— 运动到预置点位
把指定机械臂运动到先前保存的命名点位。这是最常用的运动接口。
| 字段 | 侧 | 类型 | 取值 | 说明 |
|---|---|---|---|---|
| arm_name | 请求 | string | armleft / armright | 指定的机械臂不存在时拒绝执行 |
| location_name | 请求 | string | 已保存的点位名 | after_grasp 会被固定拒绝 |
| use_joint_motion | 请求 | bool | true / false | true 使用关节空间规划;false(默认)按路点执行 |
| timeout_sec | 请求 | float64 | > 0,单位秒 | ≤ 0 时按 30 秒处理 |
| result_code / message | 结果 | int32 / string | 见 5.4 节 | 成败判定依据 |
| final_joint_positions_rad | 结果 | float64[] | 弧度 | 运动结束时的关节角 |
| progress | 反馈 | float32 | 0–1 | 执行进度 |
| points_sent / points_total | 反馈 | uint32 | 已发送路点数 / 路点总数 | |
| status_text | 反馈 | string | 当前状态说明 |
after_grasp 被固定拒绝——保存时不能用、运动到它也会被拒绝。这是系统保留名,请换一个名称。示例
① 命令行(ros2 CLI)
# 运动到 home 点位(按路点执行,超时 30 秒),并显示反馈
ros2 action send_goal /fd_arm/move_to_location fd_msgs/action/ArmMoveToLocation \
"{arm_name: 'armleft', location_name: 'home',
use_joint_motion: false, timeout_sec: 30.0}" --feedback
② Python(rclpy)
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from fd_msgs.action import ArmMoveToLocation
class MoveToLocationClient(Node):
def __init__(self):
super().__init__('move_to_location_client')
self.cli = ActionClient(self, ArmMoveToLocation, '/fd_arm/move_to_location')
if not self.cli.wait_for_server(timeout_sec=5.0):
raise RuntimeError('/fd_arm/move_to_location 不可用')
def on_feedback(self, fb):
f = fb.feedback
self.get_logger().info(
'progress=%.2f points=%d/%d %s'
% (f.progress, f.points_sent, f.points_total, f.status_text))
def send(self, arm_name, location_name, use_joint_motion=False, timeout_sec=30.0):
goal = ArmMoveToLocation.Goal()
goal.arm_name = arm_name
goal.location_name = location_name
goal.use_joint_motion = use_joint_motion
goal.timeout_sec = timeout_sec
send_fut = self.cli.send_goal_async(goal, feedback_callback=self.on_feedback)
rclpy.spin_until_future_complete(self, send_fut)
handle = send_fut.result()
if not handle.accepted:
self.get_logger().error('目标被拒绝')
return None
res_fut = handle.get_result_async()
rclpy.spin_until_future_complete(self, res_fut)
return res_fut.result().result
def main():
rclpy.init()
node = MoveToLocationClient()
result = node.send('armleft', 'home', use_joint_motion=False, timeout_sec=30.0)
if result is not None:
print('result_code =', result.result_code, '| message =', result.message)
print('final_joint_positions_rad =', list(result.final_joint_positions_rad))
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
③ C++(rclcpp)
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <fd_msgs/action/arm_move_to_location.hpp>
using ArmMoveToLocation = fd_msgs::action::ArmMoveToLocation;
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("move_to_location_client");
auto cli = rclcpp_action::create_client<ArmMoveToLocation>(
node, "/fd_arm/move_to_location");
if (!cli->wait_for_action_server(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "/fd_arm/move_to_location 不可用");
return 1;
}
ArmMoveToLocation::Goal goal;
goal.arm_name = "armleft";
goal.location_name = "home";
goal.use_joint_motion = false; // 默认按路点执行
goal.timeout_sec = 30.0;
rclcpp_action::Client<ArmMoveToLocation>::SendGoalOptions opts;
opts.feedback_callback =
[node](auto, const std::shared_ptr<const ArmMoveToLocation::Feedback> fb) {
RCLCPP_INFO(node->get_logger(), "progress=%.2f points=%u/%u %s",
fb->progress, fb->points_sent, fb->points_total,
fb->status_text.c_str());
};
auto goal_fut = cli->async_send_goal(goal, opts);
if (rclcpp::spin_until_future_complete(node, goal_fut) !=
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "发送目标失败");
return 1;
}
auto handle = goal_fut.get();
if (!handle) {
RCLCPP_ERROR(node->get_logger(), "目标被拒绝");
return 1;
}
auto res_fut = cli->async_get_result(handle);
if (rclcpp::spin_until_future_complete(node, res_fut) !=
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "等待结果失败");
return 1;
}
auto wrapped = res_fut.get();
RCLCPP_INFO(node->get_logger(), "result_code=%d message=%s",
wrapped.result->result_code, wrapped.result->message.c_str());
rclcpp::shutdown();
return 0;
}
5.2 /fd_arm/follow_trajectory —— 跟随关节轨迹
让机械臂依次经过一串关节路点(标准 trajectory_msgs/JointTrajectory)。适合连续、自定义的关节空间动作。
| 字段 | 侧 | 类型 | 取值 | 说明 |
|---|---|---|---|---|
| arm_name | 请求 | string | armleft / armright | |
| trajectory | 请求 | JointTrajectory | joint_names 用 2.3 节所列关节名;positions 单位为弧度 |
至少包含 1 个轨迹点;time_from_start 须递增 |
| timeout_sec | 请求 | float64 | > 0,单位秒 | ≤ 0 时按 60 秒处理 |
| pre_interpolated | 请求 | bool | 目前不生效,服务端始终会重新插值 | |
| 结果 / 反馈 | 与 move_to_location 相同 |
pre_interpolated=true 不会让服务端跳过插值。示例
① 命令行(ros2 CLI)
# 单点轨迹:全部关节回到 0 弧度,2 秒到位
ros2 action send_goal /fd_arm/follow_trajectory fd_msgs/action/ArmFollowTrajectory \
"{arm_name: 'armleft', timeout_sec: 30.0, pre_interpolated: false,
trajectory: {
joint_names: ['left_tl_robot_joint1','left_tl_robot_joint2','left_tl_robot_joint3',
'left_tl_robot_joint4','left_tl_robot_joint5','left_tl_robot_joint6',
'left_tl_robot_joint7'],
points: [{positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
time_from_start: {sec: 2, nanosec: 0}}]}}" --feedback
② Python(rclpy)
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from builtin_interfaces.msg import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from fd_msgs.action import ArmFollowTrajectory
DOF = 7 # 该侧自由度:通常 7,hh_y2_bdt 为 6
class FollowTrajectoryClient(Node):
def __init__(self, joint_prefix='left_'):
super().__init__('follow_trajectory_client')
self.cli = ActionClient(self, ArmFollowTrajectory, '/fd_arm/follow_trajectory')
if not self.cli.wait_for_server(timeout_sec=5.0):
raise RuntimeError('/fd_arm/follow_trajectory 不可用')
self.joint_names = ['%stl_robot_joint%d' % (joint_prefix, i)
for i in range(1, DOF + 1)]
def build_trajectory(self, waypoints):
"""waypoints: [(positions_rad_list, sec), ...],时间须递增。"""
traj = JointTrajectory()
traj.joint_names = self.joint_names
for positions_rad, sec in waypoints:
if len(positions_rad) != DOF:
raise ValueError('关节角长度必须等于 DOF=%d' % DOF)
p = JointTrajectoryPoint()
p.positions = [float(v) for v in positions_rad] # 单位:弧度
p.time_from_start = Duration(sec=int(sec), nanosec=0)
traj.points.append(p)
return traj
def send(self, arm_name, trajectory, timeout_sec=30.0):
goal = ArmFollowTrajectory.Goal()
goal.arm_name = arm_name
goal.trajectory = trajectory
goal.timeout_sec = timeout_sec
goal.pre_interpolated = False # 该字段目前不生效
send_fut = self.cli.send_goal_async(goal)
rclpy.spin_until_future_complete(self, send_fut)
handle = send_fut.result()
if not handle.accepted:
self.get_logger().error('目标被拒绝')
return None
res_fut = handle.get_result_async()
rclpy.spin_until_future_complete(self, res_fut)
return res_fut.result().result
def main():
rclpy.init()
node = FollowTrajectoryClient(joint_prefix='left_')
zero = [0.0] * DOF
traj = node.build_trajectory([(zero, 2)]) # 单点,2 秒到位
result = node.send('armleft', traj, timeout_sec=30.0)
if result is not None:
print('result_code =', result.result_code, '| message =', result.message)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
③ C++(rclcpp)
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <fd_msgs/action/arm_follow_trajectory.hpp>
using ArmFollowTrajectory = fd_msgs::action::ArmFollowTrajectory;
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("follow_trajectory_client");
auto cli = rclcpp_action::create_client<ArmFollowTrajectory>(
node, "/fd_arm/follow_trajectory");
if (!cli->wait_for_action_server(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "/fd_arm/follow_trajectory 不可用");
return 1;
}
const std::size_t dof = 7; // 该侧自由度,hh_y2_bdt 为 6
ArmFollowTrajectory::Goal goal;
goal.arm_name = "armleft";
goal.timeout_sec = 30.0;
goal.pre_interpolated = false; // 该字段目前不生效
goal.trajectory.joint_names.clear();
for (std::size_t i = 1; i <= dof; ++i) {
goal.trajectory.joint_names.push_back(
"left_tl_robot_joint" + std::to_string(i));
}
trajectory_msgs::msg::JointTrajectoryPoint p;
p.positions.assign(dof, 0.0); // 单位:弧度
p.time_from_start.sec = 2;
p.time_from_start.nanosec = 0;
goal.trajectory.points.push_back(p); // 至少 1 个点,时间须递增
auto goal_fut = cli->async_send_goal(goal);
if (rclcpp::spin_until_future_complete(node, goal_fut) !=
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "发送目标失败");
return 1;
}
auto handle = goal_fut.get();
if (!handle) {
RCLCPP_ERROR(node->get_logger(), "目标被拒绝");
return 1;
}
auto res_fut = cli->async_get_result(handle);
rclcpp::spin_until_future_complete(node, res_fut);
auto wrapped = res_fut.get();
RCLCPP_INFO(node->get_logger(), "result_code=%d message=%s",
wrapped.result->result_code, wrapped.result->message.c_str());
rclcpp::shutdown();
return 0;
}
5.3 /fd_arm/move_to_target —— 目标点运动 暂不可用
该动作接受相机坐标系下的目标位置(target_xyz_m,单位米),但目前执行时必定中止——逆解尚未实现。
move_to_location(预置点位)或 follow_trajectory(关节轨迹)。示例(替代写法)
本接口没有可用示例,下面用「不要这样写 / 应该这样写」的对照形式给出替代方案:
① 命令行(ros2 CLI)
# ✗ 错误:目标会被接受,但执行时必定中止
# ros2 action send_goal /fd_arm/move_to_target fd_msgs/action/ArmMoveToTarget \
# "{arm_name: 'armleft', target_xyz_m: {x: 0.3, y: 0.0, z: 0.4}, timeout_sec: 30.0}"
# ✓ 正确:先用目标位置示教并存为点位,再运动到该点位
ros2 service call /fd_arm/location_save fd_msgs/srv/ArmLocationSave \
"{arm_name: 'armleft', location_name: 'pick_pose', joint_positions_deg: []}"
ros2 action send_goal /fd_arm/move_to_location fd_msgs/action/ArmMoveToLocation \
"{arm_name: 'armleft', location_name: 'pick_pose',
use_joint_motion: false, timeout_sec: 30.0}"
② Python(rclpy)
# ✗ 错误写法:不要构造 ArmMoveToTarget 目标
# from fd_msgs.action import ArmMoveToTarget # 请勿使用
# goal = ArmMoveToTarget.Goal()
# goal.target_xyz_m.x = 0.3 # 会被接受,但执行必定中止
# ✓ 正确写法:改用 move_to_location(详见 5.1 节完整示例)
from fd_msgs.action import ArmMoveToLocation
goal = ArmMoveToLocation.Goal()
goal.arm_name = 'armleft'
goal.location_name = 'pick_pose' # 事先用 location_save 存好的点位
goal.use_joint_motion = False
goal.timeout_sec = 30.0
③ C++(rclcpp)
// ✗ 错误写法:不要创建 ArmMoveToTarget 的动作客户端
// #include <fd_msgs/action/arm_move_to_target.hpp> // 请勿使用
// auto cli = rclcpp_action::create_client<fd_msgs::action::ArmMoveToTarget>(
// node, "/fd_arm/move_to_target");
// ✓ 正确写法:改用 move_to_location(详见 5.1 节完整示例)
#include <fd_msgs/action/arm_move_to_location.hpp>
auto cli = rclcpp_action::create_client<fd_msgs::action::ArmMoveToLocation>(
node, "/fd_arm/move_to_location");
fd_msgs::action::ArmMoveToLocation::Goal goal;
goal.arm_name = "armleft";
goal.location_name = "pick_pose"; // 事先用 location_save 存好的点位
goal.use_joint_motion = false;
goal.timeout_sec = 30.0;
5.4 动作结束码(result_code)
move_to_location、follow_trajectory 的结果,以及 /fd_arm/motion_status 的 last_result_code,共用同一套结束码:
| result_code | 名称 | 含义 | 处理建议 |
|---|---|---|---|
| 0 | RESULT_SUCCESS | 成功到位 | 唯一表示成功的取值 |
| 1 | RESULT_CANCELED | 已取消 | 调用方主动取消,无需重试 |
| 2 | RESULT_PREEMPTED | 被新目标打断 | 检查是否并发下发了运动命令 |
| 3 | RESULT_TIMEOUT | 超时 | 调大 timeout_sec 或降低速度要求 |
| 4 | RESULT_NOT_REACHED | 未在容差范围内到位 | 默认容差约 0.035 rad;检查是否受阻 |
| 5 | RESULT_REJECTED | 目标被拒绝,未执行 | 多为点位名非法(如 after_grasp)或臂名不存在 |
| −1 | RESULT_NONE | 尚无结果 | 仅出现在 motion_status 话题中,表示尚未执行过动作 |
result_code == 0 才算成功。不要用「命令发出去了」或「等了几秒」来判断运动完成。六、完整控制流程示例
把前面 8 个接口串成一次完整的控制闭环:上电 → 设速 → 存点 → 运动 → 判定 → 清理 → 下电。
6.1 流程步骤
| 步骤 | 接口 | 要点 |
|---|---|---|
| 1 | /fd_arm/power | 上电;成功后不要只信 powered 字段 |
| 2 | /fd_arm/set_speed | 先设低速(如 10–20),确认动作正常后再提高 |
| 3 | /fd_arm/location_save | 把当前安全位姿存为点位,joint_positions_deg: [] |
| 4 | /fd_arm/move_to_location | 运动到该点位;timeout_sec 给足 |
| 5 | 结果 / /fd_arm/motion_status |
按 result_code == 0 或 busy == false 判定 |
| 6 | /fd_arm/location_delete | 清理临时点位 |
| 7 | /fd_arm/power | 下电(power_on: false) |
/fd_arm/set_manipulator:先张开夹爪 → 运动到目标点 → 闭合夹爪。6.2 Python 端到端示例(rclpy)
"""机械臂控制闭环示例:上电 → 设速 → 存点 → 运动 → 判定 → 清理 → 下电
运行前请确认:fd_arm_server 已启动、急停可用、工作空间内无人。
"""
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from fd_msgs.srv import ArmPower, ArmSetSpeed, ArmLocationSave, ArmLocationDelete
from fd_msgs.action import ArmMoveToLocation
class ArmControlDemo(Node):
ARM = 'armleft' # 按需改为 armright
def __init__(self):
super().__init__('arm_control_demo')
self.srv_power = self.create_client(ArmPower, '/fd_arm/power')
self.srv_speed = self.create_client(ArmSetSpeed, '/fd_arm/set_speed')
self.srv_save = self.create_client(ArmLocationSave, '/fd_arm/location_save')
self.srv_del = self.create_client(ArmLocationDelete, '/fd_arm/location_delete')
self.act_move = ActionClient(self, ArmMoveToLocation, '/fd_arm/move_to_location')
# ---------- 通用服务调用 ----------
def _call(self, client, srv_type, **fields):
req = srv_type.Request()
for key, value in fields.items():
setattr(req, key, value)
future = client.call_async(req)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
return future.result()
def wait_all(self):
pairs = [(self.srv_power, '/fd_arm/power'),
(self.srv_speed, '/fd_arm/set_speed'),
(self.srv_save, '/fd_arm/location_save'),
(self.srv_del, '/fd_arm/location_delete')]
for client, name in pairs:
if not client.wait_for_service(timeout_sec=5.0):
raise RuntimeError('%s 不可用' % name)
if not self.act_move.wait_for_server(timeout_sec=5.0):
raise RuntimeError('/fd_arm/move_to_location 不可用')
# ---------- 运动 ----------
def move_to(self, location_name, timeout_sec=30.0):
goal = ArmMoveToLocation.Goal()
goal.arm_name = self.ARM
goal.location_name = location_name
goal.use_joint_motion = False
goal.timeout_sec = timeout_sec
send_fut = self.act_move.send_goal_async(goal)
rclpy.spin_until_future_complete(self, send_fut)
handle = send_fut.result()
if not handle.accepted:
return None
res_fut = handle.get_result_async()
rclpy.spin_until_future_complete(self, res_fut)
return res_fut.result().result
# ---------- 主流程 ----------
def run(self):
self.wait_all()
log = self.get_logger().info
r = self._call(self.srv_power, ArmPower, arm_name=self.ARM, power_on=True)
log('[1] 上电 success=%s %s' % (r.success, r.message))
r = self._call(self.srv_speed, ArmSetSpeed, arm_name=self.ARM, speed_percent=15)
log('[2] 设速 15%% success=%s %s' % (r.success, r.message))
r = self._call(self.srv_save, ArmLocationSave, arm_name=self.ARM,
location_name='sdk_demo', joint_positions_deg=[])
log('[3] 保存点位 success=%s %s' % (r.success, r.message))
result = self.move_to('sdk_demo', timeout_sec=30.0)
if result is None:
log('[4] 运动 目标被拒绝')
else:
ok = (result.result_code == 0)
log('[4] 运动 code=%d ok=%s %s'
% (result.result_code, ok, result.message))
r = self._call(self.srv_del, ArmLocationDelete, arm_name=self.ARM,
location_name='sdk_demo')
log('[5] 删除点位 success=%s %s' % (r.success, r.message))
r = self._call(self.srv_power, ArmPower, arm_name=self.ARM, power_on=False)
log('[6] 下电 success=%s %s' % (r.success, r.message))
def main():
rclpy.init()
node = ArmControlDemo()
try:
node.run()
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
ros2 interface show fd_msgs/srv/ArmPower 等命令的输出为准;实机运行前请先在仿真或空载条件下验证。6.3 命令行端到端(bash 脚本)
#!/usr/bin/env bash
set -e
ARM=armleft
echo "[1] 上电"
ros2 service call /fd_arm/power fd_msgs/srv/ArmPower \
"{arm_name: '$ARM', power_on: true}"
echo "[2] 设速 15%"
ros2 service call /fd_arm/set_speed fd_msgs/srv/ArmSetSpeed \
"{arm_name: '$ARM', speed_percent: 15}"
echo "[3] 保存当前位姿为 sdk_demo"
ros2 service call /fd_arm/location_save fd_msgs/srv/ArmLocationSave \
"{arm_name: '$ARM', location_name: 'sdk_demo', joint_positions_deg: []}"
echo "[4] 运动到 sdk_demo(看 result_code 是否为 0)"
ros2 action send_goal /fd_arm/move_to_location fd_msgs/action/ArmMoveToLocation \
"{arm_name: '$ARM', location_name: 'sdk_demo',
use_joint_motion: false, timeout_sec: 30.0}"
echo "[5] 删除临时点位"
ros2 service call /fd_arm/location_delete fd_msgs/srv/ArmLocationDelete \
"{arm_name: '$ARM', location_name: 'sdk_demo'}"
echo "[6] 下电"
ros2 service call /fd_arm/power fd_msgs/srv/ArmPower \
"{arm_name: '$ARM', power_on: false}"
6.4 C++ 端到端
C++(rclcpp)的写法与 Python 一一对应,各接口的完整 C++ 示例已分别给在 4.1–4.5 与 5.1–5.2 节,按 6.1 的顺序组合即可:
- 创建 4 个服务客户端(
create_client)+ 1 个动作客户端(rclcpp_action::create_client); - 每个服务用
async_send_request+spin_until_future_complete同步等待; - 动作依次调用
async_send_goal→async_get_result,并在SendGoalOptions.feedback_callback中打印进度; - 统一用
result_code == 0判定成功。
七、注意事项与已知限制
7.1 五条硬性约束
result_code == 0 才算运动成功。不要用固定延时判断运动结束;等待完成只认动作结果码或 motion_status.busy == false。powered 字段不可信。/fd_arm/power 调用后直接记为已上电、不回读驱动,上电失败时 get_state.powered 也可能为 true。以机械臂能否实际运动为准。/fd_arm/move_to_target。目标会被接受,但执行必定中止(逆解未实现)。请改用 move_to_location 或 follow_trajectory。location_save.joint_positions_deg 是度,follow_trajectory.trajectory.positions 是弧度,set_manipulator.positions 是 0–1000 原始开度。/tl_control/*、/tl_driver/*、/teleop/joint_targets_* 与 /fd_arm/* 同时下发会造成命令互相干扰。7.2 机械臂相关已知限制
| 接口 / 字段 | 限制 | 建议用法 | 影响 |
|---|---|---|---|
| /fd_arm/move_to_target | 目标会被接受,但执行时必定中止(逆解尚未实现) | 请勿调用;改用 move_to_location 或 follow_trajectory |
高 |
| /fd_arm/power 的 powered | 调用后直接记为已上电,不回读驱动状态 | 上电后以机械臂实际能否运动、驱动反馈为准 | 高 |
| /teleop/joint_targets_* | 多个发布者同时发布会相互干扰 | 请勿与 /fd_arm/* 运动命令同时使用 |
高 |
| move_to_location 的 after_grasp | 该点位名会被固定拒绝 | 请勿使用该点位名(保存与运动均拒绝) | 中 |
| follow_trajectory 的 pre_interpolated | 该字段目前不生效,服务端始终重新插值 | 规划轨迹的时间与点密度时,请考虑服务端会重新插值 | 中 |
| set_manipulator 的 side=both | joint_names 为空时可能下发空的 name |
side 为 both 时必须填写 joint_names |
中 |
| /fd_arm/get_state 的 arm_name=both | 双臂配置下实际只返回右臂,不会合并左右臂 | 分别查询 armleft 和 armright |
中 |
| /tl_driver/*/set_speed | 该话题没有订阅者,发布无效 | 调整速度请使用 /fd_arm/set_speed |
中 |
| /fd_robot/fd_manipulator/finger2_cmd | 当前实现会进入 finger1 的回调 | 请勿作为第二指的控制接口 | 中 |
| /tl_driver/current_joint_states | 双臂数据合并在同一话题中 | 通过 left_ / right_ 关节名前缀区分,没有分臂话题 |
低 |
| dahuan_gripper | 无法正常启动 | 请勿作为可用末端 | 低 |
7.3 对接前检查清单
| 检查项 | 通过标准 |
|---|---|
fd_arm_server 在运行 |
ros2 service list | grep /fd_arm/ 能看到全部接口 |
| 急停可用 | 按下急停能立即停止机械臂 |
| 侧别与关节名匹配 | get_state 返回的 joint_positions_rad 长度 = 该侧 DOF |
| 速度已设低值 | 首次联调用 speed_percent: 10–20 |
| 完成判定已实现 | 代码中依据 result_code 或 busy,而非固定 sleep |
| 点位名合法 | 未使用保留名 after_grasp |
| 未混用系统内部接口 | 代码中不出现 /tl_control/*、/tl_driver/*、/teleop/* |
