add estop function

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
@@ -31,6 +32,7 @@ set(msg_files
   msg/ArmMotionParams.msg
   msg/ArmError.msg
   msg/GripperStatus.msg
+  msg/EStopState.msg
 )
 set(action_files
   action/MoveToPosition.action
@@ -59,6 +61,8 @@ set(srv_files
   srv/MotorInfo.srv
   srv/ClearArmError.srv
   srv/InverseKinematics.srv
+  srv/SetArmEnable.srv
+  srv/ResetEStop.srv
 )
 
 if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include")

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/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/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  # 执行结果描述（失败原因/调试信息）