weipengzhe/weipz-Lbot_Bridge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
weipengzhe
2026-09-17 13:42:11 +08:00
parent 9479a8ccb2
commit 50aba5217f
5 changed files with 101 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+74 -41
View File
@@ -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
+1
View File
@@ -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
+4
View File
@@ -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
+3 -4
View File
@@ -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
+19 -42
View File
@@ -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
/****************************** 主函数 ******************************/