加入随sh脚本生命周期的使能和失能功能

src/officer_sdk/lbot_driver/src/lbot_driver.cpp

@@ -150,6 +150,14 @@ bool LBot::connect_robot() {
         // 获取机械臂信息
         get_robot_info();
 
+        bool left_enable_ok = lbot_api.lbot_enable_arm(lbot_handle, LBOT_LEFT_ARM, true);
+        bool right_enable_ok = lbot_api.lbot_enable_arm(lbot_handle, LBOT_RIGHT_ARM, true);
+        if (left_enable_ok && right_enable_ok) {
+            RCLCPP_INFO(this->get_logger(), "Left and right arms enabled successfully");
+        } else {
+            RCLCPP_ERROR(this->get_logger(), "Failed to enable arms: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str());
+        }
+
         // 启动状态监控
         if (lbot_api.lbot_start_state_monitor(lbot_state_callback_wrapper, lbot_error_callback_wrapper)) {
             is_state_monitor_started_ = true;
@@ -178,6 +186,14 @@ void LBot::disconnect_robot() {
 
     RCLCPP_INFO(this->get_logger(), "Disconnecting from LBot...");
 
+    bool left_disable_ok = lbot_api.lbot_enable_arm(lbot_handle, LBOT_LEFT_ARM, false);
+    bool right_disable_ok = lbot_api.lbot_enable_arm(lbot_handle, LBOT_RIGHT_ARM, false);
+    if (left_disable_ok && right_disable_ok) {
+        RCLCPP_INFO(this->get_logger(), "Left and right arms disabled successfully");
+    } else {
+        RCLCPP_ERROR(this->get_logger(), "Failed to disable arms: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str());
+    }
+    
     // 先设置状态，避免其他线程继续操作
     conn_state_ = GlobalConnState::DISCONNECTED;
     g_conn_state.store(GlobalConnState::DISCONNECTED);  // 更新全局状态