wheelchair_state_machine

src3/wheelchair_state_machine/CMakeLists.txt

@@ -44,11 +44,6 @@ ament_target_dependencies(wheelchair_state_machine_node
   tf2_geometry_msgs
 )
 
-# 2. 测试错误发布器
-add_executable(test_error_publisher
-  src/error_code_publisher.cpp
-)
-
 ament_target_dependencies(test_error_publisher
   rclcpp
   std_msgs

src3/wheelchair_state_machine/include/wheelchair_state_machine/event_input.hpp

@@ -33,7 +33,7 @@ public:
     void triggerEvent(const std::string &event_name);
     void triggerEvent(const std::string &event_name, const std::string &data);
 
-    // 特定事件触发
+    // 特定事件触发（仅外部输入事件）
     void triggerIpadStart();
     void triggerIpadCancel();
     void triggerBluetoothDisconnected();
@@ -49,7 +49,6 @@ public:
     void triggerBatteryStartCharging();
     void triggerBatteryStopCharging();
     void triggerBatteryFull();
-    void triggerErrorCode(int error_code);
     void triggerBaseStationLost();
     void triggerSearchTimeout();
 
@@ -72,7 +71,6 @@ private:
 
     // ROS订阅者
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr cmd_enable_sub_;
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr recharge_status_sub_;
 
     // 事件回调映射
     std::map<std::string, EventCallback> event_callbacks_;
@@ -86,10 +84,6 @@ private:
     void startNavigationControl();
     void stopNavigationControl();
     void logEvent(const std::string &event_name, const std::string &details = "");
-
-    // 错误处理相关
-    void handleCriticalError(const ErrorInfo &error_info);
-    void handleErrorRecovery(const ErrorInfo &error_info);
 };
 
 #endif // EVENT_INPUT_HANDLER_HPP

src3/wheelchair_state_machine/include/wheelchair_state_machine/report.hpp

@@ -22,9 +22,6 @@ public:
 
     ReportManager(rclcpp::Node *node);
 
-    // 错误管理初始化
-    void initializeErrorManager();
-
     // 报告生成接口
     std::string generateSystemStatusReport(
         WheelchairState wheelchair_state,

src3/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -6,11 +6,13 @@
 #include <string>
 #include <map>
 #include <memory>
+#include <functional>
 #include "wheelchair_types.hpp"
+#include "error_code.hpp"
 #include <std_msgs/msg/string.hpp>
 #include <std_msgs/msg/u_int32.hpp>
 #include <rclcpp/timer.hpp>
-#include "error_code.hpp"
+
 class WorkflowManager
 {
 public:
@@ -18,27 +20,43 @@ public:
 
     WorkflowManager(rclcpp::Node *node);
 
-    // 状态机核心接口
+    // ==================== 状态机核心接口 ====================
+
+    // 事件处理接口
     bool handleEvent(const std::string &event);
+
+    // 状态查询接口
     std::string getCurrentState() const;
+    WheelchairState getCurrentWheelchairState() const { return current_state_; }
+
+    // 状态设置接口
     void setState(WheelchairState new_state);
 
-    // 状态转移控制
+    // ==================== 状态转移控制 ====================
+
+    // 标准状态转移
     void transitionToWalking();
     void transitionToSearching();
     void transitionToCharging();
     void transitionToReady();
 
-    // 回调设置
+    // 紧急状态转移
+    void emergencyTransitionToReady(const std::string &reason = "未知原因");
+
+    // ==================== 回调设置接口 ====================
+
     void setStateUpdateCallback(StateUpdateCallback callback);
     void setRechargeStatusCallback(StateUpdateCallback callback);
     void setRechargeErrorCallback(std::function<void(const ErrorInfo &)> callback);
 
-    // 定时器管理
+    // ==================== 定时器管理 ====================
+
     void startSearchTimeoutTimer();
     void stopSearchTimeoutTimer();
 
-    // 事件处理接口
+    // ==================== 事件处理接口 ====================
+
+    // 外部事件处理
     void handleLowBatteryEvent(BatteryState state, float percentage);
     void handleChargingStartEvent();
     void handleChargingStopEvent();
@@ -47,8 +65,8 @@ public:
     void handleSearchTimeoutEvent();
     void handleRechargeFailure(const std::string &reason);
 
-    // 查询接口
-    WheelchairState getCurrentWheelchairState() const { return current_state_; }
+    // 内部事件触发
+    void triggerEvent(const std::string &event_name);
 
 private:
     rclcpp::Node *node_;
@@ -60,6 +78,8 @@ private:
     // 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_;
 
     // 回调函数
@@ -67,22 +87,29 @@ private:
     StateUpdateCallback recharge_status_callback_;
     std::function<void(const ErrorInfo &)> recharge_error_callback_;
 
-    // 私有方法
+    // ==================== 私有方法 ====================
+
+    // 初始化方法
     void initializeTransitionTable();
+
+    // 事件权限检查
     bool checkEventPermission(const std::string &event);
+
+    // 事件执行
     void executeEvent(const std::string &event);
     void processStateTransition(const std::string &event);
-    void triggerEvent(const std::string &event);
+
+    // 状态发布
     void publishState(const std::string &state_str, const std::string &message = "");
+    std::string stateToString(WheelchairState state) const;
 
     // 回充状态管理
     void publishRechargeStatus(const std::string &status, const std::string &message);
     void publishRechargeErrorCode(uint32_t error_code);
 
-    // 状态转换函数
-    std::string stateToString(WheelchairState state) const;
+    // ==================== 具体事件处理函数 ====================
 
-    // 具体事件处理函数
+    // 外部事件处理
     void processIpadStartEvent();
     void processIpadCancelEvent();
     void processBluetoothDisconnected();
@@ -94,10 +121,17 @@ private:
     void processJoystickStop();
     void processPushStart();
     void processPushStop();
+
+    // 错误码处理（保留接口）
     void processErrorCode();
 
-    // 定时器回调
+    // ==================== 异常处理函数 ====================
+
+    void handleException(const std::string &context, const std::exception &e);
+
+    // ==================== 定时器回调 ====================
+
     void searchTimeoutCallback();
 };
 
-#endif
+#endif // WORKFLOW_HPP

src3/wheelchair_state_machine/src/error_code_publisher.cpp

@@ -1,68 +0,0 @@
-// error_code_publisher.cpp
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/u_int32.hpp>
-#include <chrono>
-#include <random>
-
-using namespace std::chrono_literals;
-
-class ErrorCodePublisher : public rclcpp::Node
-{
-public:
-    ErrorCodePublisher() : Node("error_code_publisher")
-    {
-        publisher_ = this->create_publisher<std_msgs::msg::UInt32>(
-            "/wheelchair/error_code", 10);
-
-        timer_ = this->create_wall_timer(
-            5000ms, // 每5秒发布一个错误码
-            std::bind(&ErrorCodePublisher::publishErrorCode, this));
-
-        // 模拟错误码列表（根据文档）
-        error_codes_ = {
-            0x10020102, // IMU数据无效
-            0x16010101, // 后雷达硬件故障
-            0x51010101, // 回充检测失败
-            0x20010109, // 电机驱动器温度过高
-            0x50010101, // 避障功能硬件故障
-        };
-
-        RCLCPP_INFO(this->get_logger(), "错误码发布器已启动");
-    }
-
-private:
-    void publishErrorCode()
-    {
-        static size_t index = 0;
-
-        if (index < error_codes_.size())
-        {
-            auto msg = std_msgs::msg::UInt32();
-            msg.data = error_codes_[index];
-
-            publisher_->publish(msg);
-
-            RCLCPP_INFO(this->get_logger(),
-                        "发布错误码: 0x%08X", msg.data);
-
-            index++;
-        }
-        else
-        {
-            // 循环发布
-            index = 0;
-        }
-    }
-
-    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr publisher_;
-    rclcpp::TimerBase::SharedPtr timer_;
-    std::vector<uint32_t> error_codes_;
-};
-
-int main(int argc, char **argv)
-{
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<ErrorCodePublisher>());
-    rclcpp::shutdown();
-    return 0;
-}

src3/wheelchair_state_machine/src/event_input.cpp

@@ -1,10 +1,12 @@
+// event_input.cpp
 #include "wheelchair_state_machine/event_input.hpp"
 #include <rclcpp/logging.hpp>
 #include <std_msgs/msg/bool.hpp>
 using namespace std::chrono_literals;
 
 EventInputHandler::EventInputHandler(rclcpp::Node *node)
-    : node_(node), battery_manager_(nullptr), lidar_controller_(nullptr), ipad_manager_(nullptr), workflow_manager_(nullptr), report_manager_(nullptr)
+    : node_(node), battery_manager_(nullptr), lidar_controller_(nullptr),
+      ipad_manager_(nullptr), workflow_manager_(nullptr), report_manager_(nullptr)
 {
     RCLCPP_INFO(node_->get_logger(), "事件输入处理器已初始化");
 }
@@ -25,9 +27,8 @@ void EventInputHandler::initializeModules()
     // 初始化工作流管理器
     workflow_manager_ = std::make_unique<WorkflowManager>(node_);
 
-    // 初始化报告管理器（包含错误管理）
+    // 初始化报告管理器
     report_manager_ = std::make_unique<ReportManager>(node_);
-    report_manager_->initializeErrorManager(); // 初始化错误管理
 
     setupModuleCallbacks();
     setupSubscriptions();
@@ -62,7 +63,7 @@ void EventInputHandler::setupModuleCallbacks()
             triggerLowBattery(state, percentage);
         });
 
-    // 2. 设置iPad管理器回调 - 只处理开始和结束
+    // 2. 设置iPad管理器回调
     ipad_manager_->setRechargeStartCallback(
         [this]()
         {
@@ -77,36 +78,7 @@ void EventInputHandler::setupModuleCallbacks()
             triggerEvent("ipad_phone_interface_cancel");
         });
 
-    // 3. 设置报告管理器错误回调
-    report_manager_->setErrorCallback(
-        [this](const ErrorInfo &error_info)
-        {
-            // 所有错误都通过报告管理器反馈
-            std::stringstream msg;
-            msg << "系统错误: " << error_info.module_name
-                << " - " << error_info.description;
-            report_manager_->reportWarning(msg.str());
-
-            // 如果错误与回充相关，触发工作流事件
-            if (error_info.getModule() == ErrorModule::RECHARGE)
-            {
-                triggerEvent("error_code_handling", error_info.description);
-            }
-        });
-
-    report_manager_->setCriticalErrorCallback(
-        [this](const ErrorInfo &error_info)
-        {
-            handleCriticalError(error_info);
-        });
-
-    report_manager_->setRecoveryCallback(
-        [this](const ErrorInfo &error_info)
-        {
-            handleErrorRecovery(error_info);
-        });
-
-    // 4. 设置工作流管理器回调
+    // 3. 设置工作流管理器状态更新回调
     workflow_manager_->setStateUpdateCallback(
         [this](const std::string &state_str, const std::string &message)
         {
@@ -130,68 +102,33 @@ void EventInputHandler::setupModuleCallbacks()
             }
         });
 
-    // 5. 设置工作流管理器回充状态回调
+    // 4. 设置工作流管理器回充状态回调
     workflow_manager_->setRechargeStatusCallback(
         [this](const std::string &status, const std::string &message)
         {
             // 处理回充状态更新
             RCLCPP_INFO(node_->get_logger(), "[回充状态] %s: %s", status.c_str(), message.c_str());
 
-            // 根据回充状态采取行动
+            // 所有回充状态都通过报告管理器反馈
             if (status == "RECHARGE_FAILED")
             {
-                // 回充失败，通过报告管理器反馈
-                std::string error_msg = "回充失败: " + message;
-                report_manager_->reportWarning(error_msg);
-
-                // 生成回充失败错误码
-                uint32_t error_code = 0x51010101; // 回充检测失败
-                report_manager_->processErrorCode(error_code);
+                report_manager_->reportWarning("回充失败: " + message);
             }
             else if (status == "RECHARGE_SUCCESS")
             {
-                // 回充成功，记录信息
                 report_manager_->reportInfo("回充成功: " + message);
             }
             else if (status == "CHARGING_INTERRUPTED")
             {
-                // 充电中断
                 report_manager_->reportWarning("充电中断: " + message);
             }
             else if (status == "CHARGING_COMPLETED")
             {
-                // 充电完成
                 report_manager_->reportInfo("充电完成: " + message);
             }
         });
 
-    // 6. 设置工作流管理器回充错误回调
-    workflow_manager_->setRechargeErrorCallback(
-        [this](const ErrorInfo &error_info)
-        {
-            // 处理回充相关错误
-            RCLCPP_WARN(node_->get_logger(), "回充错误: %s - %s",
-                        error_info.module_name.c_str(),
-                        error_info.description.c_str());
-
-            // 通过报告管理器处理错误
-            report_manager_->processErrorCode(error_info);
-
-            // 如果错误严重，触发错误处理事件
-            if (error_info.isCritical())
-            {
-                triggerEvent("error_code_handling", error_info.description);
-            }
-        });
-
-    // 7. 设置事件处理器回调 - 所有事件都转发给工作流管理器
-    registerEvent("error_code_handling",
-                  [this](const std::string &data)
-                  {
-                      RCLCPP_INFO(node_->get_logger(), "处理错误码事件: %s", data.c_str());
-                      workflow_manager_->handleEvent("error_code_handling");
-                  });
-
+    // 5. 设置事件处理器回调（仅外部输入事件）
     registerEvent("ipad_phone_interface_start",
                   [this](const std::string &data)
                   {
@@ -297,13 +234,6 @@ void EventInputHandler::setupModuleCallbacks()
                       workflow_manager_->handleEvent("push_stop");
                   });
 
-    registerEvent("error_code_handling",
-                  [this](const std::string &data)
-                  {
-                      RCLCPP_INFO(node_->get_logger(), "处理错误码事件");
-                      workflow_manager_->handleEvent("error_code_handling");
-                  });
-
     registerEvent("base_station_lost",
                   [this](const std::string &data)
                   {
@@ -331,21 +261,6 @@ void EventInputHandler::setupSubscriptions()
             this->ipad_manager_->onCmdEnable(msg);
         });
 
-    // 创建回充状态订阅者（用于监控回充超时）
-    recharge_status_sub_ = node_->create_subscription<std_msgs::msg::String>(
-        "wheelchair/recharge_timeout", 10,
-        [this](const std_msgs::msg::String::SharedPtr msg)
-        {
-            RCLCPP_INFO(node_->get_logger(), "收到回充状态: %s", msg->data.c_str());
-
-            // 检查是否是回充失败消息
-            if (msg->data.find("RECHARGE_FAILED") != std::string::npos)
-            {
-                // 回充失败，记录到报告管理器
-                report_manager_->reportWarning("回充失败通知: " + msg->data);
-            }
-        });
-
     RCLCPP_INFO(node_->get_logger(), "订阅器设置完成");
 }
 
@@ -526,86 +441,4 @@ void EventInputHandler::displaySystemStatus()
         lidar_controller_->getChargingVoltage(),
         isRechargeActive(),
         control_mode);
-}
-
-void EventInputHandler::handleCriticalError(const ErrorInfo &error_info)
-{
-    RCLCPP_ERROR(node_->get_logger(), "检测到严重错误: %s - %s",
-                 error_info.module_name.c_str(),
-                 error_info.description.c_str());
-
-    // 通过报告管理器反馈严重错误
-    std::stringstream report;
-    report << "严重错误警报!\n";
-    report << "模块: " << error_info.module_name << "\n";
-    report << "子模块: " << error_info.sub_module_name << "\n";
-    report << "错误码: 0x" << std::hex << std::setw(8) << std::setfill('0')
-           << error_info.error_code << "\n";
-    report << "描述: " << error_info.description << "\n";
-    report << "建议: 立即检查相关硬件或联系技术支持";
-
-    report_manager_->reportError(report.str());
-
-    // 根据错误类型采取行动
-    ErrorModule module = error_info.getModule();
-
-    if (module == ErrorModule::LIDAR5)
-    { // 后雷达故障
-        RCLCPP_WARN(node_->get_logger(), "后雷达故障，停止回充功能");
-        if (isRechargeActive())
-        {
-            stopNavigationControl();
-            triggerEvent("error_code_handling", "雷达故障导致回充停止");
-        }
-    }
-    else if (module == ErrorModule::DRIVER)
-    { // 电机驱动器故障
-        RCLCPP_WARN(node_->get_logger(), "电机驱动器故障，停止所有运动");
-        lidar_controller_->stopMotion();
-        triggerEvent("error_code_handling", "驱动器故障");
-    }
-    else if (module == ErrorModule::RECHARGE)
-    { // 回充功能故障
-        RCLCPP_WARN(node_->get_logger(), "回充功能故障: %s",
-                    error_info.description.c_str());
-
-        // 如果是回充超时错误
-        if (error_info.error_code == 0x51010101)
-        {
-            RCLCPP_WARN(node_->get_logger(), "回充超时30秒，通知用户");
-            // 可以通过其他方式通知用户界面
-        }
-    }
-}
-
-void EventInputHandler::handleErrorRecovery(const ErrorInfo &error_info)
-{
-    RCLCPP_INFO(node_->get_logger(), "错误已恢复: %s - %s",
-                error_info.module_name.c_str(),
-                error_info.description.c_str());
-
-    // 报告错误恢复
-    std::stringstream report;
-    report << "错误恢复通知\n";
-    report << "模块: " << error_info.module_name << "\n";
-    report << "错误: " << error_info.description << "\n";
-    report << "状态: 已恢复正常";
-
-    report_manager_->reportInfo(report.str());
-
-    // 如果恢复了关键模块，可以重新启用相关功能
-    if (error_info.getModule() == ErrorModule::LIDAR5)
-    {
-        RCLCPP_INFO(node_->get_logger(), "后雷达已恢复，可以重新启用回充功能");
-    }
-}
-
-// 修改错误码触发函数
-void EventInputHandler::triggerErrorCode(int error_code)
-{
-    std::string data = "错误码: 0x" + std::to_string(static_cast<uint32_t>(error_code));
-    triggerEvent("error_code_handling", data);
-
-    // 同时传递给报告管理器处理
-    report_manager_->processErrorCode(static_cast<uint32_t>(error_code));
 }

src3/wheelchair_state_machine/src/report.cpp

@@ -21,19 +21,6 @@ ReportManager::ReportManager(rclcpp::Node *node)
     RCLCPP_INFO(node_->get_logger(), "报告管理器已初始化");
 }
 
-void ReportManager::initializeErrorManager()
-{
-    // 订阅错误码topic
-    error_sub_ = node_->create_subscription<std_msgs::msg::UInt32>(
-        "/wheelchair/error_code", 10,
-        [this](const std_msgs::msg::UInt32::SharedPtr msg)
-        {
-            this->errorCodeCallback(msg);
-        });
-
-    RCLCPP_INFO(node_->get_logger(), "错误码订阅器已启动");
-}
-
 std::string ReportManager::generateSystemStatusReport(
     WheelchairState wheelchair_state,
     BatteryState battery_state,
@@ -240,25 +227,17 @@ ErrorInfo ReportManager::parseErrorCode(uint32_t error_code)
 
 void ReportManager::processErrorCode(uint32_t error_code)
 {
-    ErrorInfo error_info = parseErrorCode(error_code);
-    processErrorCode(error_info);
+    // 仅记录到活跃错误列表用于报告
+    std::lock_guard<std::mutex> lock(error_mutex_);
+    ErrorInfo info = parseErrorCode(error_code);
+    active_errors_[error_code] = info;
 }
 
 void ReportManager::processErrorCode(const ErrorInfo &error_info)
 {
+    // 仅记录到活跃错误列表用于报告
     std::lock_guard<std::mutex> lock(error_mutex_);
-
-    // 检查是否为新错误
-    if (active_errors_.find(error_info.error_code) == active_errors_.end())
-    {
-        // 新错误
-        handleNewError(error_info);
-    }
-    else
-    {
-        // 已有错误，更新时间戳
-        active_errors_[error_info.error_code].timestamp = error_info.timestamp;
-    }
+    active_errors_[error_info.error_code] = error_info;
 }
 
 void ReportManager::handleNewError(const ErrorInfo &error_info)