更新几个变量名

saturn_controller/src/saturn_controller.cpp

@@ -152,7 +152,7 @@ void SaturnController::cmdVelCallback(const Twist::ConstSharedPtr vel_msg) {
   rc_direct.angular.y = vel_msg->angular.y;
   rc_direct.angular.z = vel_msg->angular.z;
   rcClientInterfaceDirectionMovement(
-      robot_control::common::NAV_OR_JOY_MODE::nav_control, rc_direct,
+      robot_control::common::CONTROL_SOURCE_MODE::nav_control, rc_direct,
       robot_heartbeat_);
 }
 
@@ -180,9 +180,10 @@ void SaturnController::robotCommandCallback(
     rc_direct.angular.y = 0.0;
     rc_direct.angular.z = 0.0;
     result = rcClientInterfaceDirectionMovement(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control, rc_direct,
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control, rc_direct,
         robot_heartbeat_);
   } else if ("StandUpDown" == request->comamnd_name) {
+    // fangyong 后续站立趴下，可以直接判断BODY_POSTURE_STATE ROBOT_COMMON_STATUS里的belie_or_stand; 
     SPDLOG_INFO("cur_scene {}", common_status_data_.cur_scene);
     if ((common_status_data_.cur_scene ==
          robot_control::common::SCENE_TYPE::WALKING) ||
@@ -190,44 +191,47 @@ void SaturnController::robotCommandCallback(
          robot_control::common::SCENE_TYPE::STAIRS) ||
         (common_status_data_.cur_scene ==
          robot_control::common::SCENE_TYPE::STONE) ||
+         //fangyong 斜坡
+        (common_status_data_.cur_scene ==
+          robot_control::common::SCENE_TYPE::ROUGH) ||
         (common_status_data_.cur_scene ==
             robot_control::common::SCENE_TYPE::RL)) {
       result = rcClientInterfaceSetScene(
-          robot_control::common::NAV_OR_JOY_MODE::nav_control,
+          robot_control::common::CONTROL_SOURCE_MODE::nav_control,
           common::SCENE_TYPE::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,
+          robot_control::common::CONTROL_SOURCE_MODE::nav_control,
           common::SCENE_TYPE::WALKING);
     }
   } else if ("EnableNavigation" == request->comamnd_name) {
-    common::NAV_OR_JOY_MODE nav_joy;
-    nav_joy = common::NAV_OR_JOY_MODE::nav_control;
+    common::CONTROL_SOURCE_MODE nav_joy;
+    nav_joy = common::CONTROL_SOURCE_MODE::nav_control;
     result = rcClientInterfaceSetNavOrJoyControl(nav_joy);
   } else if ("EnableTeleoption" == request->comamnd_name) {
-    common::NAV_OR_JOY_MODE nav_joy;
-    nav_joy = common::NAV_OR_JOY_MODE::joy_control;
+    common::CONTROL_SOURCE_MODE nav_joy;
+    nav_joy = common::CONTROL_SOURCE_MODE::joy_control;
     result = rcClientInterfaceSetNavOrJoyControl(nav_joy);
   } else if ("StairsGait" == request->comamnd_name) {
     SPDLOG_INFO("StairsGait is called");
     result = rcClientInterfaceSetScene(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control,
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         common::SCENE_TYPE::STAIRS);
   } else if ("OrdinaryGait" == request->comamnd_name) {
     SPDLOG_INFO("OrdinaryGait is called");
     result = rcClientInterfaceSetScene(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control,
+        robot_control::common::CONTROL_SOURCE_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,
+        robot_control::common::CONTROL_SOURCE_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,
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         common::SCENE_TYPE::RL);
   } else if ("StayNormal" == request->comamnd_name) {
     int height = (int)(common_status_data_.max_height * 0.8 * 100);
@@ -236,7 +240,7 @@ void SaturnController::robotCommandCallback(
         "set to {} cm",
         height);
     result = rcClientInterfaceBodyHighAdjust(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control, height);
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control, height);
   } else if ("StayDown" == request->comamnd_name) {
     int height = (int)(common_status_data_.max_height * 0.6 * 100);
     SPDLOG_INFO(
@@ -244,12 +248,12 @@ void SaturnController::robotCommandCallback(
         "to {} cm",
         height);
     result = rcClientInterfaceBodyHighAdjust(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control, height);
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control, height);
   } else if ("SlopeGait" == request->comamnd_name) {
     SPDLOG_INFO("SlopeGait is called");
     result = rcClientInterfaceSetScene(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control,
-        common::SCENE_TYPE::STONE);
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control,
+        common::SCENE_TYPE::STONE); //fangyong 斜坡步态改为调用 common::SCENE_TYPE::ROUGH
   } else if ("VstairsGait" == request->comamnd_name) {
     SPDLOG_WARN("VstairsGait is not supported yet.");
     result = false;
@@ -286,7 +290,7 @@ bool SaturnController::leaveDock() {
 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)) {
+          robot_control::common::CONTROL_SOURCE_MODE::nav_control, is_on_dock)) {
     SPDLOG_WARN("gRPC not connected, command loading failed.");
     return false;
   }
@@ -316,7 +320,7 @@ void SaturnController::guardianCallback(
   float velocity_decay_ratio = guardian_msg->velocity_decay_ratio;
   std::string trigger_source = guardian_msg->trigger_source;
   rcClientInterfaceSetGuardian(
-      robot_control::common::NAV_OR_JOY_MODE::nav_control, velocity_decay_ratio,
+      robot_control::common::CONTROL_SOURCE_MODE::nav_control, velocity_decay_ratio,
       trigger_source);
 }
 
@@ -329,7 +333,7 @@ void SaturnController::guardianServiceCallback(
   // 遥控器上增加显示
   if (request->velocity_decay_ratio <= 0) {
     rcClientInterfaceSetGuardian(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control,
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         request->velocity_decay_ratio, request->trigger_source);
     // 只有"front_"source 触发的停障才会反映到"cmd_vel"中
     if (request->trigger_source.find("front") != std::string::npos) {
@@ -340,7 +344,7 @@ void SaturnController::guardianServiceCallback(
     this->guardian_velocity_decay_ratio_.store(request->velocity_decay_ratio);
     // 遥控器上增加显示
     rcClientInterfaceSetGuardian(
-        robot_control::common::NAV_OR_JOY_MODE::nav_control,
+        robot_control::common::CONTROL_SOURCE_MODE::nav_control,
         request->velocity_decay_ratio, request->trigger_source);
   } else {
     // 只有"front_"source 触发的减速才会反映到"cmd_vel"中
@@ -541,7 +545,7 @@ void SaturnController::publishDockingState() {
 }
 
 void SaturnController::publishControlMode() {
-  using SDK_CONTROL_MODE = robot_control::common::NAV_OR_JOY_MODE;
+  using SDK_CONTROL_MODE = robot_control::common::CONTROL_SOURCE_MODE;
   auto curr_mode = CONTROL_MODE::NULL_MODE;
 
   switch (common_status_data_.joy_mode) {
@@ -640,14 +644,14 @@ void SaturnController::publishRobotMode() {
 
   if ((is_robot_in_idle_state_ == true) &&
       (common_status_data_.joy_mode ==
-       robot_control::common::NAV_OR_JOY_MODE::nav_control)) {
+       robot_control::common::CONTROL_SOURCE_MODE::nav_control)) {
     robot_mode_state.data = 0;
   } else if (common_status_data_.joy_mode ==
-             robot_control::common::NAV_OR_JOY_MODE::joy_control) {
+             robot_control::common::CONTROL_SOURCE_MODE::joy_control) {
     robot_mode_state.data = 1;
   } else if ((is_robot_in_idle_state_ == false) &&
              (common_status_data_.joy_mode ==
-              robot_control::common::NAV_OR_JOY_MODE::nav_control)) {
+              robot_control::common::CONTROL_SOURCE_MODE::nav_control)) {
     robot_mode_state.data = 2;
   } else if (common_status_data_.cur_scene ==
              robot_control::common::SCENE_TYPE::CHARGE) {