recharge

src/wheelchair_state_machine/include/wheelchair_state_machine/kaiguan.hpp

@@ -0,0 +1,6 @@
+#ifndef KAIGUAN_HPP
+#define KAIGUAN_HPP
+
+// #define  DEBUG_LEVEL 1
+#define DEBUG_LEVEL 0
+#endif // KAIGUAN_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -16,6 +16,7 @@
 #include<std_msgs/msg/string.hpp>
 #include"recharge_status.hpp"
 #include<sstream>
+#include "kaiguan.hpp"
 class LidarScanController
 {
 public:

src/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -16,6 +16,7 @@
 #include <std_msgs/msg/bool.hpp>
 #include<std_msgs/msg/int32.hpp>
 #include "recharge_status.hpp"
+#include "kaiguan.hpp"
 
 class WorkflowManager
 {

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -451,7 +451,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
     RCLCPP_INFO(node_->get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
                 yaw_diff, yaw_diff * 180.0 / M_PI);
 
-    if (checkStationAlignment(dx, dy, yaw_diff))
+    if (checkStationAlignment(dx, dy, yaw_diff)&&DEBUG_LEVEL)
     {
         // 停止行车中，准备向前移动，再校准回充
         RCLCPP_WARN(node_->get_logger(), "===== 开始对准误差调整 =====");
@@ -470,7 +470,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         double adjust_angular = 0.0;
         std::string adjust_type;
 
-        const double ANGLE_8DEG = 5.0 * M_PI / 180.0; // 5度阈值
+        const double ANGLE_8DEG = 1.0 * M_PI / 180.0; // 5度阈值
                                                       // const double DY_THRESHOLD = 0.05;             // 5cm阈值
 
         // 优先调整角度
@@ -478,7 +478,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         {
             adjust_type = "角度调整";
             // 根据夹角大小计算调整时间
-            adjust_duration = fabs(yaw_diff) / 0.15; // 角速度0.25 rad/s
+            adjust_duration = fabs(yaw_diff)+1; // 角速度0.25 rad/s
             adjust_angular = (yaw_diff > 0) ? -0.15 : 0.15;
 
             RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒", adjust_duration);
@@ -668,10 +668,10 @@ bool LidarScanController::checkStationAlignment(double dx, double dy, double yaw
                     dx, yaw_diff * 180.0 / M_PI);
 
         // 判断是否对准（夹角小于10度）
-        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD)
+        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD&&std::fabs(dy)<=0.05)
         {
-            RCLCPP_INFO(node_->get_logger(), "✓ 已对准基站，继续行驶 - 夹角: %.2f°,dx=%.2f,dy=%.2f",
-                        yaw_diff * 180.0 / M_PI, dx, dy);
+            RCLCPP_INFO(node_->get_logger(), "✓ 已对准基站，继续行驶 - 夹角: %.2f°,dx=%.2f,dy=%.2f,yaw_diff=%.2f",
+                        yaw_diff * 180.0 / M_PI, dx, dy,yaw_diff);
 
             RCLCPP_DEBUG(node_->get_logger(), "对准行驶 - 线速度: %.4f", ctrl_twist_.linear.x);
         }

src/wheelchair_state_machine/src/workflow.cpp

@@ -38,25 +38,25 @@ void WorkflowManager::initializeSubscriptions()
         {
             this->stationDetectedCallback(msg);
         });
-    rotation_timeout_sub_ = node_->create_subscription<std_msgs::msg::String>(
-        "/wheelchair/rotation_timeout", 10,
-        [this](const std_msgs::msg::String::SharedPtr msg)
-        {
-            if (msg->data == "rotation_timeout")
-            {
-                RCLCPP_INFO(node_->get_logger(), "收到旋转超时信号，返回就绪状态");
-                
-                // 如果当前是旋转状态，处理超时
-                if (current_state_ == WheelchairState::ROTATING)
-                {
-                    // 发布回充失败通知
-                    publishRechargeStatus("RECHARGE_FAILED", "旋转搜索超时，未检测到基站");
-                    
-                    // 返回就绪状态
-                    transitionToReady();
-                }
-            }
-        });
+    // rotation_timeout_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    //     "/wheelchair/rotation_timeout", 10,
+    //     [this](const std_msgs::msg::String::SharedPtr msg)
+    //     {
+    //         if (msg->data == "rotation_timeout")
+    //         {
+    //             RCLCPP_INFO(node_->get_logger(), "收到旋转超时信号，返回就绪状态");
+
+    //             // 如果当前是旋转状态，处理超时
+    //             if (current_state_ == WheelchairState::ROTATING)
+    //             {
+    //                 // 发布回充失败通知
+    //                 publishRechargeStatus("RECHARGE_FAILED", "旋转搜索超时，未检测到基站");
+
+    //                 // 返回就绪状态
+    //                 transitionToReady();
+    //             }
+    //         }
+    //     });
 
     RCLCPP_INFO(node_->get_logger(), "基站检测订阅者已初始化");
     node_->declare_parameter<bool>("in_rotation_delay", false);
@@ -75,26 +75,26 @@ void WorkflowManager::stationDetectedCallback(const std_msgs::msg::Bool::SharedP
                      current_state_str.c_str());
 
         // 处理旋转中检测到基站的情况
-        if (station_detected && current_state_ == WheelchairState::ROTATING)
-        {
-            RCLCPP_INFO(node_->get_logger(),
-                        "旋转中检测到基站，记录检测时间并设置延迟切换");
+        // if (station_detected && current_state_ == WheelchairState::ROTATING)
+        // {
+        //     RCLCPP_INFO(node_->get_logger(),
+        //                 "旋转中检测到基站，记录检测时间并设置延迟切换");
 
-            // 记录检测时间和状态
-            station_detected_while_rotating_ = true;
-            station_detected_time_ = node_->now();
-            // 通知激光控制器进入旋转延迟模式
-            node_->set_parameter(rclcpp::Parameter("in_rotation_delay", true));
+        //     // 记录检测时间和状态
+        //     station_detected_while_rotating_ = true;
+        //     station_detected_time_ = node_->now();
+        //     // 通知激光控制器进入旋转延迟模式
+        //     node_->set_parameter(rclcpp::Parameter("in_rotation_delay", true));
 
-            // 启动延迟定时器（2秒后切换状态）
-            startRotationDelayTimer();
+        //     // 启动延迟定时器（2秒后切换状态）
+        //     startRotationDelayTimer();
 
-            // 发布状态信息
-            publishState("STATION_DETECTED_WHILE_ROTATING",
-                         "旋转中检测到基站，继续旋转2秒后对接");
+        //     // 发布状态信息
+        //     publishState("STATION_DETECTED_WHILE_ROTATING",
+        //                  "旋转中检测到基站，继续旋转2秒后对接");
 
-            return; // 直接返回，不立即切换状态
-        }
+        //     return; // 直接返回，不立即切换状态
+        // }
 
         // 搜索中检测到基站，立即切换状态（保持原有逻辑）
         if (station_detected && current_state_ == WheelchairState::SEARCHING)
@@ -778,8 +778,8 @@ void WorkflowManager::transitionToSearching()
             RCLCPP_INFO(node_->get_logger(), "开始搜索充电站");
             publishState("SEARCH_CHARGING_STATION_START");
             startSearchTimeoutTimer();
-            startRotationTimer();
-            // 发布回充开始通知
+            // startRotationTimer();
+            //  发布回充开始通知
             publishRechargeStatus("RECHARGE_STARTED", "回充功能已启动，正在搜索充电站");
         }
     }
@@ -1278,20 +1278,25 @@ void WorkflowManager::handleSearchTimeoutEvent()
 {
     try
     {
-        RCLCPP_INFO(node_->get_logger(), "处理搜索超时事件");
-
-        // 记录当前状态，便于调试
-        std::string current_state_str = getCurrentState();
-        RCLCPP_INFO(node_->get_logger(), "搜索超时发生时的状态: %s", current_state_str.c_str());
-
-        // 搜索超时30秒视为回充失败
-        RCLCPP_WARN(node_->get_logger(), "搜索30秒超时，回充失败");
+        if (DEBUG_LEVEL)
+        {
+            RCLCPP_INFO(node_->get_logger(), "转入旋转搜索");
+            // 进入旋转流程
+            transitionToRotating();
+        }
+        else
+        {
+            // 记录当前状态，便于调试
+            std::string current_state_str = getCurrentState();
+            RCLCPP_INFO(node_->get_logger(), "搜索超时发生时的状态: %s", current_state_str.c_str());
 
-        // 发布回充失败通知
-        publishRechargeStatus("RECHARGE_FAILED", "搜索充电站秒超时");
+            // 搜索超时30秒视为回充失败
+            RCLCPP_WARN(node_->get_logger(), "搜索超时，回充失败");
 
-        // 退出回充流程
-        transitionToReady();
+            // 发布回充失败通知
+            publishRechargeStatus("RECHARGE_FAILED", "搜索充电站秒超时");
+            transitionToReady();
+        }
     }
     catch (const std::exception &e)
     {
@@ -1467,9 +1472,9 @@ void WorkflowManager::startSearchTimeoutTimer()
             search_timeout_timer_->cancel();
         }
 
-        // 创建30秒定时器
+        // 创建10秒定时器
         search_timeout_timer_ = node_->create_wall_timer(
-            std::chrono::seconds(30),
+            std::chrono::seconds(10),
             [this]()
             {
                 this->searchTimeoutCallback();
@@ -1527,14 +1532,13 @@ void WorkflowManager::searchTimeoutCallback()
 {
     try
     {
-        // 只在搜索状态下处理搜索超时，旋转状态由RotationManager处理
         if (current_state_ == WheelchairState::SEARCHING)
         {
             double total_search_time = (node_->now() - search_start_time_).seconds();
 
-            RCLCPP_WARN(node_->get_logger(),
-                        "搜索 %.1f 秒超时，回充失败",
-                        total_search_time);
+            // RCLCPP_WARN(node_->get_logger(),
+            //             "搜索 %.1f 秒超时，回充失败",
+            //             total_search_time);
             handleSearchTimeoutEvent();
         }
         else if (current_state_ == WheelchairState::ROTATING)