ros2_robot_interface¶

通过 ROS 2 话题（以及少量 Action / 服务）与本技术栈通信的 Python 客户端。

仓库： fiveages-sim/ros2_robot_interface

公开导出：ROS2RobotInterface、ROS2RobotInterfaceConfig、ControlType、FSM 常量（FSM_HOME、FSM_HOLD、FSM_OCS2、FSM_MOVEJ、FSM_COMPLIANCE）以及 ROS2*Error 异常。完整方法文档见 GitHub 上的 API_REFERENCE.md。

安装¶

通过 fa-py-libraries 安装（Python 3.12 环境）。该仓库的 ./init.sh all 会安装 ros2_robot_interface 以及其他子模块。

git clone https://github.com/fiveages-sim/fa-py-libraries.git
cd fa-py-libraries
./init.sh all

数据生成 / 运动队列路径：在 lerobot_ros2 检出中，./init.sh all-motion（或 ./init.sh all）会把同一包装成 submodules/ros2_robot_interface。

手动备选

单独克隆 ros2_robot_interface 并 pip install -e .（在项目虚拟环境内；uv 加 --no-deps 以免从 PyPI 拉 rclpy）只用于这些伞仓之外的包开发。不要把系统 Python 上的 pip install ros2-robot-interface 当作本栈入口。

快速开始¶

下面的示例是软件包 README 的「Basic Example」，钉在提交 200ea42。如何刷新钉选（并重算 :start-line: / :end-line:）：文档构建。

from ros2_robot_interface import ROS2RobotInterface, ROS2RobotInterfaceConfig
from geometry_msgs.msg import Pose

# Create configuration
config = ROS2RobotInterfaceConfig(
    joint_states_topic="/joint_states",
    end_effector_pose_topic="/left_current_pose",
    end_effector_target_topic="/left_target",
    joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
)

# Create and connect interface
interface = ROS2RobotInterface(config)
interface.connect()

# Get joint state
joint_state = interface.get_joint_state()
if joint_state:
    print(f"Joint positions: {joint_state['positions']}")

# Get end-effector pose (returns None if not connected)
pose = interface.left_arm_handler.get_pose()
if pose:
    print(f"End-effector position: ({pose.position.x}, {pose.position.y}, {pose.position.z})")
else:
    print("Interface not connected or pose not available")

# Send target pose
target_pose = Pose()
target_pose.position.x = 0.5
target_pose.position.y = 0.0
target_pose.position.z = 0.3
target_pose.orientation.w = 1.0
interface.left_arm_handler.send_target(target_pose)

# Control gripper
interface.left_gripper_handler.send_joint_positions(0.5)  # 行程/开度目标值为 0.5（非“50%百分比”语义）

# Disconnect
interface.disconnect()

当这些话题已在 ROS 图中时，connect() 会自动检测双臂位姿话题、夹爪 / 手控制器，以及分体 vs 全身关节话题。is_connected 是属性。笛卡尔与关节下发走 handler（left_arm_handler、right_arm_handler、left_gripper_handler 等），不是 move_j / move_l 封装。

当 auto_switch_fsm_before_control=True（配置默认）时，发布位姿会经 /fsm_command 把控制器切到 OCS2，发布关节会切到 MOVEJ。

话题 ↔ API 对照¶

运行中的 OCS2 / basic_joint_controller 栈上操作员面对的话题，以及发布或读取它们的 Python 调用。下表行名来自 API_REFERENCE.md。分体 vs 全身前缀：分体控制 vs 全身控制。Int32 FSM 取值：状态机与话题。

状态¶

话题	消息	Python	说明
`/joint_states`	`sensor_msgs/JointState`	`get_joint_state()` / `get_joint_state(categorized=True)`	Arms, head, waist, grippers — whatever the robot publishes
`/left_current_pose`, `/right_current_pose`	`geometry_msgs/PoseStamped`	`left_arm_handler.get_pose()` / `right_arm_handler.get_pose()`	返回内部 `Pose`（或 `None`）
`/left_current_target`, `/right_current_target`	`geometry_msgs/PoseStamped`	`left_arm_handler.get_target_pose()`	控制器对当前笛卡尔目标的回写；供 `check_arrival()` 使用
`/body_current_pose`	`geometry_msgs/PoseStamped`	`get_body_current_pose()`	WBC 身体位姿；返回内部 `Pose`
`/body_current_target`	`geometry_msgs/PoseStamped`	`get_body_current_target_pose()`	WBC 身体指令回写
`/ocs2_wbc_controller/current_state`	`arms_ros2_control_msgs/WbcCurrentState`	`wait_until_mode_commands_applied(...)`	WBC 约束快照；确认 `/mode_command`

全身控制（WBC）可用性

/body_target*、/head_target*、/mode_command 以及 WbcCurrentState 需要 ocs2_wbc_controller 以及该机型的全身启动/配置。默认 mock 演示（demo.launch.py）和分体（split_body.launch.py）并不表示这些能力可用。该控制器是私有子模块；能力位取决于机型（WbcCapability）。公开的 Taku mock 使用 split_body.launch.py robot:=taku（臂 MPC + 身体/头 basic + 夹爪），不是 WBC。full_body.launch.py robot:=taku 需要私有 WBC 子模块；描述里有 config/ocs2/fixed_base_tcp.info 并不够。当该配置和私有模块都在时，Taku 全身默认 headMode 是 HEAD_GAZE。细节：FSM 与话题。

FSM 与 WBC 模式¶

话题	消息	Python	说明
`/fsm_command`	`std_msgs/Int32`	`send_fsm_command(command)`	`1` HOME、`2` HOLD、`3` OCS2、`4` MOVEJ、`5` COMPLIANCE（`FSM_*` 常量）。不是 `stand` / `walk` 这类字符串。
`/fsm_state`	`std_msgs/Int32`	`get_fsm_state()`	锁存；整数含义相同
`/mode_command`	`std_msgs/String`	`send_mode_command(command)`	WBC 字符串，例如 `BODY_TRACKING`、`ARMS_COUPLED`、`BASE_LOCK`。用 `wait_until_mode_commands_applied` 确认。API_REFERENCE 只映射 `BODY_` / `ARMS_` / `BASE_` —— 不能经此方法发 `HEAD_`。

笛卡尔目标（OCS2）¶

不带 stamp 的 /left_target 用一个位姿替换当前末端目标（典型于 VR / 高频率遥操作）。带 stamp 的 /left_target/stamped 会经 TF 变换到控制器坐标系并插值成 MoveL 序列（典型于视觉抓取）。未 stamp 的远距离跳变可能产生冲击。send_relative 是一次性增量，不是该坐标系下的绝对位姿。send_velocity 是锁存速度流：控制器对每条 Twist 积分；0.2 秒内没有新消息会停止；持续运动需以 ≥5 Hz 循环发布。

在 ocs2_arm_controller 上，同一条 /left_target/stamped 在 MOVEJ 下可在有 lina_planning 时跑 IK MoveL（保持 FSM=MOVEJ；关闭位姿类自动切 OCS2）。WBC 的 MOVEJ 没有 IK MoveL 路径。

话题	消息	Python	说明
`/left_target`, `/right_target`	`geometry_msgs/Pose`	`left_arm_handler.send_target(pose)`	位姿已在控制器 `base_frame`；无 TF；无 MoveL 插值
`/left_target/stamped`, `/right_target/stamped`	`geometry_msgs/PoseStamped`	`left_arm_handler.send_target_stamped(frame_id, pose)`	TF 到 `base_frame`；若已缓存 `frame_id` 也可 `send_target_stamped(pose)`
`/left_target/relative`, `/right_target/relative`	`geometry_msgs/TwistStamped`	`left_arm_handler.send_relative(dx, dy, dz, droll=0, dpitch=0, dyaw=0, frame_id="")`	一次性增量（m、rad）再走 MoveL。空 `frame_id` → 控制器 `base_frame`
`/left_target/twist`, `/right_target/twist`	`geometry_msgs/Twist`	`left_arm_handler.send_velocity(linear, angular)`	`base_frame` 下的速度 `(vx,vy,vz)` m/s 与 `(wx,wy,wz)` rad/s。全零 Twist 停止。不是 `send_cartesian_velocity`（该方法未实现）
`/dual_target/stamped`	`nav_msgs/Path`	`send_dual_arm_target_stamped(left_pose, right_pose, frame_id=...)`	Path 长度为 2（`[left, right]`）；WBC 可能再附第三个 `body` 位姿
`/target_path`	`nav_msgs/Path`	`send_target_path(left_poses, right_poses, ...)`	双臂笛卡尔路点。Python API 中已弃用；新代码使用 `execute_path`（`ExecutePath` 服务）
`/body_target`	`geometry_msgs/Pose`	`send_body_target(pose)`	WBC；在 `base_frame` 下立即采用绝对位姿。同时把 `/mode_command` 切到 `BODY_TRACKING`
`/body_target/stamped`	`geometry_msgs/PoseStamped`	`send_body_target_stamped(frame_id, pose)`	WBC 身体 MoveL
`/body_target/relative`	`geometry_msgs/TwistStamped`	`send_body_relative(dx, dy, dz, ...)`	WBC 身体一次性增量再走 MoveL
`/head_target`、`/head_target/stamped`	`geometry_msgs/Pose` / `PoseStamped`	—	WBC 头部 6D 在 `arms_target_manager` 上。API_REFERENCE 映射的是头部关节（`send_head_joint_positions`），不是这些笛卡尔话题

MoveJ 关节目标¶

std_msgs/Float64MultiArray。两者都在时 connect() 优先 WBC 话题。隐式 FSM → MOVEJ。

话题	Python	何时
`/ocs2_wbc_controller/target_joint_position/left` (or `/right`)	`left_arm_handler.send_joint_positions(...)`	全身 / `full_body.launch.py`
`/ocs2_arm_controller/target_joint_position/left` (or `/right`)	同一 handler	分体 / `split_body.launch.py`
`/ocs2_arm_controller/target_joint_position`	同上，单臂	单臂 OCS2（无 `/left` 后缀）
`/ocs2_wbc_controller/target_joint_position`	`send_dual_arm_joint_positions(...)`	WBC 上统一的双臂 + 身体
`/ocs2_wbc_controller/target_joint_position/body`	`send_body_joint_positions(...)`	全身 waist / body
`/body_joint_controller/target_joint_position`	`send_body_joint_positions(...)`	分体 waist (`basic_joint_controller`)
`/ocs2_wbc_controller/target_joint_position/head`	`send_head_joint_positions(...)`	全身 head
`/head_joint_controller/target_joint_position`	`send_head_joint_positions(...)`	分体 / always-on head `basic_joint_controller`
`/{arm_controller}/target_joint_trajectory`	`send_joint_trajectory(joint_names, waypoints, …)`	例如 `/ocs2_wbc_controller/target_joint_trajectory`；控制器名从手臂关节话题解析
`/body_joint_controller/target_joint_trajectory`	`send_body_joint_trajectory(...)`	分体腰部轨迹。WBC：同样用 `send_joint_trajectory`，关节名选躯干
`/head_joint_controller/target_joint_trajectory`	`send_head_joint_trajectory(...)`	分体头部轨迹

夹爪与手¶

connect() 会选择 hand_controller 或 gripper_controller（以及左/右名称）。离散开合是 RViz 风格话题；Float64 位置是 Python 行程命令；target_percent 为 0–1。

在 adaptive_gripper_controller 上，send_joint_positions 是直接位置模式（无力反馈）。send_target_command / send_position_percent 走开关 / 比例通道（关闭时有力反馈）。在 basic_joint_controller 手上，同样的比例/开关话题混合 Home 开合构型（target_command_enabled）。

话题	消息	Python	说明
`/left_gripper_controller/target_command` (also `/right_…`, `/gripper_controller/…`)	`std_msgs/Int32` (`0` close / `1` open)	`left_gripper_handler.send_target_command(0\|1)`	自适应夹爪开关
`/left_gripper_joint/position_command` (also `/right_…`, config `gripper_command_topic`)	`std_msgs/Float64`	`left_gripper_handler.send_joint_positions(position)`	硬件单位行程，不是 0–1 百分比。用 `gripper_min_position` / `gripper_max_position` 限幅。无自适应力反馈
`/left_gripper_controller/target_percent` (also `/right_…`, `/gripper_controller/…`)	`std_msgs/Float64` (`0.0`–`1.0`)	`left_gripper_handler.send_position_percent(percent)`	比例通道；未检测到 publisher 时抛错
`/left_hand_controller/target_command` (also `/right_…`, `/hand_controller/…`)	`std_msgs/Int32` (`0`/`1`)	检测到手控制器后同样用 `send_target_command`	`target_command_enabled` 开启时的灵巧手开合
`/left_hand_controller/target_percent`	`std_msgs/Float64` (`0.0`–`1.0`)	检测到手控制器后同样用 `send_position_percent`	`basic_joint_controller` 上的 Home 构型混合
`/left_hand_controller/target_joint_position`	`std_msgs/Float64MultiArray`	`send_left_hand_joint_positions(...)`	按关节的手部 MoveJ

腰部（`basic_joint_controller` / WBC 身体）¶

隐式 FSM → MOVEJ。分体前缀 /body_joint_controller/…；WBC 前缀 /ocs2_wbc_controller/…。话题方法发出即走；需要执行结果时 API_REFERENCE 更推荐 execute_waist_lifting_pose_*_action。

话题	消息	Python	说明
`…/waist_lifting`	`std_msgs/Float64`	`send_waist_lifting_relative_position(dz)`	单轴高度增量（米）
`…/waist_lifting_pose_relative`	`std_msgs/Float64MultiArray` `[dx, dz, dphi]`	`send_waist_lifting_pose_relative(dx, dz, dphi)`	局部相对运动
`…/waist_lifting_pose_absolute`	`std_msgs/Float64MultiArray` `[x, z, phi]`	`send_waist_lifting_pose_absolute(x, z, phi)`	绝对 `[x, z, phi]`（控制器上的 TF 坐标系）
`…/waist_lifting_command`	`std_msgs/Float64`	`send_waist_lifting_velocity_scale(scale)`	速度系数 `[-1, 1]`
`…/waist_turning_command`	`std_msgs/Float64`	`send_waist_turning_velocity_scale(scale)`	转向速度系数 `[-1, 1]`

Action ↔ API 对照¶

这些会等待 Action 结果（或超时）。不是上面的话题行。ROS2RobotInterfaceConfig 上默认的手臂名指向 ocs2_arm_controller；控制器带命名空间时请覆盖配置。腰部 Action 名会自动检测（/ocs2_wbc_controller/waist_lifting_pose 或 /body_joint_controller/waist_lifting_pose）。类型定义：arms_ros2_control_msgs README。话题 vs Action vs Service：FSM 与话题。

Action	典型路径	Python	说明
`ExecuteLinear`	`/ocs2_arm_controller/execute_linear`	`execute_movel_action`	参数化 MoveL（`LinearMessage`）。`auto_switch_fsm=True`（默认）把 FSM 切到 MOVEJ
`MovecUseIK`	`/ocs2_arm_controller/execute_circle_use_ik`	`execute_movec_action_three_point` / `execute_movec_action_parametric`	MoveC（`CircleMessage`；三点法或参数法）。默认 FSM → MOVEJ
`JointTrajectory`	`/ocs2_arm_controller/joint_trajectory_with_para`	`execute_joint_trajectory_action` / `execute_dual_arm_movej_action`	参数化 MoveJ（`JointWaypoint[]`）。话题 `send_joint_trajectory` 没有结果
`WaistLiftingPose`	`…/waist_lifting_pose`	`execute_waist_lifting_pose_absolute_action` / `execute_waist_lifting_pose_relative_action`	目标 `MODE_ABSOLUTE=0` / `MODE_RELATIVE=1`。话题 `send_waist_lifting_pose_*` 发出即走

每项都有 wait_for_*_action_server 辅助函数。无 stamp / 有 stamp 的话题切到 OCS2；这些笛卡尔 / 关节 Action 默认切到 MOVEJ。签名与 max_* 回退见 API_REFERENCE.md。

Service ↔ API 对照¶

在 msgs README 的 srv 类型里，Python 封装了 ExecutePath。Service 是一次请求/响应（没有 progress）。

Service	名称	Python	说明
`ExecutePath`	`execute_path`	`execute_path` / `execute_left_path` / `execute_right_path`	左右臂 `nav_msgs/Path` + `trajectory_duration`；隐式 FSM → OCS2。取代已弃用的 `/target_path`

msgs README 还定义了 srv ExecuteLinear、ExecuteCircle、MovecUseIK、JointTrajectory、CartesianPath 和 KinematicsService。它们没有 ros2_robot_interface 方法——若正在运行的控制器 advertise 了它们，请用 ROS 2 客户端直接调用。

call_compliance_zero_wrench() 是 API_REFERENCE 里单独的力传感器调零服务，不是上面那些笛卡尔 msgs 类型。

Lift 2S 厂商 /body_control

ARX Lift 2S 另有厂商底盘/升降命令路径（/body_control）。那套栈不是 ros2_robot_interface，也不是表中的 OCS2 身体话题（/body_joint_controller/target_joint_position 或 /ocs2_wbc_controller/target_joint_position/body）。硬件接入：ARX Lift 2S。