一、概述

本文档涵盖机器人 ROS2 接口 的完整定义，包括升降机构、头部关节、机械臂和力传感器的 全部 Topic 及对应的 消息类型。

大部分关节驱动程序和 ROS2 节点运行于 机器人小脑（嵌入式实时控制器）上， 上位机通过 ROS2 Topic 发布控制指令并订阅状态数据，实现与机器人本体的实时通信。

通信采用 ROS2 标准的 发布/订阅（Publisher / Subscriber） 模式， 所有 Topic 均位于 /fd_robot/ 命名空间下，便于权限管理和多机器人区分。

二、升降机构 ROS2 接口

2.1 控制模式

2.2 Topic 列表

2.3 Python Demo

速度控制

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

node = Node("lift_demo")
pub = node.create_publisher(Float32, "/fd_robot/lifting_motor/velocity_cmd", 1)
msg = Float32()
msg.data = 0.05  # 0.05 m/s
pub.publish(msg)
node.get_logger().info("发布速度指令: 0.05 m/s")

位置控制（设置速度 + 目标位置）

from std_msgs.msg import Float32, String, Empty

# 设置原点
status_pub = node.create_publisher(String, "/fd_robot/lifting_motor/status", 1)
status_msg = String()
status_msg.data = "set_oring"
status_pub.publish(status_msg)

# 设置位置模式速度
speed_pub = node.create_publisher(Float32, "/fd_robot/lifting_motor/position_set_speed_cmd", 1)
speed_msg = Float32()
speed_msg.data = 0.05
speed_pub.publish(speed_msg)

# 移动到 0.3m
pos_pub = node.create_publisher(Float32, "/fd_robot/lifting_motor/position_cmd", 1)
pos_msg = Float32()
pos_msg.data = 0.3
pos_pub.publish(pos_msg)

停止 / 清除故障

# 停止
stop_msg = String()
stop_msg.data = "motor_stop"
status_pub.publish(stop_msg)

# 清除编码器故障
clear_msg = String()
clear_msg.data = "encoder_exception_clear"
status_pub.publish(clear_msg)

订阅当前位置和电机信息

def pose_cb(msg):
    print(f"当前位置: {msg.data:.3f} m")

node.create_subscription(Float32, "/fd_robot/lifting_motor/current_pose", pose_cb, 10)

from fd_msgs.msg import MotorInfoMsg
def info_cb(msg):
    if msg.motor_num > 0 and msg.pose:
        print(f"电机: {msg.motor_name[0]}, 位置: {msg.pose[0]:.3f}")

node.create_subscription(MotorInfoMsg, "/fd_robot/lifting_motor/motor_info", info_cb, 5)

rclpy.spin_once(node, timeout_sec=0.5)

2.4 CLI 示例

# 速度控制 0.05m/s
ros2 topic pub /fd_robot/lifting_motor/velocity_cmd std_msgs/msg/Float32 "{data: 0.05}" --once

# 设置原点
ros2 topic pub /fd_robot/lifting_motor/status std_msgs/msg/String "{data: 'set_oring'}" --once

# 移动至 0.2m
ros2 topic pub /fd_robot/lifting_motor/position_set_speed_cmd std_msgs/msg/Float32 "{data: 0.05}" --once
ros2 topic pub /fd_robot/lifting_motor/position_cmd std_msgs/msg/Float32 "{data: 0.2}" --once

# 停止
ros2 topic pub /fd_robot/lifting_motor/status std_msgs/msg/String "{data: 'motor_stop'}" --once

2.5 FAQ

三、头部关节 ROS2 接口

3.1 后端类型

3.2 通用话题

3.3 Python Demo (通用话题)

from sensor_msgs.msg import JointState

head_node = Node("head_demo")

# 发布关节控制指令
cmd_pub = head_node.create_publisher(JointState, "/fd_robot/head_motor/cmd", 10)
cmd_msg = JointState()
cmd_msg.name = ["lower", "upper"]
cmd_msg.position = [0.0, 0.1745]  # rad
cmd_pub.publish(cmd_msg)

# 或使用角度快捷话题
from std_msgs.msg import Float32
up_pub = head_node.create_publisher(Float32, "/fd_robot/head_up_move", 10)
down_pub = head_node.create_publisher(Float32, "/fd_robot/head_down_move", 10)

up_pub.publish(Float32(data=10.0))    # 上头 10°
down_pub.publish(Float32(data=-15.0)) # 下头 -15°

# 订阅当前位姿
def pose_cb(msg):
    for name, pos in zip(msg.name, msg.position):
        print(f"{name}: {pos:.3f} rad")

head_node.create_subscription(JointState, "/fd_robot/head/current_pose", pose_cb, 10)

3.4 DM 电机私有话题（fd_* 机器人）

3.5 Python Demo (DM 电机)

from fd_msgs.msg import EnableMsg, DmUtilControlMsg, DmMITControlMsg

dm_node = Node("dm_demo")

# 使能 1 号电机
enable_pub = dm_node.create_publisher(EnableMsg, "/fd_robot/dm_motor/enable_cmd", 10)
enable_pub.publish(EnableMsg(servo_id=1, enable=1))

# MIT 模式控制
mit_pub = dm_node.create_publisher(DmMITControlMsg, "/fd_robot/dm_motor/mit_cmd", 10)
mit_msg = DmMITControlMsg()
mit_msg.servo_id = 1
mit_msg.pose = 0.1
mit_msg.vel = 0.0
mit_msg.kp = 5.0
mit_msg.kd = 0.15
mit_msg.torque = 0.0
mit_pub.publish(mit_msg)

3.6 CLI 示例

# 读取当前头部位姿
ros2 topic echo /fd_robot/head/current_pose

# 控制下头关节角度
ros2 topic pub -1 /fd_robot/head_motor/cmd sensor_msgs/msg/JointState "{name: ['lower'], position: [0.0]}"

# 上头 pitch 10°
ros2 topic pub -1 /fd_robot/head_up_move std_msgs/msg/Float32 "{data: 10.0}"

# 下头 yaw -15°
ros2 topic pub -1 /fd_robot/head_down_move std_msgs/msg/Float32 "{data: -15.0}"

3.7 FAQ

四、机械臂 ROS2 接口

4.1 关节轨迹控制

4.2 关节定义

共 14 个关节，按左右臂分组：

4.3 Python Demo

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

arm_node = Node("arm_traj_demo")
traj_pub = arm_node.create_publisher(JointTrajectory, "/tl_driver/joint_trajectory", 10)

traj_msg = JointTrajectory()
traj_msg.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"
]

point = JointTrajectoryPoint()
point.positions = [0.0, -0.5, -1.0, -1.5, 0.0, 0.5, 0.0]
point.time_from_start.sec = 2
point.time_from_start.nanosec = 0
traj_msg.points = [point]

traj_pub.publish(traj_msg)

4.4 CLI 示例

# 查看机械臂当前轨迹话题
ros2 topic list | grep trajectory

# 发送关节轨迹（示例：左臂关节1 至 0.5 rad）
ros2 topic pub /tl_driver/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
  joint_names: ['left_tl_robot_joint1'],
  points: [{ positions: [0.5], time_from_start: { sec: 1, nanosec: 0 } }]
}" --once

五、力传感器 ROS2 接口

5.1 六维力传感器

消息字段说明

5.2 拉压力传感器

消息字段说明

5.3 Python Demo

from fd_msgs.msg import SixAixForceSensorMsg, ForceSensorMsg

force_node = Node("force_demo")

def six_axis_cb(msg):
    print(f"左: F=({msg.left_forces[0]:.1f}, {msg.left_forces[1]:.1f}, {msg.left_forces[2]:.1f}) N")
    print(f"     T=({msg.left_forces[3]:.1f}, {msg.left_forces[4]:.1f}, {msg.left_forces[5]:.1f}) Nm")
    print(f"左状态: {'正常' if msg.left_status == 0 else '未连接'}")

force_node.create_subscription(SixAixForceSensorMsg, 
    \"/fd_robot/force_sensor/six_aix_force_sensor\", six_axis_cb, 10)

def pressure_cb(msg):
    for name, force, status in zip(msg.sensor_name, msg.forces, msg.status):
        print(f\"{name}: {force:.1f} N, 状态={'正常' if status == 0 else '异常'}\")

force_node.create_subscription(ForceSensorMsg,
    \"/fd_robot/force_sensor/pressure_sensor\", pressure_cb, 10)

5.4 CLI 示例

# 查看六维力传感器数据
ros2 topic echo /fd_robot/force_sensor/six_aix_force_sensor

# 查看拉压力传感器数据
ros2 topic echo /fd_robot/force_sensor/pressure_sensor

六、注意事项