[2-fix] 修复上下桩反馈过程，调整步态反馈值

        1. 修复上下桩过程，增加函数封装，并对反馈过程进行阻塞。
        2. 根据导航接口调整步态反馈值，与 X30 枚举一致。

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -25,6 +25,12 @@
 #include <spdlog/spdlog.h>
 namespace saturn_ros2
 {
+    enum class Robot_Gait : uint8_t {
+        TROT = 0,                  // 正常步态
+        NORMAL_STAIR = 1,          // 楼梯步态
+        SLOPE = 2,                 // 斜坡步态
+        SENSE_STAIR = 6            // 感知楼梯步态
+    };
 
     class SaturnController
     {
@@ -147,6 +153,12 @@ namespace saturn_ros2
 
         void robotCommandCallback(const ysc_robot_msgs::srv::RobotCommand::Request::SharedPtr request,
                                   ysc_robot_msgs::srv::RobotCommand::Response::SharedPtr response);
+        
+        bool goToDock();
+
+        bool leaveDock();
+
+        bool waitForDockFinishing(bool is_on_dock, size_t time_out_s);
 
         void guardianCallback(const daystar_navigation_msgs::msg::Guardian::SharedPtr guardian_msg);
 

saturn_controller/src/saturn_controller.cpp

@@ -122,20 +122,20 @@ namespace saturn_ros2
         const ysc_robot_msgs::srv::RobotCommand::Request::SharedPtr request,
         ysc_robot_msgs::srv::RobotCommand::Response::SharedPtr response)
     {
-        bool result = true;
+        bool result = false;
         std::string command = request->comamnd_name;
 
         SPDLOG_INFO("Received command: {}", command.c_str());
 
         if ("GoToDock" == request->comamnd_name)
         {
-            result = rcClientInterfaceEnterOrExitCharge(
-                robot_control::common::NAV_OR_JOY_MODE::nav_control, true);
+            response->result = goToDock();
+            return;
         }
         else if ("LeaveDock" == request->comamnd_name)
         {
-            result = rcClientInterfaceEnterOrExitCharge(
-                robot_control::common::NAV_OR_JOY_MODE::nav_control, false);
+            response->result = leaveDock();
+            return;
         }
         else if ("HeartBeat" == request->comamnd_name)
         {
@@ -235,10 +235,48 @@ namespace saturn_ros2
             SPDLOG_WARN("Invalid robot command name!!!");
             result = false;
         }
-
         response->result = result;
     }
 
+    bool SaturnController::goToDock() {
+        if (!waitForDockFinishing(true, 60)) {
+            return false;
+        }
+        return true;
+    }
+
+    bool SaturnController::leaveDock() {
+        if (!waitForDockFinishing(false, 60)) {
+            return false;
+        }
+        return true;
+    }
+
+    bool SaturnController::waitForDockFinishing(bool is_on_dock, size_t time_out_s) {
+        if (!rcClientInterfaceEnterOrExitCharge(
+            robot_control::common::NAV_OR_JOY_MODE::nav_control, is_on_dock)) {
+            SPDLOG_WARN("gRPC not connected, command loading failed.");
+            return false;
+        }
+        size_t retry = 0;
+        auto task_type = is_on_dock ? "GoToDock" : "LeaveDock";
+        while (++retry <= time_out_s && rclcpp::ok()) {
+            bool query_state =  
+                is_on_dock ? common_status_data_.charge_status.charge_switch_state == 
+                                robot_control::common::CHARGE_SWITCH_STATE::ENTER_SUCCEEDED
+                            : common_status_data_.charge_status.charge_switch_state == 
+                            robot_control::common::CHARGE_SWITCH_STATE::EXIT_SUCCEEDED;
+            if (query_state) {
+                SPDLOG_INFO("Robot finished {} task!", task_type);
+                return true;
+            }
+            SPDLOG_INFO("Waiting for finishing {} task for {}s", task_type, retry);
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        SPDLOG_ERROR("Robot did not finish {} task in: {} seconds.", task_type, time_out_s);
+        return false;
+    }
+
     void SaturnController::guardianCallback(
         const daystar_navigation_msgs::msg::Guardian::SharedPtr guardian_msg)
     {
@@ -482,27 +520,30 @@ namespace saturn_ros2
         auto gait_scene_state = std_msgs::msg::UInt8();
         if (common_status_data_.cur_scene == robot_control::common::SCENE_TYPE::LIE_DOWN)
         {
-            gait_scene_state.data = 1;
+            gait_scene_state.data = 3;  // TODO
         }
         else if (common_status_data_.cur_scene == robot_control::common::SCENE_TYPE::WALKING)
         {
-            gait_scene_state.data = 2;
+            gait_scene_state.data = 
+                static_cast<decltype(gait_scene_state.data)>(saturn_ros2::Robot_Gait::TROT);
         }
         else if (common_status_data_.cur_scene == robot_control::common::SCENE_TYPE::STAIRS)
         {
-            gait_scene_state.data = 3;
+            gait_scene_state.data = 
+                static_cast<decltype(gait_scene_state.data)>(saturn_ros2::Robot_Gait::NORMAL_STAIR);
         }
         else if (common_status_data_.cur_scene == robot_control::common::SCENE_TYPE::CHARGE)
         {
-            gait_scene_state.data = 4;
+            gait_scene_state.data = 4;  // TODO
         }
         else if (common_status_data_.cur_scene == robot_control::common::SCENE_TYPE::STONE)
         {
-            gait_scene_state.data = 5;
+            gait_scene_state.data = 
+                static_cast<decltype(gait_scene_state.data)>(saturn_ros2::Robot_Gait::SLOPE);
         }
         else
         {
-            gait_scene_state.data = 0;
+            gait_scene_state.data = 5;  // TODO
         }
         scene_publisher_->publish(gait_scene_state);
     }