add estop function

Reviewed-on: #1

src/CMakeLists.txt

@@ -20,6 +20,7 @@ set(msg_files
   msg/PoseArrayClassAndID.msg
   msg/PoseClassAndID.msg
   msg/RobotWorkInfo.msg
+  msg/RobotArmStatus.msg
   msg/DistMsg.msg
   msg/InputMsg.msg
   msg/ImuMsg.msg
@@ -29,6 +30,9 @@ set(msg_files
   msg/SkillCall.msg
   msg/MetaKey.msg
   msg/ArmMotionParams.msg
+  msg/ArmError.msg
+  msg/GripperStatus.msg
+  msg/EStopState.msg
 )
 set(action_files
   action/MoveToPosition.action
@@ -55,13 +59,19 @@ set(srv_files
   srv/MotorParam.srv
   srv/GripperCmd.srv
   srv/MotorInfo.srv
+  srv/ClearArmError.srv
+  srv/InverseKinematics.srv
+  srv/SetArmEnable.srv
+  srv/ResetEStop.srv
 )
 
-include_directories(include)
-ament_export_include_directories(include)
-install(DIRECTORY include/
-  DESTINATION include/interfaces
-)
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include")
+  include_directories(include)
+  ament_export_include_directories(include)
+  install(DIRECTORY include/
+    DESTINATION include/interfaces
+  )
+endif()
 
 # uncomment the following section in order to fill in
 # further dependencies manually.

src/action/Arm.action

@@ -1,17 +1,20 @@
-int8 body_id
-int8 data_type
-int16 data_length
-int64 command_id
-int64 frame_time_stamp
-float64[] data_array
+# Action Goal: 机械臂控制指令
+int8 body_id          # 身体部位ID (左臂=0, 右臂=1， 双臂=2)
+int8 data_type        # 数据类型标识 (2-角度控制，对应单臂的数据长度为6，双臂为12， 数组填充6个关节的角度值； 3-位姿控制，对应单臂的数据长度为7，双臂为14， 数组填充手臂的末端位姿)
+int16 data_length     # 数据长度 (角度控制，对应单臂的数据长度为6，双臂为12；位姿控制，对应单臂的数据长度为7，双臂为14)
+int64 command_id      # 指令ID，用于唯一标识该条指令
+int64 frame_time_stamp # 时间戳 (单位: 纳秒/微秒，视系统定义而定)
+float64[] data_array  # 数据数组 (角度控制，对应单臂的数据长度为6，双臂为12， 数组填充6个关节的角度值； 位姿控制，对应单臂的数据长度为7，双臂为14， 数组填充手臂的末端位姿)
 ---
-int32 result
-int64 command_id
-int64 end_time
-geometry_msgs/Pose pose
+# Action Result: 执行结果
+int32 result          # 执行结果状态码 (0=成功, 非0=错误码)
+int64 command_id      # 对应的指令ID
+int64 end_time        # 执行结束时间戳
+geometry_msgs/Pose pose # 最终机械臂末端姿态
 ---
-int64 command_id
-int16 int_lenth
-int16 float_length
-int32[] int_data_array
-float64[] float_data_array
+# Action Feedback: 执行反馈
+int64 command_id      # 正在执行的指令ID
+int16 int_lenth       # 整数数据长度
+int16 float_length    # 浮点数数据长度
+int32[] int_data_array # 整数反馈数据数组 (例如: 状态标志位)
+float64[] float_data_array # 浮点数反馈数据数组 (例如: 当前关节角度, 实时进度)

src/action/ArmSpaceControl.action

@@ -1,10 +1,10 @@
-# Goal: target pose in space
-# Example fields; adjust as needed for your robot
-geometry_msgs/PoseStamped target
+# Action Goal: 机械臂空间位置控制
+# 目标姿态，包含位置(x,y,z)和方向(quaternion)
+geometry_msgs/PoseStamped target # 机械臂末端目标位姿 (Frame ID + Pose)
 ---
-# Result
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 是否成功到达目标 (true=成功, false=失败)
+string message  # 结果描述信息 (例如: "Target reached", "Planning failed")
 ---
-# Feedback
-float32 progress
+# Action Feedback: 执行进度
+float32 progress # 执行进度百分比 (0.0 ~ 1.0)

src/action/CameraTakePhoto.action

@@ -1,8 +1,10 @@
-# Goal: hand control parameters
-int32 mode  # 0=open, 1=close
-float32 effort
+# Action Goal: 相机拍照控制
+int32 mode  # 拍照模式 (例如: 0=单拍, 1=连拍, 2=录像)
+float32 effort # 力度/强度参数 (保留字段，可能用于变焦或光圈控制) [0.0 - 1.0]
 ---
-bool success
-string message
+# Action Result: 拍照结果
+bool success    # 操作是否成功
+string message  # 结果信息 (例如: "Photo saved to /tmp/img.jpg")
 ---
-float32 progress
+# Action Feedback: 执行进度
+float32 progress # 执行进度 (0.0 ~ 1.0)

src/action/DualArm.action

@@ -1,29 +1,25 @@
 # DualArm.action 
 
-uint8 STATUS_PLANNING = 0   # 规划中
-uint8 STATUS_EXECUTING = 1  # 执行中
-uint8 STATUS_DONE = 2       # 完成
+# 状态常量定义
+uint8 STATUS_PLANNING = 0   # 状态: 规划中
+uint8 STATUS_EXECUTING = 1  # 状态: 执行中
+uint8 STATUS_DONE = 2       # 状态: 完成
 
-uint8 ARM_LEFT = 0    # 左臂
-uint8 ARM_RIGHT = 1   # 右臂
-uint8 ARM_DUAL = 2    # 双臂
-
-uint8 arm_type                       # 运动臂类型 | ARM_LEFT/ARM_RIGHT/ARM_DUAL
-interfaces/ArmMotionParams[] arm_motion_params   # 每条手臂的运动参数
-float64 velocity_scaling             # 速度缩放因子  [0,1] 
-float64 acceleration_scaling         # 加速度缩放因子 [0,1] 
-float64 cartesian_step               # MOVEL步长 m [0,0.5] 
+# Action Goal: 双臂控制目标
+interfaces/ArmMotionParams[] arm_motion_params   # 每条手臂的运动参数 (包含目标位姿、轨迹等)
+int32 velocity_scaling               # 速度百分比系数  [1, 100] (100表示全速)
 
 ---
 
-bool success                         # 执行是否成功
-string message                       # 结果描述
-float64 final_progress               # 最终进度  [0,1]
+# Action Result: 执行结果
+bool success                         # 执行是否成功 (true=成功, false=失败)
+string message                       # 结果描述信息
+float64 final_progress               # 最终进度  [0.0, 1.0]
 
 ---
 
-float64 progress                     # 实时进度  [0,1]
-uint8 status                         # 实时状态  STATUS_PLANNING/STATUS_EXECUTING/STATUS_DONE
-float64[] joints_left                # 左臂当前关节角（弧度）
-float64[] joints_right               # 右臂当前关节角（弧度）
-
+# Action Feedback: 实时反馈
+float64 progress                     # 实时总体进度  [0.0, 1.0]
+uint8 status                         # 实时状态 (0:规划中, 1:执行中, 2:完成)
+float64[] joints_left                # 左臂当前关节角 [单位: 弧度]
+float64[] joints_right               # 右臂当前关节角 [单位: 弧度]

src/action/ExecuteBtAction.action

@@ -1,47 +1,24 @@
-###############################################################
 # ExecuteBtAction
-#
-# Goal:
-#   action_name: A skill sequence or single skill name. For a single
-#                skill this is typically the canonical CamelCase name
-#                (e.g. "ArmSpaceControl"). Multiple skills may be
-#                separated by commas/semicolons/spaces.
-#
-# Result:
-#   success:     Overall success flag for the entire (possibly multi)
-#                skill sequence.
-#   message:     Human readable summary (e.g. "Seq{A,B}: A(OK), B(FAIL)[detail]").
-#   total_skills:How many skills were requested (after parsing).
-#   succeeded_skills: Number of skills that finished with success.
-#
-# Feedback (streamed):
-#   stage:        High‑level stage label: START | WAIT | RUN | END.
-#   current_skill:Name of the skill currently being processed.
-#   progress:     Float in [0,1]. START of first skill => 0.0, END of last
-#                 skill => 1.0. Intermediate values are proportional to
-#                 (completed_skills + stage_offset)/total_skills where
-#                 stage_offset is 0 for START/WAIT/RUN and 1 for END of a skill.
-#   detail:       Optional detail (e.g. retry attempt, OK/FAIL, waiting cause).
-###############################################################
+# 行为树执行 Action
+# 用于执行单个或序列化的机器人技能(Skills)
 
-# Goal
-# epoch: Sequence epoch (monotonically increasing id assigned by Cerebrum). Used so the
-#        server can reject stale goals and stop publishing feedback from older epochs.
-uint32 epoch
-string action_name
-interfaces/SkillCall[] calls
+# Action Goal: 执行目标
+# epoch: 序列纪元ID (由大脑分配的单调递增ID)，用于防止执行过期的目标
+uint32 epoch        # 纪元ID
+string action_name  # 动作名称 (单技能如 "ArmSpaceControl", 或多技能组合)
+interfaces/SkillCall[] calls # 技能调用参数列表 (包含每个技能的具体参数)
 
 ---
-# Result
-bool success
-string message
-int32 total_skills
-int32 succeeded_skills
+# Action Result: 执行结果
+bool success        # 整体序列执行是否成功
+string message      # 人类可读的结果摘要 (例如: "Seq{A,B}: A(OK), B(FAIL)")
+int32 total_skills  # 请求执行的技能总数
+int32 succeeded_skills # 成功完成的技能数量
 
 ---
-# Feedback
-uint32 epoch
-string stage
-string current_skill
-float32 progress
-string detail
+# Action Feedback: 实时反馈
+uint32 epoch        # 当前执行的纪元ID
+string stage        # 当前高层阶段状态: START | WAIT | RUN | END
+string current_skill # 当前正在处理的技能名称
+float32 progress    # 总体进度 [0.0, 1.0]
+string detail       # 详细状态信息 (例如: 重试次数, 等待原因)

src/action/GripperCmd.action

@@ -1,11 +1,18 @@
-uint8 loc
-uint8 speed
-uint8 torque
-uint8 mode
+# GripperCmd.action
+# 夹爪控制 Action
+
+# Action Goal: 控制指令
+uint8 loc     # 目标位置 (0~255， 0时夹爪完全打开；255时夹爪完全关闭)
+uint8 speed   # 动作速度 (0~255， 0时动作速度最慢；255时动作速度最快)
+uint8 torque  # 动作力矩 (0~255， 0时动作力矩最弱；255时动作力矩最强)
+uint8 mode    # 控制模式 (0=位置控制, 1=力矩控制， 2=位置+力矩控制， 默认适用模式2)
 ---
-uint8 result
-string state
+# Action Result: 执行结果
+uint8 result  # 结果状态码 (1=成功, 其他=错误)
+string state  # 最终状态描述 (例如: "手指已到达指定的位置，没有检测到物体", "手指在闭合检测到物体")
 ---
-uint8 loc
-uint8 speed
-uint8 torque
+# Action Feedback: 实时状态
+uint8 loc     # 当前实际位置 (0~255， 0时夹爪完全打开；255时夹爪完全关闭)
+uint8 speed   # 当前实际速度 (0~255， 0时动作速度最慢；255时动作速度最快)
+uint8 torque  # 当前实际力矩 (0~255， 0时动作力矩最弱；255时动作力矩最强)
+string state  # 当前状态描述 (例如: "手指已到达指定的位置，没有检测到物体", "手指在闭合检测到物体")

src/action/HandControl.action

@@ -1,8 +1,13 @@
-# Goal: hand control parameters
-int32 mode  # 0=open, 1=close
-float32 effort
+# HandControl.action
+# 灵巧手/机械手控制 Action
+
+# Action Goal: 目标参数
+int32 mode  # 控制模式 (0=张开/Open, 1=闭合/Close, 其他自定义)
+float32 effort # 力度/抓取力 [0.0 - 1.0]
 ---
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 是否成功
+string message  # 结果信息
 ---
-float32 progress
+# Action Feedback: 执行进度
+float32 progress # 动作完成进度 (0.0 - 1.0)

src/action/LegControl.action

@@ -1,7 +1,12 @@
-# Goal: leg control parameters
-geometry_msgs/TwistStamped target
+# LegControl.action
+# 腿部控制 Action
+
+# Action Goal: 腿部运动目标
+geometry_msgs/TwistStamped target # 目标速度/位姿 (通常用于控制行走速度或身体姿态)
 ---
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 是否成功
+string message  # 结果描述
 ---
-float32 progress
+# Action Feedback: 执行进度
+float32 progress # 进度 (0.0 - 1.0)

src/action/MoveHome.action

@@ -1,8 +1,12 @@
-# 目标 回零点
+# MoveHome.action
+# 机器人回零点 Action
+
+# Action Goal: 目标
+# 无特定目标参数，请求即代表回零
 ---
-# 结果：运动成功，或执行完毕
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 运动是否成功 / 是否执行完毕
+string message  # 结果描述信息
 ---
-# 反馈：各关节位置，运动进度
-int64[] joint_values
+# Action Feedback: 实时反馈
+int64[] joint_values # 各关节当前位置值 (数组)

src/action/MoveLeg.action

@@ -1,9 +1,12 @@
-# 目标 腿伸长或缩短运动
-float32 move_up_distance
+# MoveLeg.action
+# 腿部升降/伸缩控制 Action
+
+# Action Goal: 目标
+float32 move_up_distance # 腿部向上/向下移动距离 [单位: m] (正值向上，负值向下)
 ---
-# 结果：运动成功，或执行完毕
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 运动是否成功 / 是否执行完毕
+string message  # 结果描述信息
 ---
-# 反馈：各关节角度
-int64[] joint_values
+# Action Feedback: 实时反馈
+int64[] joint_values # 各关节当前角度/位置值

src/action/MoveToPosition.action

@@ -1,12 +1,25 @@
-# Goal definition
-float32 target_x
-float32 target_y
+# MoveToPosition.action
+# 移动到底盘指定位置 Action
+
+# Action Goal: 移动目标 (笛卡尔空间 + 欧拉角方向)
+float32 target_x            # 目标位置 X轴坐标 (世界坐标系) [单位: m]
+float32 target_y            # 目标位置 Y轴坐标 (世界坐标系) [单位: m]
+float32 target_z            # 目标位置 Z轴坐标 (世界坐标系) [单位: m]
+float32 target_angle_pitch  # 目标 俯仰角 (Pitch) [单位: 度 deg]
+float32 target_angle_roll   # 目标 横滚角 (Roll)  [单位: 度 deg]
+float32 target_angle_yaw    # 目标 偏航角 (Yaw)   [单位: 度 deg]
+
 ---
-# Result definition
-bool success
-string message
+# Action Result: 执行结果
+bool success                # 整体执行结果 (true=成功, false=失败)
+string message              # 结果摘要信息 (例如: "failed by some reasons")
+
 ---
-# Feedback definition
-float32 current_x
-float32 current_y
-float32 progress
+# Action Feedback: 实时反馈
+float32 current_x           # 当前位置 X轴坐标 [单位: m]
+float32 current_y           # 当前位置 Y轴坐标 [单位: m]
+float32 current_z           # 当前位置 Z轴坐标 [单位: m]
+float32 current_angle_pitch # 当前 俯仰角 [单位: 度 deg]
+float32 current_angle_roll  # 当前 横滚角 [单位: 度 deg]
+float32 current_angle_yaw   # 当前 偏航角 [单位: 度 deg]
+float32 progress            # 执行进度 [0.0 ~ 1.0] (0.0=开始, 1.0=结束)

src/action/MoveWaist.action

@@ -1,10 +1,13 @@
-# 目标 腰部运动
-float32 move_pitch_degree
-float32 move_yaw_degree
+# MoveWaist.action
+# 腰部运动控制 Action
+
+# Action Goal: 目标
+float32 move_pitch_degree # 腰部俯仰运动角度 [单位: 度 deg, 正值俯身，负值仰身 - 视坐标系定义]
+float32 move_yaw_degree   # 腰部偏航运动角度 [单位: 度 deg, 正值左转，负值右转 - 视坐标系定义]
 ---
-# 结果：运动成功，或执行完毕
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 运动是否成功 / 是否执行完毕
+string message  # 结果描述信息
 ---
-# 反馈：各关节角度
-int64[] joint_values
+# Action Feedback: 实时反馈
+int64[] joint_values # 各关节当前角度/位置值

src/action/MoveWheel.action

@@ -1,11 +1,14 @@
-# 目标 底盘运动
-float32 move_distance
-float32 move_angle
+# MoveWheel.action
+# 底盘轮子运动控制 Action
+
+# Action Goal: 目标
+float32 move_distance # 直线移动距离 [单位: m] (正值前进，负值后退)
+float32 move_angle    # 旋转角度 [单位: 度 deg] (正值左转，负值右转 - 视坐标系定义)
 ---
-# 结果：运动成功，或执行完毕
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 运动是否成功 / 是否执行完毕
+string message  # 结果描述信息
 ---
-# 反馈：当前位置，当前角度，运动进度
-float32 current_pos
-float32 current_angle
+# Action Feedback: 实时反馈
+float32 current_pos   # 当前移动距离 [单位: m]
+float32 current_angle # 当前旋转角度 [单位: 度 deg]

src/action/SlamMode.action

@@ -1,6 +1,12 @@
-bool enable_mapping
+# SlamMode.action
+# SLAM 建图模式切换 Action
+
+# Action Goal: 模式设置
+bool enable_mapping # 是否开启建图模式 (true=开启建图, false=定位模式/关闭建图)
 ---
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 切换是否成功
+string message  # 结果信息
 ---
-string state
+# Action Feedback:状态反馈
+string state    # 当前状态描述 (例如: "Mapping", "Localization Only")

src/action/VisionGraspObject.action

@@ -1,7 +1,12 @@
-# Goal: specify object id/name
-string object_id
+# VisionGraspObject.action
+# 视觉抓取物体 Action
+
+# Action Goal: 抓取目标
+string object_id # 目标物体ID或名称
 ---
-bool success
-string message
+# Action Result: 执行结果
+bool success    # 抓取是否成功
+string message  # 结果信息
 ---
-float32 grasp_progress
+# Action Feedback: 执行进度
+float32 grasp_progress # 抓取过程进度 [0.0 - 1.0]

src/include/arm_action_define.h

@@ -1,35 +0,0 @@
-#ifndef ARM_ACTION_DEFINE_H
-#define ARM_ACTION_DEFINE_H
-
-#include "err_code.h"
-
-#define USED_ARM_DOF                (6)
-#define GOAL_DATA_LENGTH            (7)
-#define USED_OTHER_DOF              (17)
-
-typedef enum {
-    ARM_COMMAND_TYPE_ANGLE_STEP_ON = 0,
-    ARM_COMMAND_TYPE_POSE_STEP_ON,
-    ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE,
-    ARM_COMMAND_TYPE_POSE_DIRECT_MOVE,
-    ARM_COMMAND_TYPE_ERR
-} ArmCommandTypeE;
-
-typedef enum {
-    POSE_POSITION_X = 0,
-    POSE_POSITION_Y,
-    POSE_POSITION_Z,
-    POSE_QUATERNION_X,
-    POSE_QUATERNION_Y,
-    POSE_QUATERNION_Z,
-    POSE_QUATERNION_W,
-    POSE_DIMENSION, // should be 7
-} PoseDimensionE;
-
-typedef enum {
-    LEFT_ARM = 0,
-    RIGHT_ARM,
-    ARM_ID_ERR
-} ArmIdE;
-
-#endif // ARM_ACTION_DEFINE_H

src/include/err_code.h

@@ -1,14 +0,0 @@
-#ifndef ERR_CODE_H
-#define ERR_CODE_H
-
-typedef enum {
-    OK = 0,
-    UNKNOWN_ERR = -1,
-    ARM_NOW_FORCE_MOVING = -2,
-    ARM_COLLISION = -3,
-    ARM_AIM_CANNOT_REACH = -4,
-    ARM_NOW_NO_GOAL = -5,
-    ARM_GOAL_CANCELLED = -6,
-} ErrCodeE;
-
-#endif // ERR_CODE_H

src/msg/ArmError.msg

@@ -0,0 +1,6 @@
+# ArmError.msg
+# 机械臂错误状态消息
+
+int8 arm_id           # 机械臂ID (0=左臂, 1=右臂, 2=双臂)
+uint16[] err_flag     # 错误标志位数组 (每一位代表一种特定的错误类型，如过流、过热、通信丢失等)
+uint16[] brake_state  # 抱闸/制动器状态数组 (例如: 1=抱死, 0=松开)

src/msg/ArmMotionParams.msg

@@ -1,13 +1,16 @@
 # ArmMotionParams.msg - 单臂运动参数
+# 用于描述单条机械臂的运动方式和目标
 
-uint8 MOVEJ = 0
-uint8 MOVEL = 1
+# 运动类型常量
+uint8 MOVEJ = 0       # 关节空间运动 (Move Joint)
+uint8 MOVEL = 1       # 笛卡尔空间直线运动 (Move Linear)
 
-uint8 ARM_LEFT = 0
-uint8 ARM_RIGHT = 1
+# 机械臂ID常量
+uint8 ARM_LEFT = 0    # 左臂
+uint8 ARM_RIGHT = 1   # 右臂
 
 uint8 arm_id                  # 机械臂ID 0: 左臂, 1: 右臂
 uint8 motion_type             # 运动模式: MOVEJ / MOVEL
-geometry_msgs/Pose target_pose    # 笛卡尔目标位姿 (仅 MOVEL 使用)
-float64[] target_joints           # 目标关节角(rad) (仅 MOVEJ 使用)
-
+geometry_msgs/Pose target_pose    # 笛卡尔目标位姿 (仅 MOVEL 使用) [位置m, 姿态quaternion]
+float64[] target_joints           # 目标关节角 (仅 MOVEJ 使用) [单位: 弧度 rad]
+int32 blend_radius                # 轨迹交融半径百分比系数 [0, 100] (0表示不启用blending，>0表示启用轨迹混合)

src/msg/DistMsg.msg

@@ -1,5 +1,8 @@
-string source
-string type
-string position
-int32[] ranges
-sensor_msgs/PointCloud2 points
+# DistMsg.msg
+# 距离传感/点云数据消息(简化版)
+
+string source         # 数据来源设备ID
+string type           # 数据类型描述
+string position       # 传感器安装位置
+int32[] ranges        # 距离测量值数组 [单位: mm 或 cm，视具体传感器而定]
+sensor_msgs/PointCloud2 points # 对应的点云数据 (如有)

src/msg/EStopState.msg

@@ -0,0 +1,12 @@
+# EStopState.msg
+# 急停状态广播消息（由HAL/驱动层向上游发布）
+#
+# 语义：
+# - latched=true 表示急停已触发并处于锁存态
+# - generation 每次“新触发事件”递增，便于上层检测边沿
+
+builtin_interfaces/Time stamp  # 状态时间戳
+bool latched                         # 急停锁存状态
+uint8 source                         # 触发源（0=unknown,1=io,2=plc,3=driver_fault,4=software）
+string reason                        # 触发原因文本
+uint32 generation                    # 触发代次（单调递增）

src/msg/GripperStatus.msg

@@ -0,0 +1,8 @@
+# GripperStatus.msg
+# 夹爪状态消息
+
+std_msgs/Header header # 消息头 (包含时间戳和坐标系ID)
+float32 loc            # 当前位置 (0~255， 0时夹爪完全打开；255时夹爪完全关闭)
+float32 speed          # 当前速度 (0~255， 0时动作速度最慢；255时动作速度最快)
+float32 torque         # 当前力矩 (0~255， 0时动作力矩最弱；255时动作力矩最强)
+string state           # 状态描述字符串 (例如: "手指已到达指定的位置，没有检测到物体", "手指在闭合检测到物体")

src/msg/ImgMsg.msg

@@ -1,7 +1,10 @@
-sensor_msgs/Image image_depth
-sensor_msgs/Image image_color
-float64[] karr
-float64[] darr
-string source
-string position
-string type
+# ImgMsg.msg
+# 图像及相机参数消息
+
+sensor_msgs/Image image_depth # 深度图像
+sensor_msgs/Image image_color # 彩色图像
+float64[] karr                # 相机内参矩阵 K (通常为 flattened 3x3 matrix)
+float64[] darr                # 畸变系数 D (通常为 5 或 8 个参数)
+string source                 # 图像来源相机ID
+string position               # 相机安装位置
+string type                   # 图像类型描述

src/msg/ImuMsg.msg

@@ -1,4 +1,7 @@
-string source
-string type
-string position
-sensor_msgs/Imu imu
+# ImuMsg.msg
+# IMU 传感器消息封装
+
+string source     # 数据来源IMU ID
+string type       # 类型描述
+string position   # IMU 安装位置
+sensor_msgs/Imu imu # 标准ROS IMU消息 (包含角速度、线加速度、姿态)

src/msg/InputMsg.msg

@@ -1,5 +1,8 @@
-string source
-string type
-string position
-int32 code
-float32 value
+# InputMsg.msg
+# 通用输入消息 (如按键、模拟量输入)
+
+string source   # 输入源名称
+string type     # 输入类型
+string position # 输入设备位置
+int32 code      # 输入码值 (Key code等)
+float32 value   # 输入模拟值

src/msg/MetaKey.msg

@@ -1,10 +1,13 @@
-string source
-string type
-string position
-int8 a
-int8 b
-int8 sk
-int8 trigger
-int8 sk_x
-int8 sk_y
-geometry_msgs/Pose pose
+# MetaKey.msg
+# 元数据/按键事件消息
+
+string source   # 来源
+string type     # 类型
+string position # 位置
+int8 a         # 按钮 A 状态
+int8 b         # 按钮 B 状态
+int8 sk        # 特殊功能键状态
+int8 trigger   # 扳机键状态
+int8 sk_x      # 摇杆 X轴数值
+int8 sk_y      # 摇杆 Y轴数值
+geometry_msgs/Pose pose # 对应的手柄/控制器位姿

src/msg/MotorCmd.msg

@@ -1,6 +1,9 @@
-string target
-string type
-string position
-int32[] motor_id
-float32[] motor_angle
-float32[] motor_speed
+# MotorCmd.msg
+# 电机控制指令消息
+
+string target          # 目标设备ID
+string type            # 控制类型 (位置/速度/力矩)
+string position        # 电机位置标识
+int32[] motor_id       # 电机ID列表
+float32[] motor_angle  # 目标角度列表 [deg]
+float32[] motor_speed  # 目标速度列表 [rpm or deg/s]

src/msg/MotorPos.msg

@@ -1,6 +1,9 @@
-string source
-string type
-string position
-int32[] motor_id
-float32[] motor_angle
-float32[] motor_speed
+# MotorPos.msg
+# 电机实时位置/状态反馈消息
+
+string source          # 数据来源ID
+string type            # 数据类型
+string position        # 位置标识
+int32[] motor_id       # 电机ID列表
+float32[] motor_angle  # 当前角度列表
+float32[] motor_speed  # 当前速度列表

src/msg/PoseArrayClassAndID.msg

@@ -1,2 +1,5 @@
-std_msgs/Header header
-PoseClassAndID[] objects
+# PoseArrayClassAndID.msg
+# 带类别和ID的位姿数组消息
+
+std_msgs/Header header         # 消息头
+interfaces/PoseClassAndID[] objects # 物体列表

src/msg/PoseClassAndID.msg

@@ -1,5 +1,7 @@
-string class_name
-int32 class_id
-geometry_msgs/Pose pose
-float64[] grab_width
+# PoseClassAndID.msg
+# 单个物体的类别、ID和位姿描述
 
+string class_name      # 类别名称
+int32 class_id         # 类别ID
+geometry_msgs/Pose pose # 物体位姿
+float64[] grab_width   # 抓取宽度/尺寸信息

src/msg/RobotArmStatus.msg

@@ -0,0 +1,9 @@
+builtin_interfaces/Time stamp
+string[] joint_names
+float64[] joint_positions
+bool[] joint_enabled
+int32[] joint_error_codes
+bool estop_latched
+uint32 estop_generation
+geometry_msgs/Pose left_arm_pose
+geometry_msgs/Pose right_arm_pose

src/msg/RobotWorkInfo.msg

@@ -1,14 +1,18 @@
-int64     msg_id
-string    working_state        #[Standby, Working, Fault]
-int8      battery_capacity     #[0-100]
-float32   working_time         #1.5 (h)
-string    nav_state            #[Normal, Fault]
-string    comm_quality         #[Excellent, Good, Fair, Poor, NoSignal]
-string[]  task                 #[last_task, next_task, current_task], [StockIn, StockOut, None]
-string    expt_completion_time ##2025-11-11 12:34:56
-string    work_log             #
-string    skill                #MoveWaist
-string    action_name          #s1_waist_bend_down
-string    instance_params      #
-string    bt_node_status       #[IDLE, RUNNING, SUCCESS, FAILURE, SKIPPED]
-string    smacc_state          #[StIdle, StExecuting, StCompleted, StFailed, StEmergency]
+# RobotWorkInfo.msg
+# 机器人工作状态汇总信息
+
+int64     msg_id               # 消息序列号
+builtin_interfaces/Time stamp  # 时间戳
+string    working_state        # 工作状态 [Standby, Working, Fault]
+int8      battery_capacity     # 电池电量 [0-100] %
+float32   working_time         # 累计工作时间 [h] (例如 1.5)
+string    nav_state            # 导航模块状态 [Normal, Fault]
+string    comm_quality         # 通信质量 [Excellent, Good, Fair, Poor, NoSignal]
+string[]  task                 # 任务队列信息 [last_task, next_task, current_task] (例如: StockIn, StockOut, None)
+string    expt_completion_time # 预计完成时间 (格式: 2025-11-11 12:34:56)
+string    work_log             # 工作日志摘要
+string    skill                # 当前执行的技能 (例如: MoveWaist)
+string    action_name          # 当前动作名称 (例如: s1_waist_bend_down)
+string    instance_params      # 实例参数
+string    bt_node_status       # 行为树节点状态 [IDLE, RUNNING, SUCCESS, FAILURE, SKIPPED]
+string    smacc_state          # 状态机状态 [StIdle, StExecuting, StCompleted, StFailed, StEmergency]

src/msg/SkillCall.msg

@@ -1,8 +1,10 @@
-# Generic skill call descriptor for ExecuteBtAction (scheme A)
-string name           # Skill name, e.g., "MoveWaist" / "MoveLeg"
-string interface_type # e.g., "interfaces/action/MoveWaist" or "interfaces/srv/VisionObjectRecognition"
-string call_kind      # "action" | "srv"
-string topic          # optional topic/service override; empty => use defaults/parameters
-string payload_yaml   # YAML string of Goal/Request fields; may be empty for default goal
-string instance_name  # optional instance name; empty => use default
-int32 timeout_ms   # optional timeout; 0 => use default
+# SkillCall.msg
+# 技能调用描述符 (用于 ExecuteBtAction)
+
+string name           # 技能名称, 例如 "MoveWaist" / "MoveLeg"
+string interface_type # 接口类型描述, 例如 "interfaces/action/MoveWaist" 或 "interfaces/srv/VisionObjectRecognition"
+string call_kind      # 调用类型: "action" | "srv"
+string topic          # (可选) 话题/服务名称覆盖; 为空则使用默认值
+string payload_yaml   # Goal/Request 字段的 YAML 格式字符串; 为空则使用默认目标
+string instance_name  # (可选) 实例名称; 为空则使用默认
+int32 timeout_ms      # (可选) 超时时间(ms); 0 表示使用默认值

src/package.xml

@@ -11,6 +11,8 @@
   <build_depend>rosidl_default_generators</build_depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>builtin_interfaces</depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/srv/BtRebuild.srv

@@ -1,6 +1,9 @@
-string type     #rebuild type: "Trigger", "Local", "Remote"
-string config
-string param
+# BtRebuild.srv
+# 行为树重构服务
+
+string type     # 重构类型: "Trigger"-触发特定的行为树xml序列, "Local"-预留未使用, "Remote"-发送config/params，生成新的行为树序列
+string config   # 动作序列
+string param    # 附加参数
 ---
-bool success
-string message
+bool success    # 是否成功
+string message  # 结果信息

src/srv/ClearArmError.srv

@@ -0,0 +1,8 @@
+# ClearArmError.srv
+# 清除机械臂错误服务
+
+int8 arm_id     # 机械臂ID (例如: 0=左臂, 1=右臂, 2=双臂)
+int8 joint_num  # 关节编号 (1~7表示第1个到第7个关节, 0表示所有关节)
+---
+bool success    # 操作是否成功
+string message  # 结果信息

src/srv/GripperCmd.srv

@@ -1,6 +1,9 @@
-uint8 devid
-uint8 loc
-uint8 speed
-uint8 torque
+# GripperCmd.srv
+# 夹爪控制服务 (非Action版)
+
+uint8 devid   # 设备ID
+uint8 loc     # 目标位置 (0~255， 0时夹爪完全打开；255时夹爪完全关闭)
+uint8 speed   # 动作速度 (0~255， 0时动作速度最慢；255时动作速度最快)
+uint8 torque  # 动作力矩 (0~255， 0时动作力矩最弱；255时动作力矩最强)
 ---
-string state
+string state  # 最终状态描述

src/srv/InverseKinematics.srv

@@ -0,0 +1,5 @@
+int32 arm_id                    #0-左臂，1-右臂
+geometry_msgs/Pose pose         #目标位姿
+---
+int32 result                    #结果：0-成功，非0-失败
+float32[] joint_angles          #解算出来的关节角

src/srv/MapLoad.srv

@@ -1,4 +1,7 @@
-string map_url
+# MapLoad.srv
+# 地图加载服务
+
+string map_url  # 地图文件路径或URL
 ---
-bool success
-string message
+bool success    # 加载是否成功
+string message  # 结果信息

src/srv/MapSave.srv

@@ -1,4 +1,7 @@
-string map_url
+# MapSave.srv
+# 地图保存服务
+
+string map_url  # 保存目标路径或URL
 ---
-bool success
-string message
+bool success    # 保存是否成功
+string message  # 结果信息

src/srv/MotorInfo.srv

@@ -1,6 +1,9 @@
-string target
-string type
-uint8[] motor_ids
+# MotorInfo.srv
+# 获取电机信息服务
+
+string target      # 目标设备
+string type        # 请求类型
+uint8[] motor_ids  # 请求的电机ID列表
 ---
-float32[] motor_angles
-bool ret
+float32[] motor_angles # 返回的电机角度列表
+bool ret               # 是否成功

src/srv/MotorParam.srv

@@ -1,6 +1,9 @@
-uint16 motor_id
-uint16 max_speed
-uint16 add_acc
-uint16 dec_acc
+# MotorParam.srv
+# 电机参数配置服务
+
+uint16 motor_id   # 电机ID
+uint16 max_speed  # 最大速度限制
+uint16 add_acc    # 加速度参数
+uint16 dec_acc    # 减速度参数
 ---
-int16 ret
+int16 ret         # 设置结果代码

src/srv/ResetEStop.srv

@@ -0,0 +1,11 @@
+# ResetEStop.srv
+# 急停复位服务（用于清除上层软件锁存态）
+#
+# 注意：
+# - 该服务仅表达“复位请求”，是否允许复位由安全链路策略决定
+# - 硬件安全回路未恢复时，应返回 success=false
+
+bool reset        # true=请求复位，false=无效请求
+---
+bool success      # true=复位成功，false=复位失败
+string message    # 结果描述（失败原因/诊断信息）

src/srv/SetArmEnable.srv

@@ -0,0 +1,25 @@
+# SetArmEnable.srv
+# 机械臂关节上下使能服务
+#
+# 用途：
+# - 对单关节进行使能/失能
+# - 对单臂全部关节进行使能/失能
+# - 对双臂全部关节（或同编号关节）进行使能/失能
+#
+# 参数组合说明：
+# - arm_id=0, joint_num=0: 左臂全部关节
+# - arm_id=1, joint_num=0: 右臂全部关节
+# - arm_id=2, joint_num=0: 双臂全部关节
+# - arm_id=0/1, joint_num=1~6: 指定单臂的指定关节
+# - arm_id=2, joint_num=1~6: 双臂的同编号关节（如左右第3关节）
+#
+# 注意：
+# - joint_num=0 表示“该作用范围内的全部关节”
+# - 关节编号按 1 开始计数
+
+int8 arm_id     # 机械臂ID: 0=左臂, 1=右臂, 2=双臂
+int8 joint_num  # 关节编号: 1~6=指定关节, 0=全部关节
+bool enable     # 目标状态: true=使能, false=失能
+---
+bool success    # true=操作成功, false=操作失败
+string message  # 执行结果描述（失败原因/调试信息）

src/srv/VisionObjectRecognition.srv

@@ -1,8 +1,11 @@
-string camera_position
+# VisionObjectRecognition.srv
+# 视觉物体识别服务
+
+string camera_position  # 相机位置标识 ("top": 头部相机; "left": 左臂相机; "right": 右臂相机)
 
 ---
 
-std_msgs/Header header
-string info
-bool success
-interfaces/PoseClassAndID[] objects
+std_msgs/Header header              # 消息头 (时间戳等)
+string info                         # 识别结果摘要信息
+bool success                        # 识别是否成功（true: 成功; false: 失败）
+interfaces/PoseClassAndID[] objects # 识别到的物体列表