wheelchair_state_machine

src3/wheelchair_state_machine/CMakeLists.txt

@@ -26,6 +26,7 @@ add_executable(wheelchair_state_machine_node
   src/lidascan_ctrl.cpp
   src/recharge_tool.cpp
   src/report.cpp
+  src/rotation_manager.cpp
 )
 
 # 设置头文件路径
@@ -44,15 +45,9 @@ ament_target_dependencies(wheelchair_state_machine_node
   tf2_geometry_msgs
 )
 
-ament_target_dependencies(test_error_publisher
-  rclcpp
-  std_msgs
-)
-
 # 安装规则
 install(TARGETS
   wheelchair_state_machine_node
-  test_error_publisher
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 

src3/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -17,29 +17,29 @@ class LidarScanController
 public:
     using LaserScan = sensor_msgs::msg::LaserScan;
     using LaserScanSharedPtr = sensor_msgs::msg::LaserScan::SharedPtr;
-    
-    LidarScanController(rclcpp::Node* node);
+
+    LidarScanController(rclcpp::Node *node);
     ~LidarScanController();
-    
+
     // 初始化
     void initialize();
-    
+
     // 导航控制接口（外部调用）
     void startNavigation();
     void stopNavigation();
     void stopMotion();
-    
+
     // 充电电压检测接口
     void setChargingVoltage(bool value);
     bool getChargingVoltage() const;
-    
+
     // 状态查询
     bool isActive() const { return active_; }
     ChargingState getChargingState() const { return charging_state_; }
-    
+
 private:
-    rclcpp::Node* node_;
-    
+    rclcpp::Node *node_;
+
     // ROS2 订阅者和发布者
     rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_ctrl_;
@@ -47,74 +47,74 @@ private:
     rclcpp::TimerBase::SharedPtr navigation_timer_;
     rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
     rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;
-    
+
     // 状态变量
     bool active_;
     ChargingState charging_state_;
     geometry_msgs::msg::Twist ctrl_twist_;
     rclcpp::Time ctrl_twist_time_;
     rclcpp::Time detect_station_time_;
-    
+
     // 参数
     ScanFilterParam param_;
     double distance_threshold_;
     int fit_window_size_;
     double fit_residual_threshold_;
     int frame_index_;
-    
+
     // 充电检测
     bool detect_charging_voltage_;
     rclcpp::Time detect_charging_voltage_time_;
-    
+
     // ========== 核心算法函数 ==========
-    
+
     // 激光数据处理
-    void processLaserScan(const LaserScanSharedPtr& msg);
-    bool processSegmentData(const LaserScanSharedPtr& msg, int begin_index, int end_index);
-    void procData(RechargeResult& result);
-    
+    void processLaserScan(const LaserScanSharedPtr &msg);
+    bool processSegmentData(const LaserScanSharedPtr &msg, int begin_index, int end_index);
+    void procData(RechargeResult &result);
+
     // 运动控制算法
-    void controlRechargeMotion(const RechargeResult& result);
-    
+    void controlRechargeMotion(const RechargeResult &result);
+
     // 过滤算法
     void filterScanModify0(LaserScanSharedPtr msg);
     void filterScanModify1(LaserScanSharedPtr msg);
     void filterScanModify2(LaserScanSharedPtr msg);
     void filterScanModify3(LaserScanSharedPtr msg);
     std::pair<int, int> filterScanModify4(LaserScanSharedPtr msg);
-    
+
     // 坐标转换
-    std::map<int, std::pair<double, double>> laserScanToXY(const LaserScanSharedPtr& scan_msg);
+    std::map<int, std::pair<double, double>> laserScanToXY(const LaserScanSharedPtr &scan_msg);
     std::map<int, std::pair<double, double>> laserScanToXY1(int begin_index, int end_index,
-                                                           const LaserScanSharedPtr& scan_msg);
-    
+                                                            const LaserScanSharedPtr &scan_msg);
+
     // 分段处理算法
     std::vector<std::pair<int, int>> getSections(
         int begin_index, int end_index,
-        const LaserScanSharedPtr& msg,
-        const std::map<int, std::pair<double, double>>& xy_list);
-    
-    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> 
-    getSegments(const std::map<int, std::pair<double, double>>& xy_points_map);
-    
-    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> 
-    filterSegments(const std::map<int, std::tuple<int, int, double, double, double, double, double, int>>& turn_segments);
-    
+        const LaserScanSharedPtr &msg,
+        const std::map<int, std::pair<double, double>> &xy_list);
+
+    std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
+    getSegments(const std::map<int, std::pair<double, double>> &xy_points_map);
+
+    std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
+    filterSegments(const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments);
+
     // 可视化
-    void publishRechargeVisualization(const RechargeResult& result);
-    
+    void publishRechargeVisualization(const RechargeResult &result);
+
     // ========== 辅助函数 ==========
-    void publishFilteredScan(const LaserScanSharedPtr& original_msg,
-                            const std::vector<float>& filtered_ranges);
+    void publishFilteredScan(const LaserScanSharedPtr &original_msg,
+                             const std::vector<float> &filtered_ranges);
     LaserScanSharedPtr resetLaserData(LaserScanSharedPtr msg, int b_index, int e_index);
     bool isValid(float value) const;
-    int getFixedIndex(const LaserScanSharedPtr& msg, int i) const;
+    int getFixedIndex(const LaserScanSharedPtr &msg, int i) const;
     int getFixed() const { return 0; }
-    Eigen::MatrixXd getPointList(const std::vector<IndexPoint>& points);
-    
+    Eigen::MatrixXd getPointList(const std::vector<IndexPoint> &points);
+
     // 定时器回调
     void navigationTimerCallback();
-    
+
     // 控制发布
     void publishControl();
 };

src3/wheelchair_state_machine/include/wheelchair_state_machine/rotation_manager.hpp

@@ -0,0 +1,58 @@
+// rotation_manager.hpp
+#ifndef ROTATION_MANAGER_HPP
+#define ROTATION_MANAGER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <memory>
+
+class RotationManager
+{
+public:
+    RotationManager(rclcpp::Node *node);
+
+    // 核心接口
+    void startRotation();
+    void stopRotation();
+
+    // 状态查询
+    bool isRotating() const { return is_rotating_; }
+
+    // 定时器回调
+    void rotationTimerCallback();
+
+    // 获取旋转持续时间
+    double getRotationDuration() const;
+
+    // 检查是否需要停止旋转（旋转超时）
+    bool shouldStopRotation() const;
+
+private:
+    rclcpp::Node *node_;
+
+    // ROS发布者
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
+
+    // 定时器
+    rclcpp::TimerBase::SharedPtr rotation_timer_;
+    rclcpp::TimerBase::SharedPtr rotation_timeout_timer_;
+
+    // 状态变量
+    bool is_rotating_;
+    rclcpp::Time rotation_start_time_;
+    double rotation_timeout_; // 旋转超时时间（秒）
+
+    // 控制参数
+    double angular_velocity_; // 角速度
+    double linear_velocity_;  // 线速度
+
+    // 发布旋转控制命令
+    void publishRotationCommand();
+
+    // 记录日志
+    void logInfo(const std::string &message);
+    void logWarning(const std::string &message);
+    void logError(const std::string &message);
+};
+
+#endif // ROTATION_MANAGER_HPP

src3/wheelchair_state_machine/include/wheelchair_state_machine/wheelchair_types.hpp

@@ -11,6 +11,7 @@ enum class WheelchairState
 {
     READY,     // 就绪中
     WALKING,   // 行走中
+    ROTATING,  // 旋转中
     SEARCHING, // 搜索中
     CHARGING   // 充电中
 };
@@ -28,11 +29,11 @@ enum class ChargingState
 // 电池状态枚举
 enum class BatteryState
 {
-    NORMAL,      // 正常
-    LOW,         // 低电量
-    CRITICAL,    // 严重低电量
-    CHARGING,    // 充电中
-    FULL         // 充满
+    NORMAL,   // 正常
+    LOW,      // 低电量
+    CRITICAL, // 严重低电量
+    CHARGING, // 充电中
+    FULL      // 充满
 };
 
 // ==================== 数据结构 ====================

src3/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -12,6 +12,7 @@
 #include <std_msgs/msg/string.hpp>
 #include <std_msgs/msg/u_int32.hpp>
 #include <rclcpp/timer.hpp>
+#include "rotation_manager.hpp" // 新增头文件
 
 class WorkflowManager
 {
@@ -37,12 +38,17 @@ public:
     // 标准状态转移
     void transitionToWalking();
     void transitionToSearching();
+    void transitionToRotating();
     void transitionToCharging();
     void transitionToReady();
 
     // 紧急状态转移
     void emergencyTransitionToReady(const std::string &reason = "未知原因");
 
+    // ==================== 旋转管理接口 ====================
+    void startRotationTimer();      // 新增
+    void checkRotationTransition(); // 新增
+
     // ==================== 回调设置接口 ====================
 
     void setStateUpdateCallback(StateUpdateCallback callback);
@@ -53,6 +59,7 @@ public:
 
     void startSearchTimeoutTimer();
     void stopSearchTimeoutTimer();
+    void stopRotationTimer();
 
     // ==================== 事件处理接口 ====================
 
@@ -75,12 +82,17 @@ private:
     WheelchairState current_state_;
     std::map<std::string, std::map<WheelchairState, bool>> transition_table_;
 
+    // 旋转管理器
+    std::unique_ptr<RotationManager> rotation_manager_;
+
     // ROS发布者
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr state_publisher_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr recharge_timeout_publisher_;
 
     // 定时器
     rclcpp::TimerBase::SharedPtr search_timeout_timer_;
+    rclcpp::TimerBase::SharedPtr rotation_check_timer_; // 新增
+    rclcpp::Time search_start_time_;                    // 新增
 
     // 回调函数
     StateUpdateCallback state_update_callback_;
@@ -91,6 +103,7 @@ private:
 
     // 初始化方法
     void initializeTransitionTable();
+    void initializeRotationManager();
 
     // 事件权限检查
     bool checkEventPermission(const std::string &event);
@@ -132,6 +145,6 @@ private:
     // ==================== 定时器回调 ====================
 
     void searchTimeoutCallback();
+    void rotationCheckCallback();
 };
-
-#endif // WORKFLOW_HPP
+#endif

src3/wheelchair_state_machine/src/event_input.cpp

@@ -26,7 +26,6 @@ void EventInputHandler::initializeModules()
 
     // 初始化工作流管理器
     workflow_manager_ = std::make_unique<WorkflowManager>(node_);
-
     // 初始化报告管理器
     report_manager_ = std::make_unique<ReportManager>(node_);
 
@@ -91,6 +90,12 @@ void EventInputHandler::setupModuleCallbacks()
             {
                 startNavigationControl();
             }
+            else if (current_state == WheelchairState::ROTATING) // 新增：旋转状态
+            {
+                // 旋转状态由旋转管理器控制，不需要启动导航控制
+                // 可以添加其他处理逻辑
+                RCLCPP_INFO(node_->get_logger(), "进入旋转搜索状态");
+            }
             else if (current_state == WheelchairState::CHARGING)
             {
                 lidar_controller_->stopMotion();

src3/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -1059,4 +1059,4 @@ bool LidarScanController::getChargingVoltage() const
         return true;
     }
     return false;
-}
+}

src3/wheelchair_state_machine/src/report.cpp

@@ -497,6 +497,8 @@ std::string ReportManager::wheelchairStateToString(WheelchairState state) const
         return "就绪中";
     case WheelchairState::WALKING:
         return "行走中";
+    case WheelchairState::ROTATING:
+        return "旋转中";
     case WheelchairState::SEARCHING:
         return "搜索中";
     case WheelchairState::CHARGING:

src3/wheelchair_state_machine/src/rotation_manager.cpp

@@ -0,0 +1,149 @@
+// rotation_manager.cpp
+#include "wheelchair_state_machine/rotation_manager.hpp"
+#include <rclcpp/logging.hpp>
+
+using namespace std::chrono_literals;
+
+RotationManager::RotationManager(rclcpp::Node *node)
+    : node_(node), is_rotating_(false), rotation_timeout_(20.0) // 旋转20秒超时
+      ,
+      angular_velocity_(0.3) // 角速度0.3 rad/s
+      ,
+      linear_velocity_(0.0) // 线速度0 m/s
+{
+    // 创建控制命令发布者
+    cmd_vel_publisher_ = node_->create_publisher<geometry_msgs::msg::Twist>(
+        "/cmd_vel_raw", 10);
+
+    logInfo("旋转管理器初始化完成");
+}
+
+void RotationManager::startRotation()
+{
+    if (is_rotating_)
+    {
+        logWarning("旋转已经在进行中");
+        return;
+    }
+
+    is_rotating_ = true;
+    rotation_start_time_ = node_->now();
+
+    // 创建旋转定时器（100ms周期）
+    rotation_timer_ = node_->create_wall_timer(
+        100ms,
+        [this]()
+        {
+            this->rotationTimerCallback();
+        });
+
+    // 创建旋转超时定时器
+    rotation_timeout_timer_ = node_->create_wall_timer(
+        std::chrono::duration<double>(rotation_timeout_),
+        [this]()
+        {
+            logInfo("旋转超时，自动停止旋转");
+            stopRotation();
+        });
+
+    logInfo("开始旋转 - 角速度: " + std::to_string(angular_velocity_) +
+            " rad/s, 持续时间: " + std::to_string(rotation_timeout_) + "秒");
+}
+
+void RotationManager::stopRotation()
+{
+    if (!is_rotating_)
+    {
+        logWarning("旋转未启动，无需停止");
+        return;
+    }
+
+    // 停止定时器
+    if (rotation_timer_)
+    {
+        rotation_timer_->cancel();
+        rotation_timer_.reset();
+    }
+
+    if (rotation_timeout_timer_)
+    {
+        rotation_timeout_timer_->cancel();
+        rotation_timeout_timer_.reset();
+    }
+
+    // 发送停止命令
+    geometry_msgs::msg::Twist stop_cmd;
+    stop_cmd.linear.x = 0.0;
+    stop_cmd.angular.z = 0.0;
+    cmd_vel_publisher_->publish(stop_cmd);
+
+    // 更新状态
+    is_rotating_ = false;
+
+    double duration = getRotationDuration();
+    logInfo("停止旋转 - 持续时间: " + std::to_string(duration) + "秒");
+}
+
+void RotationManager::rotationTimerCallback()
+{
+    if (!is_rotating_)
+    {
+        return;
+    }
+
+    publishRotationCommand();
+}
+
+void RotationManager::publishRotationCommand()
+{
+    geometry_msgs::msg::Twist rotation_cmd;
+    rotation_cmd.linear.x = linear_velocity_;
+    rotation_cmd.angular.z = angular_velocity_;
+
+    cmd_vel_publisher_->publish(rotation_cmd);
+
+    // 每2秒记录一次旋转状态
+    static int log_counter = 0;
+    if (log_counter++ % 20 == 0) // 100ms * 20 = 2秒
+    {
+        double elapsed = (node_->now() - rotation_start_time_).seconds();
+        logInfo("旋转中 - 已旋转: " + std::to_string(elapsed) +
+                "秒, 角速度: " + std::to_string(angular_velocity_) + " rad/s");
+    }
+}
+
+double RotationManager::getRotationDuration() const
+{
+    if (!is_rotating_)
+    {
+        return 0.0;
+    }
+
+    return (node_->now() - rotation_start_time_).seconds();
+}
+
+bool RotationManager::shouldStopRotation() const
+{
+    if (!is_rotating_)
+    {
+        return false;
+    }
+
+    double elapsed = getRotationDuration();
+    return elapsed >= rotation_timeout_;
+}
+
+void RotationManager::logInfo(const std::string &message)
+{
+    RCLCPP_INFO(node_->get_logger(), "[RotationManager] %s", message.c_str());
+}
+
+void RotationManager::logWarning(const std::string &message)
+{
+    RCLCPP_WARN(node_->get_logger(), "[RotationManager] %s", message.c_str());
+}
+
+void RotationManager::logError(const std::string &message)
+{
+    RCLCPP_ERROR(node_->get_logger(), "[RotationManager] %s", message.c_str());
+}

src3/wheelchair_state_machine/src/workflow.cpp

@@ -13,6 +13,9 @@ WorkflowManager::WorkflowManager(rclcpp::Node *node)
     // 初始化状态转移表
     initializeTransitionTable();
 
+    // 初始化旋转管理器
+    initializeRotationManager();
+
     // 创建状态发布者
     state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
         "wheelchair/state", 10);
@@ -24,6 +27,12 @@ WorkflowManager::WorkflowManager(rclcpp::Node *node)
     RCLCPP_INFO(node_->get_logger(), "工作流管理器已初始化");
 }
 
+void WorkflowManager::initializeRotationManager()
+{
+    rotation_manager_ = std::make_unique<RotationManager>(node_);
+    RCLCPP_INFO(node_->get_logger(), "旋转管理器已初始化");
+}
+
 void WorkflowManager::initializeTransitionTable()
 {
     transition_table_.clear();
@@ -32,6 +41,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> ipad_start_perms = {
         {WheelchairState::READY, true},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["ipad_phone_interface_start"] = ipad_start_perms;
@@ -40,6 +50,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> ipad_cancel_perms = {
         {WheelchairState::READY, true},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, true}};
     transition_table_["ipad_phone_interface_cancel"] = ipad_cancel_perms;
@@ -48,6 +59,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> bluetooth_disconnect_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, true}};
     transition_table_["bluetooth_disconnected"] = bluetooth_disconnect_perms;
@@ -56,6 +68,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> bluetooth_connect_perms = {
         {WheelchairState::READY, true},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["bluetooth_connected"] = bluetooth_connect_perms;
@@ -64,6 +77,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> base_power_off_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, true}};
     transition_table_["base_station_power_off"] = base_power_off_perms;
@@ -72,6 +86,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> low_battery_perms = {
         {WheelchairState::READY, true},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["low_battery_warning"] = low_battery_perms;
@@ -80,6 +95,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> lock_vehicle_perms = {
         {WheelchairState::READY, true},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["lock_vehicle"] = lock_vehicle_perms;
@@ -88,6 +104,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> unlock_vehicle_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["unlock_vehicle"] = unlock_vehicle_perms;
@@ -96,6 +113,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> joystick_pull_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, false}};
     transition_table_["joystick_pull_back"] = joystick_pull_perms;
@@ -104,6 +122,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> joystick_stop_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, false}};
     transition_table_["joystick_stop"] = joystick_stop_perms;
@@ -112,6 +131,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> push_start_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, true}};
     transition_table_["push_start"] = push_start_perms;
@@ -120,6 +140,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> push_stop_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, false}};
     transition_table_["push_stop"] = push_stop_perms;
@@ -128,6 +149,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> battery_start_charging_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, true}};
     transition_table_["battery_start_charging"] = battery_start_charging_perms;
@@ -136,6 +158,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> battery_stop_charging_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, true}};
     transition_table_["battery_stop_charging"] = battery_stop_charging_perms;
@@ -144,6 +167,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> battery_full_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, true}};
     transition_table_["battery_full"] = battery_full_perms;
@@ -152,6 +176,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> error_code_perms = {
         {WheelchairState::READY, true},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, true}};
     transition_table_["error_code_handling"] = error_code_perms;
@@ -160,6 +185,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> base_station_lost_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["base_station_lost"] = base_station_lost_perms;
@@ -168,6 +194,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> search_timeout_perms = {
         {WheelchairState::READY, false},
         {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, false}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, false}};
     transition_table_["search_30s_timeout"] = search_timeout_perms;
@@ -333,7 +360,10 @@ void WorkflowManager::processStateTransition(const std::string &event)
                 transitionToCharging();
             }
             break;
-
+        case WheelchairState::ROTATING:
+            // 旋转中状态下，所有事件默认不处理，可以添加特定逻辑
+            RCLCPP_DEBUG(node_->get_logger(), "旋转中状态下忽略事件: %s", event.c_str());
+            break;
         case WheelchairState::SEARCHING:
             if (event == "base_station_lost")
             {
@@ -376,15 +406,132 @@ void WorkflowManager::processStateTransition(const std::string &event)
     }
 }
 
+// ==================== 旋转管理接口 ====================
+
+void WorkflowManager::startRotationTimer()
+{
+    try
+    {
+        // 创建10秒后检查旋转的定时器
+        rotation_check_timer_ = node_->create_wall_timer(
+            std::chrono::seconds(10),
+            [this]()
+            {
+                this->rotationCheckCallback();
+            });
+
+        RCLCPP_INFO(node_->get_logger(), "旋转检查定时器已启动 (10秒后检查)");
+    }
+    catch (const std::exception &e)
+    {
+        handleException("启动旋转检查定时器", e);
+    }
+}
+
+void WorkflowManager::checkRotationTransition()
+{
+    try
+    {
+        if (current_state_ == WheelchairState::SEARCHING)
+        {
+            double search_duration = (node_->now() - search_start_time_).seconds();
+
+            // 如果搜索时间超过10秒，进入旋转状态
+            if (search_duration >= 10.0)
+            {
+                RCLCPP_INFO(node_->get_logger(),
+                            "搜索已持续 %.1f 秒，进入旋转状态", search_duration);
+                transitionToRotating();
+            }
+            else
+            {
+                RCLCPP_DEBUG(node_->get_logger(),
+                             "搜索中，已持续 %.1f 秒，还需 %.1f 秒进入旋转",
+                             search_duration, 10.0 - search_duration);
+            }
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("检查旋转转移", e);
+    }
+}
+
+void WorkflowManager::stopRotationTimer()
+{
+    try
+    {
+        if (rotation_check_timer_)
+        {
+            rotation_check_timer_->cancel();
+            rotation_check_timer_.reset();
+            RCLCPP_INFO(node_->get_logger(), "旋转检查定时器已停止");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("停止旋转检查定时器", e);
+    }
+}
+
+void WorkflowManager::transitionToRotating()
+{
+    try
+    {
+        if (current_state_ == WheelchairState::SEARCHING)
+        {
+            setState(WheelchairState::ROTATING);
+
+            // 启动旋转
+            if (rotation_manager_)
+            {
+                rotation_manager_->startRotation();
+                RCLCPP_INFO(node_->get_logger(), "轮椅开始旋转搜索");
+                publishState("WHEELCHAIR_ROTATING", "旋转搜索充电站");
+
+                // 发布回充状态
+                publishRechargeStatus("ROTATING_SEARCH",
+                                      "搜索10秒未找到基站，开始旋转搜索");
+            }
+            else
+            {
+                RCLCPP_ERROR(node_->get_logger(), "旋转管理器未初始化");
+            }
+
+            // 停止旋转检查定时器
+            stopRotationTimer();
+        }
+        else
+        {
+            RCLCPP_WARN(node_->get_logger(),
+                        "当前状态 %s 无法转换为旋转状态",
+                        getCurrentState().c_str());
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("转换到旋转状态", e);
+    }
+}
+
 void WorkflowManager::transitionToWalking()
 {
     try
     {
+        // 如果正在旋转，先停止旋转
+        if (rotation_manager_ && rotation_manager_->isRotating())
+        {
+            rotation_manager_->stopRotation();
+        }
         if (current_state_ == WheelchairState::READY ||
             current_state_ == WheelchairState::SEARCHING ||
-            current_state_ == WheelchairState::CHARGING)
+            current_state_ == WheelchairState::CHARGING ||
+            current_state_ == WheelchairState::ROTATING)
         {
             setState(WheelchairState::WALKING);
+            // 停止所有定时器
+            stopSearchTimeoutTimer();
+            stopRotationTimer();
             RCLCPP_INFO(node_->get_logger(), "轮椅开始行走");
             publishState("WHEELCHAIR_WALKING");
         }
@@ -403,10 +550,11 @@ void WorkflowManager::transitionToSearching()
             current_state_ == WheelchairState::WALKING)
         {
             setState(WheelchairState::SEARCHING);
+            search_start_time_ = node_->now();
             RCLCPP_INFO(node_->get_logger(), "开始搜索充电站");
             publishState("SEARCH_CHARGING_STATION_START");
             startSearchTimeoutTimer();
-
+            startRotationTimer();
             // 发布回充开始通知
             publishRechargeStatus("RECHARGE_STARTED", "回充功能已启动，正在搜索充电站");
         }
@@ -446,6 +594,12 @@ void WorkflowManager::transitionToReady()
         std::string previous_state = getCurrentState();
         setState(WheelchairState::READY);
         stopSearchTimeoutTimer();
+        stopRotationTimer();
+        // 如果正在旋转，停止旋转
+        if (rotation_manager_ && rotation_manager_->isRotating())
+        {
+            rotation_manager_->stopRotation();
+        }
         RCLCPP_INFO(node_->get_logger(), "返回到就绪状态 (前状态: %s)",
                     previous_state.c_str());
         publishState("WHEELCHAIR_READY");
@@ -975,6 +1129,16 @@ void WorkflowManager::emergencyTransitionToReady(const std::string &reason)
             search_timeout_timer_->cancel();
             search_timeout_timer_.reset();
         }
+        if (rotation_check_timer_)
+        {
+            rotation_check_timer_->cancel();
+            rotation_check_timer_.reset();
+        }
+        // 停止旋转
+        if (rotation_manager_ && rotation_manager_->isRotating())
+        {
+            rotation_manager_->stopRotation();
+        }
 
         // 强制设置状态
         current_state_ = WheelchairState::READY;
@@ -985,7 +1149,7 @@ void WorkflowManager::emergencyTransitionToReady(const std::string &reason)
         state_publisher_->publish(msg);
 
         // 如果之前是回充相关状态，发布回充失败
-        if (previous_state == "搜索中" || previous_state == "充电中")
+        if (previous_state == "搜索中" || previous_state == "充电中" || previous_state == "旋转中")
         {
             publishRechargeStatus("RECHARGE_EMERGENCY_FAILURE",
                                   "系统异常导致回充紧急终止: " + reason);
@@ -1075,13 +1239,53 @@ void WorkflowManager::stopSearchTimeoutTimer()
     }
 }
 
-void WorkflowManager::searchTimeoutCallback()
+// ==================== 定时器回调 ====================
+
+void WorkflowManager::rotationCheckCallback()
 {
     try
     {
         if (current_state_ == WheelchairState::SEARCHING)
         {
-            RCLCPP_WARN(node_->get_logger(), "搜索充电站超时30秒，回充失败");
+            checkRotationTransition();
+        }
+        else
+        {
+            RCLCPP_DEBUG(node_->get_logger(),
+                         "旋转检查回调触发，但当前不在搜索状态");
+            stopRotationTimer();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("旋转检查定时器回调", e);
+    }
+}
+
+void WorkflowManager::searchTimeoutCallback()
+{
+    try
+    {
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::ROTATING)
+        {
+            double total_search_time = 0.0;
+
+            if (current_state_ == WheelchairState::SEARCHING)
+            {
+                total_search_time = (node_->now() - search_start_time_).seconds();
+            }
+            else if (current_state_ == WheelchairState::ROTATING && rotation_manager_)
+            {
+                // 计算总搜索时间：搜索时间 + 旋转时间
+                double search_duration = 10.0; // 搜索了10秒后才开始旋转
+                double rotation_duration = rotation_manager_->getRotationDuration();
+                total_search_time = search_duration + rotation_duration;
+            }
+
+            RCLCPP_WARN(node_->get_logger(),
+                        "总搜索时间 %.1f 秒超时（包括旋转），回充失败",
+                        total_search_time);
             handleSearchTimeoutEvent();
         }
         else
@@ -1140,6 +1344,8 @@ std::string WorkflowManager::stateToString(WheelchairState state) const
         return "就绪中";
     case WheelchairState::WALKING:
         return "行走中";
+    case WheelchairState::ROTATING: // 新增
+        return "旋转中";
     case WheelchairState::SEARCHING:
         return "搜索中";
     case WheelchairState::CHARGING: