Python 接口¶

用 ROS2RobotInterface 从 Python 控制机器人。

前提条件¶

已安装 ROS 2 Jazzy
机器人演示正在运行（mock、Gazebo 或真实硬件）
Python 3.12 (ROS 2 Jazzy)

安装 fa-py-libraries¶

cd ~/
git clone https://github.com/fiveages-sim/fa-py-libraries.git
cd fa-py-libraries
./init.sh all    # Python 3.12 env; installs ros2_robot_interface

./init.sh all 会初始化子模块、创建环境并安装 Python 包。然后用 ./run.sh（或 ./run.sh viser）启动。

手动备选

在系统 Python 上 pip install ros2-robot-interface 不是文档主入口。若必须单独装这个包，请在 ./init.sh env 3.12 之后的 fa-py-libraries 环境内安装。

Connect, read, command¶

类名是 ROS2RobotInterface 与 ROS2RobotInterfaceConfig。README「Basic Example」（配置、connect()、get_joint_state()、left_arm_handler）在 ros2_robot_interface。

from geometry_msgs.msg import Pose
from ros2_robot_interface import (
    FSM_HOLD,
    ROS2RobotInterface,
    ROS2RobotInterfaceConfig,
)

config = ROS2RobotInterfaceConfig(
    joint_states_topic="/joint_states",
    end_effector_pose_topic="/left_current_pose",
    end_effector_target_topic="/left_target",
)
interface = ROS2RobotInterface(config)
interface.connect()

joint_state = interface.get_joint_state()
if joint_state:
    print(joint_state["positions"])

pose = interface.left_arm_handler.get_pose()

target = Pose()
target.position.x = 0.5
target.position.z = 0.3
target.orientation.w = 1.0
interface.left_arm_handler.send_target(target)

# Stroke in hardware units (not a 0–1 percent)
interface.left_gripper_handler.send_joint_positions(0.01)

interface.send_fsm_command(FSM_HOLD)
interface.disconnect()

connect() 看到 /right_current_pose 后，双臂模式下才有 right_arm_handler / right_gripper_handler。笛卡尔 / 关节 / 夹爪话题（含 */twist、*/relative、target_percent 以及腰部 waist_*），以及 execute_*_action 与 execute_path：ros2_robot_interface（话题 / Action / Service ↔ API 对照）。

结合 Viser 可视化¶

从 fa-py-libraries 启动 Viser（./run.sh viser）。这是主入口；不要从 lerobot_ros2 启动，也不要把 pip install ros2-viser 当成产品启动方式。

cd ~/fa-py-libraries
./run.sh viser

ros2_viser 是该命令的库依赖。在自己的脚本里嵌入 ROS2ViserVisualizer 见 ros2-viser。

FSM 命令¶

/fsm_command 是 std_msgs/Int32（不是 stand / walk 这类字符串）。用 send_fsm_command 发送整数（1 HOME、2 HOLD、3 OCS2、4 MOVEJ、5 COMPLIANCE）。取值与合法转换取决于正在运行的控制器。见状态机与话题。

WBC 的身体 / 手臂 / 底盘字符串经 send_mode_command 发到 /mode_command，与 /fsm_command 不是同一条话题。该路径需要 ocs2_wbc_controller 以及全身启动/配置——默认 mock / 分体演示并不表示它可用。

带参数的 MoveL / MoveC / MoveJ 用 execute_movel_action、execute_movec_action_* 和 execute_joint_trajectory_action 等待结果。双臂笛卡尔路径走 execute_path 服务（不是已弃用的 /target_path 话题）。

Full method list¶

方法签名、到位检查与 Action 见 API_REFERENCE.md。

故障排除¶

连接失败¶

确认机器人演示正在运行
确认 ROS 2 的 Domain ID 一致
确认话题正在发布：ros2 topic list
首条消息到来前，get_joint_state() / get_pose() 返回 None；若从未调用 connect()，下发方法会抛出 ROS2NotConnectedError

运动指令被忽略¶

Check the controller FSM (get_fsm_state() / ros2 topic echo /fsm_state)
位姿目标需要 OCS2；关节目标需要 MOVEJ（接口可替你切换）
确认目标在关节限位范围内

导入错误¶

cd ~/fa-py-libraries
./init.sh all