recharge

src/wheelchair_state_machine/include/wheelchair_state_machine/rotation_manager.hpp

@@ -36,8 +36,9 @@ private:
     // ROS发布者
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr rotation_timeout_pub_;
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub1_;
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub2_;
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub1_;//对外
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub2_;//对外
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;
     // 定时器
     rclcpp::TimerBase::SharedPtr rotation_timer_;
     rclcpp::TimerBase::SharedPtr rotation_timeout_timer_;

src/wheelchair_state_machine/src/event_input.cpp

@@ -103,6 +103,7 @@ void EventInputHandler::setupModuleCallbacks()
             }
             else if (current_state == WheelchairState::CHARGING)
             {
+                stopNavigationControl();
                 lidar_controller_->stopMotion();
                 lidar_controller_->setChargingVoltage(true);
             }

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -69,7 +69,6 @@ void LidarScanController::initialize()
         "/charging/voltage_detected", 10);
     pub1_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/status1",10);
     pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
-
     // 初始化控制消息
     ctrl_twist_.linear.x = 0.0;
     ctrl_twist_.angular.z = 0.0;
@@ -407,6 +406,7 @@ void LidarScanController::procData(RechargeResult &result)
 
 void LidarScanController::controlRechargeMotion(const RechargeResult &result)
 {
+    if (!active_) return;
     publishStationDetected(true);
     bool in_rotation_delay = node_->get_parameter("in_rotation_delay").as_bool();
     if (in_rotation_delay)
@@ -432,6 +432,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         ctrl_twist_.linear.x = 0.0;
         ctrl_twist_.angular.z = 0.0;
         ctrl_twist_time_ = node_->now();
+        stopNavigation();
         return;
     }
 
@@ -506,7 +507,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
                 rclcpp::Time adjust_start = node_->now();
                 int publish_count = 0;
 
-                while ((node_->now() - adjust_start).seconds() < adjust_duration)
+                while (rclcpp::ok() && active_ &&(node_->now() - adjust_start).seconds() < adjust_duration)
                 {
                     pub_ctrl_->publish(adjust_cmd);
                     publish_count++;
@@ -540,7 +541,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
             rclcpp::Time forward_start = node_->now();
             int forward_count = 0;
 
-            while ((node_->now() - forward_start).seconds() < forward_duration)
+            while (rclcpp::ok() && active_ &&(node_->now() - forward_start).seconds() < forward_duration)
             {
                 pub_ctrl_->publish(forward_cmd);
                 forward_count++;
@@ -648,6 +649,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         auto msg =std_msgs::msg::Int32();
         msg.data=RECHARGE_FEEDBACK_SUCCESS_DOCKED;
         pub2_->publish(msg);
+        stopNavigation();
 
         // 模拟检测到充电电压
         setChargingVoltage(true);
@@ -1317,9 +1319,10 @@ bool LidarScanController::getChargingVoltage() const
     auto diff = now - detect_charging_voltage_time_;
 
     // 如果检测到充电电压且在0.1秒内，认为有效
-    if (detect_charging_voltage_ && diff.seconds() < 0.1)
-    {
-        return true;
-    }
-    return false;
+    // if (detect_charging_voltage_ && diff.seconds() < 0.1)
+    // {
+    //     return true;
+    // }
+    // return false;
+    return detect_charging_voltage_; 
 }

src/wheelchair_state_machine/src/rotation_manager.cpp

@@ -5,7 +5,7 @@
 using namespace std::chrono_literals;
 
 RotationManager::RotationManager(rclcpp::Node *node)
-    : node_(node), is_rotating_(false), rotation_timeout_(40.0) // 旋转20秒超时
+    : node_(node), is_rotating_(false), rotation_timeout_(40.0) // 旋转40秒超时
       ,
       angular_velocity_(0.15) // 角速度0.3 rad/s
       ,