[fix] 修复robot_state逻辑, 重构dock_state/control_mode/scene逻辑

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -27,7 +27,8 @@ enum class Robot_Gait : uint8_t {
   TROT = 0,          // 正常步态
   NORMAL_STAIR = 1,  // 楼梯步态
   SLOPE = 2,         // 斜坡步态
-  SENSE_STAIR = 6    // 感知楼梯步态
+  SENSE_STAIR = 6,   // 感知楼梯步态
+  NULL_MODE = 9     // 无步态
 };
 
 class SaturnController {
@@ -86,30 +87,36 @@ class SaturnController {
   rclcpp::Publisher<Imu>::SharedPtr imu_publisher_;
 
   // 机器人基本状态
-  //  0 -> lie down
-  //  1 -> standingup
-  //  2 -> standup
-  //  3 -> enable forcecontrol
-  //  4 -> motion
-  //  5 -> sitting down
-  //  6 -> fall down
-  //  7 -> null
+  enum class ROBOT_STATE : uint32_t {
+    LIE_DOWN = 0,
+    STANDING_UP = 1,
+    STAND_UP = 2,
+    ENABLE_FORCECONTROL = 3,
+    STEPPING = 4,
+    SITTING_DOWN = 5,
+    FALL_DOWN = 6,
+    NULL_STATE = 7
+  };
   rclcpp::Publisher<UInt32>::SharedPtr robot_state_publisher_;
 
   // dock_state(std_msgs::msg::UInt32): 机器人上桩状态
-  // 0 -> idle
-  // 1 -> going to dock
-  // 2 -> stand above dock
-  // 3 -> adjust pose
-  // 4 -> lie on dock
-  // 5 -> standing after charging
-  // 6 -> leaving dock
-  // 7 -> leave dock
+  enum class DOCK_STATE : uint32_t {
+    IDLE = 0,
+    GOING_TO_DOCK = 1,
+    STND_ABOVE_DOCK = 2,
+    ADJUST_POSE = 3,
+    LIE_ON_DOCKER = 4,
+    STANDING_AFTER_CHARGING = 5,
+    LEAVING_DOCK = 6,
+    LEAVE_DOCK = 7,
+  };
   rclcpp::Publisher<UInt32>::SharedPtr docking_state_publisher_;
 
-  // 控制模式
-  //  0 -> 手持遥控
-  //  1 -> 导航
+  enum class CONTROL_MODE : uint8_t {
+    JOY_CONTROL = 0,  //  0 -> 手持遥控
+    NAVI_CONTROL = 1, //  1 -> 导航
+    NULL_MODE = 2,
+  };
   rclcpp::Publisher<UInt8>::SharedPtr control_mode_publisher_;
 
   // 设置六足机器人场景

saturn_controller/src/saturn_controller.cpp

@@ -1,9 +1,9 @@
 #include "saturn_controller/saturn_controller.hpp"
+#include <cstddef>
 namespace saturn_ros2 {
 SaturnController::SaturnController(const rclcpp::NodeOptions &options)
     : node_(std::make_shared<rclcpp::Node>("saturn_controller", options)),
-      enable_guardian_(true),
-      guardian_velocity_decay_ratio_(2.0) {
+      enable_guardian_(true), guardian_velocity_decay_ratio_(2.0) {
   SPDLOG_INFO("====== SaturnController created ======");
   config_handler_ = std::make_shared<ConfigHandler>(node_);
   init();
@@ -159,7 +159,7 @@ void SaturnController::cmdVelCallback(const Twist::ConstSharedPtr vel_msg) {
   }
   if (std::fabs(actual_velocity_decay_ratio) < 1e-4)
     SPDLOG_INFO("Guardian triggered, ignoring cmd_vel.x... ");
-  rc_direct.linear.x = vel_msg->linear.x * actual_velocity_decay_ratio;  // todo
+  rc_direct.linear.x = vel_msg->linear.x * actual_velocity_decay_ratio; // todo
   rc_direct.linear.y = vel_msg->linear.y;
   rc_direct.linear.z = vel_msg->linear.z;
   rc_direct.angular.x = vel_msg->angular.x;
@@ -464,81 +464,165 @@ void SaturnController::leggedOodmTimerCallback() {
   // 先把足式里程计放在主callback里发布，如果对不同频率有需求，再重新使用这里
 }
 
-void SaturnController::publishRobotState() {  // todo 这里逻辑还不完善
-  auto robot_state = std_msgs::msg::UInt32();
-  if (common_status_data_.belie_or_stand == 3) {
-    robot_state.data = 5;
-  } else if (common_status_data_.belie_or_stand ==
-                 robot_control::common::BODY_POSTURE_STATE_::MOVING ||
-             common_status_data_.belie_or_stand ==
-                 robot_control::common::BODY_POSTURE_STATE_::STANDUP) {
-    robot_state.data = 2;
-  } else {
-    robot_state.data = 7;
+void SaturnController::publishRobotState() { // todo 这里逻辑还不完善
+  using SDK_BODY_POSTURE_STATE = common::BODY_POSTURE_STATE;
+  auto curr_state = ROBOT_STATE::NULL_STATE;
+
+  switch (common_status_data_.belie_or_stand) {
+    case SDK_BODY_POSTURE_STATE::NULL_POSTURE: {
+        curr_state = ROBOT_STATE::NULL_STATE;
+        break;
+    }
+    case SDK_BODY_POSTURE_STATE::STARTMOVING:
+    case SDK_BODY_POSTURE_STATE::MOVING:
+    case SDK_BODY_POSTURE_STATE::STANDUP: {
+        curr_state = ROBOT_STATE::STAND_UP;
+        break;
+    }
+    case SDK_BODY_POSTURE_STATE::LIEDOWN: {
+        curr_state = ROBOT_STATE::LIE_DOWN;
+        break;
+    }
+    case SDK_BODY_POSTURE_STATE::CHARGING: {
+        curr_state = ROBOT_STATE::NULL_STATE;
+        break;
+    }
+    default: {
+        curr_state = ROBOT_STATE::NULL_STATE;
+        break;
+    }
   }
+  auto robot_state = std_msgs::msg::UInt32();
+  robot_state.data = static_cast<uint32_t>(curr_state);
   robot_state_publisher_->publish(robot_state);
 }
 
 void SaturnController::publishDockingState() {
-  auto pub_dock_state = std_msgs::msg::UInt32();
-  if (common_status_data_.charge_status.charge_switch_state ==
-      robot_control::common::CHARGE_SWITCH_STATE::NULL_CHARGE_SWITCH) {
-    pub_dock_state.data = 0;
-  } else if (common_status_data_.charge_status.charge_switch_state ==
-             robot_control::common::CHARGE_SWITCH_STATE::ENTERING_TILE) {
-    pub_dock_state.data = 1;
-  } else if (common_status_data_.charge_status.charge_switch_state ==
-             robot_control::common::CHARGE_SWITCH_STATE::ENTER_SUCCEEDED) {
-    pub_dock_state.data = 4;
-  } else if (common_status_data_.charge_status.charge_switch_state ==
-             robot_control::common::CHARGE_SWITCH_STATE::EXITING_TILE) {
-    pub_dock_state.data = 6;
-  } else if (common_status_data_.charge_status.charge_switch_state ==
-             robot_control::common::CHARGE_SWITCH_STATE::EXIT_SUCCEEDED) {
-    pub_dock_state.data = 0;
+  using SDK_CHARGE_SWITCH_STATE = robot_control::common::CHARGE_SWITCH_STATE;
+  auto curr_state = DOCK_STATE::IDLE;
+
+  switch (common_status_data_.charge_status.charge_switch_state) {
+    case SDK_CHARGE_SWITCH_STATE::NULL_CHARGE_SWITCH: {
+        curr_state = DOCK_STATE::IDLE;
+        break;
+    }
+    case SDK_CHARGE_SWITCH_STATE::EXIT_SUCCEEDED: {
+        curr_state = DOCK_STATE::IDLE;
+        break;
+    }
+    case SDK_CHARGE_SWITCH_STATE::EXITING_TILE: {
+        curr_state = DOCK_STATE::LEAVING_DOCK;
+        break;
+    }
+    case SDK_CHARGE_SWITCH_STATE::ENTER_SUCCEEDED: {
+        curr_state = DOCK_STATE::LIE_ON_DOCKER;
+        break;
+    }
+    case SDK_CHARGE_SWITCH_STATE::ENTERING_TILE: {
+        curr_state = DOCK_STATE::GOING_TO_DOCK;
+        break;
+    }
+    case SDK_CHARGE_SWITCH_STATE::EXIT_FAILED:
+    case SDK_CHARGE_SWITCH_STATE::ENTER_FAILED:
+    default: {
+        curr_state = DOCK_STATE::IDLE;
+        break;
+    }
   }
+  auto pub_dock_state = std_msgs::msg::UInt32();
+  pub_dock_state.data = static_cast<uint32_t>(curr_state);
   docking_state_publisher_->publish(pub_dock_state);
 }
 
 void SaturnController::publishControlMode() {
-  auto nav_or_manual_state = std_msgs::msg::UInt8();
-  if (common_status_data_.joy_mode ==
-      robot_control::common::NAV_OR_JOY_MODE::nav_control) {
-    nav_or_manual_state.data = 1;
-  } else if (common_status_data_.joy_mode ==
-             robot_control::common::NAV_OR_JOY_MODE::joy_control) {
-    nav_or_manual_state.data = 0;
+  using SDK_CONTROL_MODE = robot_control::common::NAV_OR_JOY_MODE;
+  auto curr_mode = CONTROL_MODE::NULL_MODE;
+
+  switch (common_status_data_.joy_mode) {
+  case SDK_CONTROL_MODE::nav_control: {
+    curr_mode = CONTROL_MODE::NAVI_CONTROL;
+    break;
+  }
+  case SDK_CONTROL_MODE::joy_control: {
+    curr_mode = CONTROL_MODE::JOY_CONTROL;
+    break;
+  }
+  default: {
+    curr_mode = CONTROL_MODE::NULL_MODE;
+    break;
   }
+  }
+  auto nav_or_manual_state = std_msgs::msg::UInt8();
+  nav_or_manual_state.data = static_cast<uint8_t>(curr_mode);
   control_mode_publisher_->publish(nav_or_manual_state);
 }
 
 void SaturnController::publishScene() {
-  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 = 3;  // TODO
-  } else if (common_status_data_.cur_scene ==
-             robot_control::common::SCENE_TYPE::WALKING) {
-    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 = 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;  // TODO
-  } else if (common_status_data_.cur_scene ==
-             robot_control::common::SCENE_TYPE::STONE) {
-    gait_scene_state.data = static_cast<decltype(gait_scene_state.data)>(
-        saturn_ros2::Robot_Gait::SLOPE);
-  } else {
-    gait_scene_state.data = 5;  // TODO
+  using SDK_SCENE_TYPE = robot_control::common::SCENE_TYPE;
+  auto curr_scene = Robot_Gait::TROT;
+  switch (common_status_data_.cur_scene) {
+  case SDK_SCENE_TYPE::NULL_SCENE: {
+    curr_scene = Robot_Gait::NULL_MODE;
+    break;
+  }
+  case SDK_SCENE_TYPE::LIE_DOWN: {
+    curr_scene = Robot_Gait::NULL_MODE;
+    break;
+  }
+  case SDK_SCENE_TYPE::WALKING: {
+    curr_scene = Robot_Gait::TROT;
+    break;
+  }
+  case SDK_SCENE_TYPE::STAIRS: {
+    curr_scene = Robot_Gait::NORMAL_STAIR;
+    break;
+  }
+  case SDK_SCENE_TYPE::CHARGE: {
+    curr_scene = Robot_Gait::NULL_MODE;
+    break;
   }
+  case SDK_SCENE_TYPE::STONE: {
+    curr_scene = Robot_Gait::SLOPE;
+    break;
+  }
+  case SDK_SCENE_TYPE::PERCEIVE_STAIRS: {
+    curr_scene = Robot_Gait::SENSE_STAIR;
+    break;
+  }
+  case SDK_SCENE_TYPE::SNOW:
+  case SDK_SCENE_TYPE::SLIPPY:
+  default: {
+    curr_scene = Robot_Gait::NULL_MODE;
+    break;
+  }
+  }
+  // if (common_status_data_.cur_scene ==
+  //     robot_control::common::SCENE_TYPE::LIE_DOWN) {
+  //   gait_scene_state.data = 3; // TODO
+  // } else if (common_status_data_.cur_scene ==
+  //            robot_control::common::SCENE_TYPE::WALKING) {
+  //   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 = 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; // TODO
+  // } else if (common_status_data_.cur_scene ==
+  //            robot_control::common::SCENE_TYPE::STONE) {
+  //   gait_scene_state.data = static_cast<decltype(gait_scene_state.data)>(
+  //       saturn_ros2::Robot_Gait::SLOPE);
+  // } else {
+  //   gait_scene_state.data = 5; // TODO
+  // }
+  auto gait_scene_state = std_msgs::msg::UInt8();
+  gait_scene_state.data = static_cast<uint8_t>(curr_scene);
   scene_publisher_->publish(gait_scene_state);
 }
 
-void SaturnController::publishBatteryState() {  // todo
+void SaturnController::publishBatteryState() { // todo
   float battery_power = 0.0;
   battery_power = common_status_data_.charge_status.battery_info.level / 100;
   if (battery_power < 0) {
@@ -617,5 +701,5 @@ void SaturnController::publishLeggedOdom() {
   legged_odom_publisher_->publish(odometry_data);
 }
 
-}  // namespace saturn_ros2
+} // namespace saturn_ros2
 RCLCPP_COMPONENTS_REGISTER_NODE(saturn_ros2::SaturnController)