wheelchair_state_machine

src/wheelchair_state_machine/include/wheelchair_state_machine/battery_manager.hpp

@@ -12,35 +12,35 @@ class BatteryManager
 public:
     using BatteryStateCallback = std::function<void(BatteryState, float)>;
     using LowBatteryCallback = std::function<void(BatteryState, float)>;
-    
-    BatteryManager(rclcpp::Node* node, 
-                   float low_threshold = 20.0f, 
+
+    BatteryManager(rclcpp::Node *node,
+                   float low_threshold = 20.0f,
                    float critical_threshold = 10.0f);
-    
+
     void initialize();
     void updateFromMessage(const sensor_msgs::msg::BatteryState::SharedPtr msg);
     void setBatteryStateCallback(BatteryStateCallback callback);
     void setLowBatteryCallback(LowBatteryCallback callback);
-    
+
     BatteryState getBatteryState() const { return battery_state_; }
     float getBatteryPercentage() const { return battery_percentage_; }
     float getLowThreshold() const { return low_battery_threshold_; }
     float getCriticalThreshold() const { return critical_battery_threshold_; }
-    
+
     std::string stateToString() const;
-    
+
 private:
-    rclcpp::Node* node_;
+    rclcpp::Node *node_;
     rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
-    
+
     BatteryState battery_state_;
     float battery_percentage_;
     float low_battery_threshold_;
     float critical_battery_threshold_;
-    
+
     BatteryStateCallback battery_state_callback_;
     LowBatteryCallback low_battery_callback_;
-    
+
     void batteryStateCallback(const sensor_msgs::msg::BatteryState::SharedPtr msg);
     void updateBatteryState(float percentage);
     void checkBatteryThresholds();

src/wheelchair_state_machine/include/wheelchair_state_machine/error_code.hpp

@@ -6,107 +6,119 @@
 #include <cstdint>
 
 // 错误码模块定义（与文档对应）
-enum class ErrorModule : uint8_t {
-    SYSTEM      = 0x01,    // 系统
-    IMU         = 0x10,    // IMU
-    GPS         = 0x11,    // GPS
-    LIDAR1      = 0x12,    // 左雷达
-    LIDAR2      = 0x13,    // 右雷达
-    LIDAR3      = 0x14,    // 左斜雷达
-    LIDAR4      = 0x15,    // 右斜雷达
-    LIDAR5      = 0x16,    // 后雷达
-    CAN0        = 0x02,    // CAN0
-    CAN1        = 0x03,    // CAN1
-    CAN2        = 0x04,    // CAN2
-    DRIVER      = 0x20,    // 电机驱动器
-    TOP         = 0x21,    // TOP驱动器
-    OBSTACLE    = 0x50,    // 避障功能
-    RECHARGE    = 0x51     // 回充功能
+enum class ErrorModule : uint8_t
+{
+    SYSTEM = 0x01,   // 系统
+    IMU = 0x10,      // IMU
+    GPS = 0x11,      // GPS
+    LIDAR1 = 0x12,   // 左雷达
+    LIDAR2 = 0x13,   // 右雷达
+    LIDAR3 = 0x14,   // 左斜雷达
+    LIDAR4 = 0x15,   // 右斜雷达
+    LIDAR5 = 0x16,   // 后雷达
+    CAN0 = 0x02,     // CAN0
+    CAN1 = 0x03,     // CAN1
+    CAN2 = 0x04,     // CAN2
+    DRIVER = 0x20,   // 电机驱动器
+    TOP = 0x21,      // TOP驱动器
+    OBSTACLE = 0x50, // 避障功能
+    RECHARGE = 0x51  // 回充功能
 };
 
 // 错误码子模块定义
-enum class ErrorSubModule : uint8_t {
-    HARDWARE    = 0x01,    // 硬件
-    DATA        = 0x02,    // 数据
-    TIMING      = 0x03,    // 时间同步
-    CONFIG      = 0x04,    // 配置
-    COMMUNICATION = 0x05,  // 通信
-    CALIBRATION = 0x06,    // 标定
-    PERFORMANCE = 0x07,    // 性能
-    POWER       = 0x08,    // 电源
-    TEMPERATURE = 0x09,    // 温度
-    OTHER       = 0x0A     // 其他
+enum class ErrorSubModule : uint8_t
+{
+    HARDWARE = 0x01,      // 硬件
+    DATA = 0x02,          // 数据
+    TIMING = 0x03,        // 时间同步
+    CONFIG = 0x04,        // 配置
+    COMMUNICATION = 0x05, // 通信
+    CALIBRATION = 0x06,   // 标定
+    PERFORMANCE = 0x07,   // 性能
+    POWER = 0x08,         // 电源
+    TEMPERATURE = 0x09,   // 温度
+    OTHER = 0x0A          // 其他
 };
 
 // 通用错误编号
-enum class ErrorCode : uint16_t {
-    NO_ERROR            = 0x0000,
-    DEVICE_NOT_FOUND    = 0x0001,
-    DATA_INVALID        = 0x0002,
-    TIMEOUT             = 0x0003,
-    CONFIG_INVALID      = 0x0004,
-    COMMUNICATION_FAIL  = 0x0005,
-    CALIBRATION_FAIL    = 0x0006,
-    PERFORMANCE_LOW     = 0x0007,
-    POWER_ABNORMAL      = 0x0008,
-    TEMPERATURE_HIGH    = 0x0009,
-    UNKNOWN_ERROR       = 0x000A
+enum class ErrorCode : uint16_t
+{
+    NO_ERROR = 0x0000,
+    DEVICE_NOT_FOUND = 0x0001,
+    DATA_INVALID = 0x0002,
+    TIMEOUT = 0x0003,
+    CONFIG_INVALID = 0x0004,
+    COMMUNICATION_FAIL = 0x0005,
+    CALIBRATION_FAIL = 0x0006,
+    PERFORMANCE_LOW = 0x0007,
+    POWER_ABNORMAL = 0x0008,
+    TEMPERATURE_HIGH = 0x0009,
+    UNKNOWN_ERROR = 0x000A
 };
 
 // 错误码数据结构
-struct ErrorInfo {
-    uint32_t error_code;           // 完整错误码 0xMMSSEEEE
-    std::string module_name;       // 模块名称
-    std::string sub_module_name;   // 子模块名称
-    std::string description;       // 错误描述
-    uint64_t timestamp;           // 时间戳（毫秒）
-    bool is_recovered;            // 是否已恢复
-    
+struct ErrorInfo
+{
+    uint32_t error_code;         // 完整错误码 0xMMSSEEEE
+    std::string module_name;     // 模块名称
+    std::string sub_module_name; // 子模块名称
+    std::string description;     // 错误描述
+    uint64_t timestamp;          // 时间戳（毫秒）
+    bool is_recovered;           // 是否已恢复
+
     // 解析错误码
-    ErrorModule getModule() const {
+    ErrorModule getModule() const
+    {
         return static_cast<ErrorModule>((error_code >> 24) & 0xFF);
     }
-    
-    ErrorSubModule getSubModule() const {
+
+    ErrorSubModule getSubModule() const
+    {
         return static_cast<ErrorSubModule>((error_code >> 16) & 0xFF);
     }
-    
-    uint16_t getErrorNumber() const {
+
+    uint16_t getErrorNumber() const
+    {
         return error_code & 0xFFFF;
     }
-    
+
     // 判断是否为严重错误
-    bool isCritical() const {
+    bool isCritical() const
+    {
         // 根据文档，硬件错误和部分功能错误为严重错误
         auto sub = getSubModule();
         auto module = getModule();
-        
+
         // 硬件错误总是严重
-        if (sub == ErrorSubModule::HARDWARE) return true;
-        
+        if (sub == ErrorSubModule::HARDWARE)
+            return true;
+
         // 关键模块的通信错误
-        if (sub == ErrorSubModule::COMMUNICATION) {
-            return (module == ErrorModule::IMU || 
+        if (sub == ErrorSubModule::COMMUNICATION)
+        {
+            return (module == ErrorModule::IMU ||
                     module == ErrorModule::LIDAR1 ||
                     module == ErrorModule::DRIVER);
         }
-        
+
         // 回充功能相关的错误
-        if (module == ErrorModule::RECHARGE) {
+        if (module == ErrorModule::RECHARGE)
+        {
             uint16_t error_num = getErrorNumber();
             // 根据文档，回充功能错误码 0x0001-0x0003 为严重错误
             return (error_num >= 0x0001 && error_num <= 0x0003);
         }
-        
+
         return false;
     }
 };
 
 // 错误码消息类型（用于ROS topic）
-struct ErrorCodeMsg {
+struct ErrorCodeMsg
+{
     uint32_t error_code;
     uint64_t timestamp;
-    uint8_t error_level;  // 0: INFO, 1: WARNING, 2: ERROR, 3: CRITICAL
+    uint8_t error_level; // 0: INFO, 1: WARNING, 2: ERROR, 3: CRITICAL
     std::string description;
 };
 

src/wheelchair_state_machine/include/wheelchair_state_machine/ipad_manager.hpp

@@ -12,34 +12,34 @@ public:
     // 回调函数类型
     using RechargeStartCallback = std::function<void()>;
     using RechargeCancelCallback = std::function<void()>;
-    
-    IpadManager(rclcpp::Node* node);
-    
+
+    IpadManager(rclcpp::Node *node);
+
     // 核心接口 - 只处理开始和结束
     void onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg);
-    
+
     // 回调设置
     void setRechargeStartCallback(RechargeStartCallback callback);
     void setRechargeCancelCallback(RechargeCancelCallback callback);
-    
+
     // 状态查询（可选）
     bool isRechargeActive() const { return recharge_active_; }
-    
+
 private:
-    rclcpp::Node* node_;
-    
+    rclcpp::Node *node_;
+
     // 回调函数
     RechargeStartCallback recharge_start_callback_;
     RechargeCancelCallback recharge_cancel_callback_;
-    
+
     // 状态
     bool recharge_active_;
     bool cmd_enable_;
-    
+
     // 内部方法
     void startRecharge();
     void cancelRecharge();
-    void logInfo(const std::string& message);
+    void logInfo(const std::string &message);
 };
 
 #endif // IPAD_MANAGER_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/recharge_tool.hpp

@@ -12,16 +12,16 @@ public:
     static double angleBetween(const IndexPoint &p1, const IndexPoint &p2);
     static double angleDiff(double a1, double a2);
     static double averageAngle(const std::vector<double> &angles);
-    
+
     static std::vector<std::vector<IndexPoint>> splitPointsToSegments(
         const std::vector<IndexPoint> &points,
         double angle_threshold_deg = 15.0,
         int history_window_size = 3);
-    
+
     static std::pair<double, double> calculateAngleBisector(double k1, double b1, double k2, double b2);
     static std::pair<double, double> fitLineLeastSquares(const Eigen::MatrixXd &points);
     static std::pair<double, double> computeDockingVelocity(double dx, double dy, double yaw_error);
-    
+
     // 直线拟合（简化版，用于基础功能）
     static std::pair<double, double> simpleFitLine(const std::vector<IndexPoint> &points);
 };

src/wheelchair_state_machine/include/wheelchair_state_machine/rotation_manager.hpp

@@ -29,7 +29,6 @@ public:
 
 private:
     rclcpp::Node *node_;
-
     // ROS发布者
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
 

src/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -75,6 +75,9 @@ public:
     // 内部事件触发
     void triggerEvent(const std::string &event_name);
 
+    void startRotationTimeoutTimer();
+    void stopRotationTimeoutTimer();
+
 private:
     rclcpp::Node *node_;
 

src/wheelchair_state_machine/src/battery_manager.cpp

@@ -5,14 +5,10 @@
 using namespace std::chrono_literals;
 using std::placeholders::_1;
 
-BatteryManager::BatteryManager(rclcpp::Node* node, 
-                               float low_threshold, 
+BatteryManager::BatteryManager(rclcpp::Node *node,
+                               float low_threshold,
                                float critical_threshold)
-    : node_(node)
-    , battery_state_(BatteryState::NORMAL)
-    , battery_percentage_(100.0f)
-    , low_battery_threshold_(low_threshold)
-    , critical_battery_threshold_(critical_threshold)
+    : node_(node), battery_state_(BatteryState::NORMAL), battery_percentage_(100.0f), low_battery_threshold_(low_threshold), critical_battery_threshold_(critical_threshold)
 {
 }
 
@@ -21,16 +17,16 @@ void BatteryManager::initialize()
     // 声明电池阈值参数
     node_->declare_parameter<float>("low_battery_threshold", low_battery_threshold_);
     node_->declare_parameter<float>("critical_battery_threshold", critical_battery_threshold_);
-    
+
     // 获取电池阈值参数
     low_battery_threshold_ = node_->get_parameter("low_battery_threshold").as_double();
     critical_battery_threshold_ = node_->get_parameter("critical_battery_threshold").as_double();
-    
+
     // 创建电池状态订阅者
     battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
         "battery_state", 10,
         std::bind(&BatteryManager::batteryStateCallback, this, _1));
-    
+
     RCLCPP_INFO(node_->get_logger(), "电池管理器已初始化");
     RCLCPP_INFO(node_->get_logger(), "电池阈值 - 低电量: %.1f%%, 严重低电量: %.1f%%",
                 low_battery_threshold_, critical_battery_threshold_);
@@ -119,7 +115,7 @@ void BatteryManager::updateBatteryState(float percentage)
                     batteryStateToString(old_state).c_str(),
                     batteryStateToString(battery_state_).c_str(),
                     percentage);
-        
+
         if (battery_state_callback_)
             battery_state_callback_(battery_state_, battery_percentage_);
     }

src/wheelchair_state_machine/src/ipad_manager.cpp

@@ -14,15 +14,18 @@ void IpadManager::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
 {
     bool new_enable = msg->data;
     logInfo("收到命令使能: " + std::to_string(new_enable));
-
+    if (new_enable == false)
+    {
+        return;
+    }
     // 状态变化检查
-    if (!cmd_enable_ && !new_enable)
+    if (cmd_enable_ == false && new_enable == true)
     {
         // 命令使能从false变为true - 启动回充
         cmd_enable_ = true;
         startRecharge();
     }
-    else if (cmd_enable_ && new_enable)
+    else if (cmd_enable_ == true && new_enable == true)
     {
         // 命令使能从true变为false - 取消回充
         cmd_enable_ = false;

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

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

src/wheelchair_state_machine/src/main.cpp

@@ -11,13 +11,13 @@ public:
     {
         event_handler_ = std::make_unique<EventInputHandler>(this);
         event_handler_->initializeModules();
-        
+
         // 显示系统状态
         event_handler_->displaySystemStatus();
-        
+
         RCLCPP_INFO(this->get_logger(), "智能轮椅状态机已启动");
     }
-    
+
 private:
     std::unique_ptr<EventInputHandler> event_handler_;
 };
@@ -25,9 +25,9 @@ private:
 int main(int argc, char **argv)
 {
     rclcpp::init(argc, argv);
-    
+
     auto node = std::make_shared<WheelchairStateMachineNode>();
-    
+
     rclcpp::spin(node);
     rclcpp::shutdown();
     return 0;

src/wheelchair_state_machine/src/recharge_tool.cpp

@@ -3,7 +3,7 @@
 #include <cmath>
 #include <algorithm>
 #include <numeric>
-#include<deque>
+#include <deque>
 
 double RechargeTool::angleBetween(const IndexPoint &p1, const IndexPoint &p2)
 {

src/wheelchair_state_machine/src/rotation_manager.cpp

@@ -7,7 +7,7 @@ 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
+      angular_velocity_(0.15) // 角速度0.3 rad/s
       ,
       linear_velocity_(0.0) // 线速度0 m/s
 {
@@ -83,7 +83,6 @@ void RotationManager::stopRotation()
     double duration = getRotationDuration();
     logInfo("停止旋转 - 持续时间: " + std::to_string(duration) + "秒");
 }
-
 void RotationManager::rotationTimerCallback()
 {
     if (!is_rotating_)

src/wheelchair_state_machine/src/workflow.cpp

@@ -500,6 +500,11 @@ void WorkflowManager::transitionToRotating()
 
             // 停止旋转检查定时器
             stopRotationTimer();
+
+            // 关键修复：进入旋转状态时，停止搜索超时定时器
+            stopSearchTimeoutTimer();
+
+            RCLCPP_INFO(node_->get_logger(), "已停止搜索超时定时器，进入旋转搜索模式");
         }
         else
         {
@@ -570,11 +575,23 @@ void WorkflowManager::transitionToCharging()
     try
     {
         if (current_state_ == WheelchairState::SEARCHING ||
-            current_state_ == WheelchairState::WALKING)
+            current_state_ == WheelchairState::WALKING ||
+            current_state_ == WheelchairState::ROTATING) // 增加旋转状态
         {
             setState(WheelchairState::CHARGING);
             RCLCPP_INFO(node_->get_logger(), "开始充电");
+
+            // 停止所有定时器
             stopSearchTimeoutTimer();
+            stopRotationTimer();
+
+            // 如果正在旋转，停止旋转
+            if (rotation_manager_ && rotation_manager_->isRotating())
+            {
+                rotation_manager_->stopRotation();
+                RCLCPP_INFO(node_->get_logger(), "停止旋转，开始充电");
+            }
+
             publishState("CHARGING_START");
 
             // 发布回充成功通知
@@ -593,13 +610,18 @@ 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(), "停止旋转，返回到就绪状态");
         }
+
         RCLCPP_INFO(node_->get_logger(), "返回到就绪状态 (前状态: %s)",
                     previous_state.c_str());
         publishState("WHEELCHAIR_READY");
@@ -1032,6 +1054,10 @@ void WorkflowManager::handleSearchTimeoutEvent()
     {
         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秒超时，回充失败");
 
@@ -1134,6 +1160,7 @@ void WorkflowManager::emergencyTransitionToReady(const std::string &reason)
             rotation_check_timer_->cancel();
             rotation_check_timer_.reset();
         }
+
         // 停止旋转
         if (rotation_manager_ && rotation_manager_->isRotating())
         {
@@ -1266,31 +1293,32 @@ void WorkflowManager::searchTimeoutCallback()
 {
     try
     {
-        if (current_state_ == WheelchairState::SEARCHING ||
-            current_state_ == WheelchairState::ROTATING)
+        // 只在搜索状态下处理搜索超时，旋转状态由RotationManager处理
+        if (current_state_ == WheelchairState::SEARCHING)
         {
-            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;
-            }
+            double total_search_time = (node_->now() - search_start_time_).seconds();
 
             RCLCPP_WARN(node_->get_logger(),
-                        "总搜索时间 %.1f 秒超时（包括旋转），回充失败",
+                        "搜索 %.1f 秒超时，回充失败",
                         total_search_time);
             handleSearchTimeoutEvent();
         }
+        else if (current_state_ == WheelchairState::ROTATING)
+        {
+            // 旋转状态不应该触发搜索超时，这应该是个bug
+            RCLCPP_ERROR(node_->get_logger(),
+                         "错误：旋转状态下触发了搜索超时回调！当前状态: %s",
+                         getCurrentState().c_str());
+
+            // 停止搜索超时定时器
+            stopSearchTimeoutTimer();
+            RCLCPP_INFO(node_->get_logger(), "已纠正：停止旋转状态下的搜索超时定时器");
+        }
         else
         {
-            RCLCPP_DEBUG(node_->get_logger(), "搜索超时定时器触发，但当前不在搜索状态");
+            RCLCPP_DEBUG(node_->get_logger(),
+                         "搜索超时定时器触发，但当前不在搜索状态: %s",
+                         getCurrentState().c_str());
             stopSearchTimeoutTimer();
         }
     }