recharge

src/recharge/include/kaiguan.hpp

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

src/recharge/include/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/recharge/include/workflow.hpp

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

src/recharge/src/lidascan_ctrl.cpp

@@ -493,7 +493,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(), "===== 开始对准误差调整 =====");

src/recharge/src/workflow.cpp

@@ -778,7 +778,7 @@ 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", "搜索充电站秒超时");
+            // 搜索超时10秒视为回充失败
+            RCLCPP_WARN(node_->get_logger(), "搜索超时，回充失败");
 
-        // 退出回充流程
-        transitionToReady();
+            // 发布回充失败通知
+            publishRechargeStatus("RECHARGE_FAILED", "搜索充电站秒超时");
+            transitionToReady();
+        }
     }
     catch (const std::exception &e)
     {
@@ -1467,15 +1472,15 @@ 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();
             });
 
-        RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已启动 (30秒)");
+        RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已启动 (10秒)");
     }
     catch (const std::exception &e)
     {
@@ -1531,10 +1536,6 @@ void WorkflowManager::searchTimeoutCallback()
         if (current_state_ == WheelchairState::SEARCHING)
         {
             double total_search_time = (node_->now() - search_start_time_).seconds();
-
-            RCLCPP_WARN(node_->get_logger(),
-                        "搜索 %.1f 秒超时，回充失败",
-                        total_search_time);
             handleSearchTimeoutEvent();
         }
         else if (current_state_ == WheelchairState::ROTATING)