From 50aba5217fe8544a5fa031db47b342d648fb138c Mon Sep 17 00:00:00 2001 From: weipengzhe <3389096668@qq.com> Date: Thu, 17 Sep 2026 13:42:11 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=89=8B=E9=83=A8=E4=B8=B2?= =?UTF-8?q?=E8=81=94=E6=8E=A7=E5=88=B6=E9=A9=B1=E5=8A=A8,=E5=8A=A0?= =?UTF-8?q?=E5=85=A5=E7=B3=BB=E7=BB=9F=E6=8A=A5=E9=94=99=E4=BF=A1=E6=81=AF?= =?UTF-8?q?=E8=AF=9D=E9=A2=98=E5=8F=91=E5=B8=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 115 +++++++++++------- .../lbot_arm_interfaces/CMakeLists.txt | 1 + .../lbot_arm_interfaces/msg/SystemError.msg | 4 + .../include/lbot_driver/lbot_driver.h | 7 +- .../lbot_driver/src/lbot_driver.cpp | 61 +++------- 5 files changed, 101 insertions(+), 87 deletions(-) create mode 100644 src/officer_sdk/lbot_arm_interfaces/msg/SystemError.msg diff --git a/README.md b/README.md index bd44893..a39cb43 100644 --- a/README.md +++ b/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 声明： diff --git a/src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt b/src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt index 87b542c..74e7793 100755 --- a/src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt +++ b/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 ) diff --git a/src/officer_sdk/lbot_arm_interfaces/msg/SystemError.msg b/src/officer_sdk/lbot_arm_interfaces/msg/SystemError.msg new file mode 100644 index 0000000..f46a83f --- /dev/null +++ b/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 diff --git a/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h index 2aeb98d..ed28611 100644 --- a/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h +++ b/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::SharedPtr right_joint_pub_; rclcpp::Publisher::SharedPtr left_pose_pub_; rclcpp::Publisher::SharedPtr right_pose_pub_; + rclcpp::Publisher::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::SharedPtr left_hand_l10_force_sub_; rclcpp::Subscription::SharedPtr left_hand_l10_speed_sub_; rclcpp::Subscription::SharedPtr left_hand_l20_joint_sub_; - rclcpp::Subscription::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::SharedPtr right_hand_l10_force_sub_; rclcpp::Subscription::SharedPtr right_hand_l10_speed_sub_; rclcpp::Subscription::SharedPtr right_hand_l20_joint_sub_; - rclcpp::Subscription::SharedPtr right_hand_l20_series_position_sub_; }; } // namespace lbot_driver diff --git a/src/officer_sdk/lbot_driver/src/lbot_driver.cpp b/src/officer_sdk/lbot_driver/src/lbot_driver.cpp index 9d6affc..2ca3868 100644 --- a/src/officer_sdk/lbot_driver/src/lbot_driver.cpp +++ b/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("right_arm/joint_states", 10); left_pose_pub_ = this->create_publisher("left_arm/pose_states", 10); right_pose_pub_ = this->create_publisher("right_arm/pose_states", 10); + system_error_pub_ = this->create_publisher("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 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( - "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(msg->data[0]); - for (size_t i = 0; i < 6; ++i) { - cmd.data[i] = static_cast(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( - "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(msg->data[0]); - for (size_t i = 0; i < 6; ++i) { - cmd.data[i] = static_cast(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 /****************************** 主函数 ******************************/