[feat] 增加RL(强化学习步态)的切换

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -25,10 +25,12 @@
 #include "ysc_robot_msgs/srv/robot_command.hpp"
 namespace saturn_ros2 {
 enum class Robot_Gait : uint8_t {
-  TROT = 0,          // 正常步态
-  NORMAL_STAIR = 1,  // 楼梯步态
-  SLOPE = 2,         // 斜坡步态
-  SENSE_STAIR = 6,   // 感知楼梯步态
+  TROT = 0,           // 正常步态
+  NORMAL_STAIR = 1,   // 楼梯步态
+  SLOPE = 2,          // 斜坡步态
+  SENSE_STAIR = 6,    // 感知楼梯步态
+  RL_TROT = 0x20,     // RL 行走步态
+  RL_MOUNTAIN = 0x21, // RL 山地步态
 };
 
 class SaturnController {

saturn_controller/src/saturn_controller.cpp

@@ -189,12 +189,14 @@ void SaturnController::robotCommandCallback(
         (common_status_data_.cur_scene ==
          robot_control::common::SCENE_TYPE::STAIRS) ||
         (common_status_data_.cur_scene ==
-         robot_control::common::SCENE_TYPE::STONE)) {
+         robot_control::common::SCENE_TYPE::STONE) ||
+        (common_status_data_.cur_scene ==
+            robot_control::common::SCENE_TYPE::RL)) {
       result = rcClientInterfaceSetScene(
           robot_control::common::NAV_OR_JOY_MODE::nav_control,
           common::SCENE_TYPE::LIE_DOWN);
-    } else if (common_status_data_.cur_scene ==
-               robot_control::common::LIE_DOWN) {
+      }
+    else if (common_status_data_.cur_scene == robot_control::common::LIE_DOWN) {
       result = rcClientInterfaceSetScene(
           robot_control::common::NAV_OR_JOY_MODE::nav_control,
           common::SCENE_TYPE::WALKING);
@@ -217,6 +219,16 @@ void SaturnController::robotCommandCallback(
     result = rcClientInterfaceSetScene(
         robot_control::common::NAV_OR_JOY_MODE::nav_control,
         common::SCENE_TYPE::WALKING);
+  } else if ("RLOrdinaryGait" == request->comamnd_name) {
+    SPDLOG_INFO("RLOrdinaryGait is called");
+    result = rcClientInterfaceSetScene(
+        robot_control::common::NAV_OR_JOY_MODE::nav_control,
+        common::SCENE_TYPE::RL);
+  } else if ("RLMountainGait" == request->comamnd_name) {
+    SPDLOG_INFO("RLMountainGait is called");
+    result = rcClientInterfaceSetScene(
+        robot_control::common::NAV_OR_JOY_MODE::nav_control,
+        common::SCENE_TYPE::RL);
   } else if ("StayNormal" == request->comamnd_name) {
     int height = (int)(common_status_data_.max_height * 0.8 * 100);
     SPDLOG_INFO(
@@ -583,6 +595,10 @@ void SaturnController::publishGait() {
       curr_scene = Robot_Gait::SENSE_STAIR;
       break;
     }
+    case SDK_SCENE_TYPE::RL: {
+      curr_scene = Robot_Gait::RL_TROT;
+      break;
+    }
     case SDK_SCENE_TYPE::SNOW:
     case SDK_SCENE_TYPE::SLIPPY:
     default: {