[fix] 移除停障开关以确保业务逻辑正常

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -111,8 +111,8 @@ class SaturnController {
   rclcpp::Publisher<UInt32>::SharedPtr docking_state_publisher_;
 
   enum class CONTROL_MODE : uint8_t {
-    JOY_CONTROL = 0,  //  0 -> 手持遥控
-    NAVI_CONTROL = 1, //  1 -> 导航
+    JOY_CONTROL = 0,   //  0 -> 手持遥控
+    NAVI_CONTROL = 1,  //  1 -> 导航
     NULL_MODE = 2,
   };
   rclcpp::Publisher<UInt8>::SharedPtr control_mode_publisher_;
@@ -142,10 +142,6 @@ class SaturnController {
   rclcpp::CallbackGroup::SharedPtr robot_command_service_cgb_;
   rclcpp::Service<RobotCommand>::SharedPtr robot_command_service_;
 
-  rclcpp::CallbackGroup::SharedPtr enable_guardian_cbg_;
-  rclcpp::Service<SetBool>::SharedPtr enable_guardian_service_;
-  std::atomic<bool> enable_guardian_;
-
   // 用于导航停障中的服务接口
   rclcpp::CallbackGroup::SharedPtr guardian_service_cgb_;
   rclcpp::Service<GuardianSrv>::SharedPtr guardian_service_;

saturn_controller/src/saturn_controller.cpp

@@ -1,9 +1,10 @@
 #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) {
+      guardian_velocity_decay_ratio_(2.0) {
   SPDLOG_INFO("====== SaturnController created ======");
   config_handler_ = std::make_shared<ConfigHandler>(node_);
   init();
@@ -59,27 +60,6 @@ void SaturnController::init() {
       std::bind(&SaturnController::cmdVelCallback, this, std::placeholders::_1),
       cmd_vel_sub_options);
 
-  enable_guardian_cbg_ = node_->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive);
-  enable_guardian_service_ = node_->create_service<SetBool>(
-      "enable_guardian",
-      [this](const SetBool::Request::SharedPtr request,
-             SetBool::Response::SharedPtr respond) {
-        if (request->data != this->enable_guardian_.load()) {
-          this->enable_guardian_.store(request->data);
-          const auto &info =
-              request->data ? "Enable guardian !!!" : "Disable guardian !!!";
-          SPDLOG_WARN("############### {} ###############", info);
-        } else {
-          std::string info = "Guardian is already ";
-          info += request->data ? "Enabled" : "Disabled";
-          SPDLOG_WARN("############### {} ###############", info);
-        }
-        respond->success = true;
-        return;
-      },
-      rclcpp::ServicesQoS().get_rmw_qos_profile(), enable_guardian_cbg_);
-
   guardian_callback_cg_ = node_->create_callback_group(
       rclcpp::CallbackGroupType::MutuallyExclusive);
 
@@ -154,12 +134,9 @@ void SaturnController::cmdVelCallback(const Twist::ConstSharedPtr vel_msg) {
                                       : nominal_velocity_decay_ratio < 0.0f
                                           ? 0.0f
                                           : nominal_velocity_decay_ratio;
-  if (!enable_guardian_.load()) {
-    actual_velocity_decay_ratio = 1.0f;
-  }
   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,40 +441,40 @@ void SaturnController::leggedOodmTimerCallback() {
   // 先把足式里程计放在主callback里发布，如果对不同频率有需求，再重新使用这里
 }
 
-void SaturnController::publishRobotState() { // todo 这里逻辑还不完善
+void SaturnController::publishRobotState() {  // todo 这里逻辑还不完善
   using SDK_SCENE_TYPE = robot_control::common::SCENE_TYPE;
   auto curr_state = ROBOT_STATE::LIE_DOWN;
 
   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;
+    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;
     }
-    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();
@@ -510,32 +487,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);
@@ -547,18 +524,18 @@ void SaturnController::publishControlMode() {
   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;
-  }
+    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);
@@ -569,47 +546,47 @@ void SaturnController::publishGait() {
   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::TROT;
-    break;
-  }
-  case SDK_SCENE_TYPE::LIE_DOWN: {
-    curr_scene = Robot_Gait::TROT;
-    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::TROT;
-    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::TROT;
-    break;
-  }
+    case SDK_SCENE_TYPE::NULL_SCENE: {
+      curr_scene = Robot_Gait::TROT;
+      break;
+    }
+    case SDK_SCENE_TYPE::LIE_DOWN: {
+      curr_scene = Robot_Gait::TROT;
+      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::TROT;
+      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::TROT;
+      break;
+    }
   }
   auto gait_scene_state = std_msgs::msg::UInt8();
   gait_scene_state.data = static_cast<uint8_t>(curr_scene);
   gait_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) {
@@ -688,5 +665,5 @@ void SaturnController::publishLeggedOdom() {
   legged_odom_publisher_->publish(odometry_data);
 }
 
-} // namespace saturn_ros2
+}  // namespace saturn_ros2
 RCLCPP_COMPONENTS_REGISTER_NODE(saturn_ros2::SaturnController)