[fix] 增加/robot_state话题中，判断静止或移动的逻辑

移除不合规的NULL_STATE状态

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -94,7 +94,6 @@ class SaturnController {
     STEPPING = 4,
     SITTING_DOWN = 5,
     FALL_DOWN = 6,
-    NULL_STATE = 7
   };
   rclcpp::Publisher<UInt32>::SharedPtr robot_state_publisher_;
 

saturn_controller/src/saturn_controller.cpp

@@ -465,36 +465,41 @@ void SaturnController::leggedOodmTimerCallback() {
 }
 
 void SaturnController::publishRobotState() { // todo 这里逻辑还不完善
-  using SDK_BODY_POSTURE_STATE = common::BODY_POSTURE_STATE;
-  auto curr_state = ROBOT_STATE::NULL_STATE;
+  using SDK_SCENE_TYPE = robot_control::common::SCENE_TYPE;
+  auto curr_state = ROBOT_STATE::LIE_DOWN;
 
-  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: {
-        curr_state = ROBOT_STATE::STEPPING;
-        break;
-    }
-    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;
+  switch (common_status_data_.cur_scene) {
+  case SDK_SCENE_TYPE::NULL_SCENE: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
+  case SDK_SCENE_TYPE::LIE_DOWN: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
+  case SDK_SCENE_TYPE::WALKING:
+  case SDK_SCENE_TYPE::STONE:
+  case SDK_SCENE_TYPE::PERCEIVE_STAIRS:
+  case SDK_SCENE_TYPE::SNOW:
+  case SDK_SCENE_TYPE::SLIPPY:
+  case SDK_SCENE_TYPE::STAIRS: {
+    if (common_status_data_.cur_speed >= 0.05) {
+      curr_state = ROBOT_STATE::STEPPING;
+    } else {
+      curr_state = ROBOT_STATE::STAND_UP;
     }
+    break;
+  }
+  case SDK_SCENE_TYPE::CHARGE: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
+  default: {
+    curr_state = ROBOT_STATE::LIE_DOWN;
+    break;
+  }
   }
+
   auto robot_state = std_msgs::msg::UInt32();
   robot_state.data = static_cast<uint32_t>(curr_state);
   robot_state_publisher_->publish(robot_state);
@@ -505,32 +510,32 @@ void SaturnController::publishDockingState() {
   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;
-    }
+  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);
@@ -599,27 +604,6 @@ void SaturnController::publishGait() {
     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);
   gait_publisher_->publish(gait_scene_state);