删除手部串联控制驱动,加入系统报错信息话题发布

README.md

@@ -49,6 +49,80 @@ ros2 topic echo /robot1/right_arm/joint_states
 
 当前反馈中没有关节加速度字段。如果外部软件必须获取关节加速度，需要在上游根据 `velocity[]` 和时间戳自行差分，或者让厂家提供 SDK 原生加速度反馈接口。
 
+## 机器人健康/故障状态
+
+当前 `officer_sdk/lbot_driver` 已发布系统错误信息 topic：
+
+```text
+/robot1/system_error
+```
+
+消息类型：
+
+```text
+lbot_arm_interfaces/msg/SystemError
+```
+
+消息定义：
+
+```text
+std_msgs/Header header
+int32 error_code
+string error_msg
+bool connected
+```
+
+字段含义：
+
+```text
+error_code  SDK 或 driver 上报的错误码
+error_msg   错误信息
+connected   发布错误时 driver 是否认为机器人处于连接状态
+```
+
+示例：
+
+```bash
+ros2 topic echo /robot1/system_error
+```
+
+目前该 topic 会在以下情况发布：
+
+```text
+SDK error callback 触发时
+连接机器人失败时，error_code = -1
+状态监控启动失败时，error_code = -2
+```
+
+注意：SDK 当前状态结构 `lbot_full_state_t` 只包含左右臂关节状态、末端位姿、时间戳和 IP；没有完整的故障码、告警码、使能状态、急停状态等字段。因此 `/robot1/system_error` 当前是错误事件 topic，不是完整健康状态快照。
+
+仍然可以辅助查看 ROS 日志：
+
+```text
+/rosout
+```
+
+示例：
+
+```bash
+ros2 topic echo /rosout
+```
+
+也可以查看启动脚本日志：
+
+```bash
+tail -f ~/workspace/wpz_LbotArm/log/lbot_arm_control.log
+```
+
+如果后续需要稳定给外部软件使用的完整健康状态快照，建议在 `SystemError.msg` 之外再新增 `RobotHealth.msg`，并在连接/重连逻辑、使能/急停服务回调中维护并周期发布该状态。需要修改的位置：
+
+```text
+src/officer_sdk/lbot_arm_interfaces/msg/RobotHealth.msg
+src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt
+src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h
+src/officer_sdk/lbot_driver/src/lbot_driver.cpp
+```
+
 ## 机械臂关节空间控制
 
 外部软件如果需要发送机械臂各关节目标位置，使用 MoveJ 服务，不是 topic。
@@ -173,45 +247,6 @@ ros2 topic pub --once /robot1/right_hand/set_l20_joint std_msgs/msg/Int32MultiAr
 "{data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}"
 ```
 
-## L20/R20 单指串联位置控制
-
-如果只需要控制单根手指，可以使用：
-
-```text
-/robot1/left_hand/set_l20_series_position
-/robot1/right_hand/set_l20_series_position
-```
-
-消息类型：
-
-```text
-std_msgs/msg/Int32MultiArray
-```
-
-当前约定的数据格式：
-
-```text
-data[0]      finger 编号
-data[1..6]   该手指的 6 个位置参数
-```
-
-finger 编号：
-
-```text
-0 thumb
-1 index
-2 middle
-3 ring
-4 little
-```
-
-示例，控制左手食指：
-
-```bash
-ros2 topic pub --once /robot1/left_hand/set_l20_series_position std_msgs/msg/Int32MultiArray \
-"{data: [1, 0, 30, 30, 0, 0, 0]}"
-```
-
 ## 相关代码位置
 
 反馈发布：
@@ -238,8 +273,6 @@ L20/R20 灵巧手 topic：
 ```text
 LeftArmServiceNode::left_hand_l20_set_joint_callback()
 RightArmServiceNode::right_hand_l20_set_joint_callback()
-LeftArmServiceNode::left_hand_l20_set_series_position_callback()
-RightArmServiceNode::right_hand_l20_set_series_position_callback()
 ```
 
 SDK API 声明：

src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt

@@ -46,6 +46,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/LbotPose.msg"
     "msg/LbotFrame.msg"
     "msg/FollowJoint.msg"
+    "msg/SystemError.msg"
     
     DEPENDENCIES std_msgs geometry_msgs sensor_msgs
  )

src/officer_sdk/lbot_arm_interfaces/msg/SystemError.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+int32 error_code
+string error_msg
+bool connected

src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h

@@ -54,6 +54,7 @@
 #include "lbot_arm_interfaces/msg/lbot_pose.hpp"
 #include "lbot_arm_interfaces/msg/lbot_frame.hpp"
 #include "lbot_arm_interfaces/msg/follow_joint.hpp"
+#include "lbot_arm_interfaces/msg/system_error.hpp"
 
 // 自定义 Service 类型
 #include "lbot_arm_interfaces/srv/change_frame.hpp"
@@ -118,6 +119,7 @@ public:
     std::mutex reconnect_thread_mutex_;
 
     void disconnect_robot();
+    void publish_system_error(int error_code, const char* error_msg);
 
 private:
     // 初始化和连接相关
@@ -140,6 +142,7 @@ private:
     rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr right_joint_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr left_pose_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr right_pose_pub_;
+    rclcpp::Publisher<lbot_arm_interfaces::msg::SystemError>::SharedPtr system_error_pub_;
 
     /****************************** 订阅器 ******************************/
     
@@ -190,7 +193,6 @@ private:
     void left_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
     void left_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
     void left_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg);
-    void left_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg);
     
     /****************************** 连接状态检查函数 ******************************/
     
@@ -295,7 +297,6 @@ private:
     rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_force_sub_;
     rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_speed_sub_;
     rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr left_hand_l20_joint_sub_;
-    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr left_hand_l20_series_position_sub_;
 };
 
 // 右臂服务节点 - 负责右臂所有服务
@@ -318,7 +319,6 @@ private:
     void right_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
     void right_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg);
     void right_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg);
-    void right_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg);
     
     /****************************** 连接状态检查函数 ******************************/
     
@@ -423,7 +423,6 @@ private:
     rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_force_sub_;
     rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_speed_sub_;
     rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr right_hand_l20_joint_sub_;
-    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr right_hand_l20_series_position_sub_;
 };
 
 }  // namespace lbot_driver

src/officer_sdk/lbot_driver/src/lbot_driver.cpp

@@ -36,6 +36,10 @@ void lbot_state_callback_wrapper(const lbot_full_state_t* state) {
 void lbot_error_callback_wrapper(int error_code, const char* error_msg) {
     if (LBot::g_instance && LBot::g_instance->shutting_down_) return; 
 
+    if (LBot::g_instance) {
+        LBot::g_instance->publish_system_error(error_code, error_msg);
+    }
+
     if (error_msg) {
         RCLCPP_ERROR(
             rclcpp::get_logger("lbot_driver"),
@@ -75,6 +79,7 @@ LBot::LBot(const std::string& node_name) : rclcpp::Node(node_name) {
     right_joint_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("right_arm/joint_states", 10);
     left_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("left_arm/pose_states", 10);
     right_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("right_arm/pose_states", 10);
+    system_error_pub_ = this->create_publisher<lbot_arm_interfaces::msg::SystemError>("system_error", 10);
 
     // 创建关节跟随订阅器（避免阻塞服务）
     auto sub_opt = rclcpp::SubscriptionOptions();
@@ -120,6 +125,18 @@ LBot::~LBot() {
     disconnect_robot();
 }
 
+void LBot::publish_system_error(int error_code, const char* error_msg) {
+    if (!system_error_pub_) return;
+
+    lbot_arm_interfaces::msg::SystemError msg;
+    msg.header.stamp = this->get_clock()->now();
+    msg.header.frame_id = "lbot_driver";
+    msg.error_code = error_code;
+    msg.error_msg = error_msg ? error_msg : "";
+    msg.connected = g_conn_state.load() == GlobalConnState::CONNECTED;
+    system_error_pub_->publish(msg);
+}
+
 bool LBot::connect_robot() {
     std::lock_guard<std::mutex> lock(conn_mutex_);
     if(conn_state_ == GlobalConnState::CONNECTED) return true;
@@ -140,10 +157,12 @@ bool LBot::connect_robot() {
         } else {
             is_state_monitor_started_ = false;
             RCLCPP_ERROR(this->get_logger(), "Failed to start state monitor");
+            publish_system_error(-2, "Failed to start state monitor");
         }
 
     } else {
         RCLCPP_ERROR(this->get_logger(), "Failed to connect to LBot");
+        publish_system_error(-1, "Failed to connect to LBot");
     }
 
     bool success = lbot_handle->id > 0;
@@ -507,10 +526,6 @@ void LeftArmServiceNode::create_services() {
         "left_hand/set_l20_joint", 10,
         std::bind(&LeftArmServiceNode::left_hand_l20_set_joint_callback, this, std::placeholders::_1),
         sub_opt);
-    left_hand_l20_series_position_sub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(
-        "left_hand/set_l20_series_position", 10,
-        std::bind(&LeftArmServiceNode::left_hand_l20_set_series_position_callback, this, std::placeholders::_1),
-        sub_opt);
 }
 
 /****************************** 左臂服务回调函数实现 ******************************/
@@ -1036,23 +1051,6 @@ void LeftArmServiceNode::left_hand_l20_set_joint_callback(const std_msgs::msg::I
     }
 }
 
-void LeftArmServiceNode::left_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) {
-    if (!msg || msg->data.size() < 7) return;
-    if (!check_connection_state("left_hand/set_l20_series_position")) {
-        return;
-    }
-
-    lbot_l20_series_cmd_t cmd{};
-    cmd.finger = static_cast<lbot_finger_type_t>(msg->data[0]);
-    for (size_t i = 0; i < 6; ++i) {
-        cmd.data[i] = static_cast<uint8_t>(msg->data[i + 1]);
-    }
-
-    if (!lbot_api.lbot_l20_set_series_position(lbot_handle, LBOT_LEFT_ARM, &cmd)) {
-        RCLCPP_ERROR(this->get_logger(), "Failed to set left L20 hand series position");
-    }
-}
-
 // ========== 右臂服务节点实现 ==========
 RightArmServiceNode::RightArmServiceNode(const std::string& node_name) : rclcpp::Node(node_name) {
     // 设置独特的logger名称
@@ -1173,10 +1171,6 @@ void RightArmServiceNode::create_services() {
         "right_hand/set_l20_joint", 10,
         std::bind(&RightArmServiceNode::right_hand_l20_set_joint_callback, this, std::placeholders::_1),
         sub_opt);
-    right_hand_l20_series_position_sub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(
-        "right_hand/set_l20_series_position", 10,
-        std::bind(&RightArmServiceNode::right_hand_l20_set_series_position_callback, this, std::placeholders::_1),
-        sub_opt);
 }
 
 /****************************** 右臂服务回调函数实现 ******************************/
@@ -1692,23 +1686,6 @@ void RightArmServiceNode::right_hand_l20_set_joint_callback(const std_msgs::msg:
     }
 }
 
-void RightArmServiceNode::right_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) {
-    if (!msg || msg->data.size() < 7) return;
-    if (!check_connection_state("right_hand/set_l20_series_position")) {
-        return;
-    }
-
-    lbot_l20_series_cmd_t cmd{};
-    cmd.finger = static_cast<lbot_finger_type_t>(msg->data[0]);
-    for (size_t i = 0; i < 6; ++i) {
-        cmd.data[i] = static_cast<uint8_t>(msg->data[i + 1]);
-    }
-
-    if (!lbot_api.lbot_l20_set_series_position(lbot_handle, LBOT_RIGHT_ARM, &cmd)) {
-        RCLCPP_ERROR(this->get_logger(), "Failed to set right L20 hand series position");
-    }
-}
-
 }  // namespace lbot_driver
 
 /****************************** 主函数 ******************************/