diff --git a/src/officer_sdk/lbot_driver/src/lbot_driver.cpp b/src/officer_sdk/lbot_driver/src/lbot_driver.cpp index 2ca3868..028aef6 100644 --- a/src/officer_sdk/lbot_driver/src/lbot_driver.cpp +++ b/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; @@ -177,6 +185,14 @@ void LBot::disconnect_robot() { if (conn_state_ == GlobalConnState::DISCONNECTED) return; 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;