wheelchair_state_machine

src/interface/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(interface)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# 生成接口 - 这个命令会自动处理所有需要的导出
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/SceneControls.srv"
+  DEPENDENCIES std_msgs
+)
+
+# 对于简单的接口包，只需要这个就够了
+# rosidl_generate_interfaces 会自动处理头文件安装和导出
+
+ament_package()

src/interface/msg/Empty.msg

@@ -0,0 +1 @@
+# 空消息，可用于简单触发

src/interface/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>interface</name>
+  <version>0.0.0</version>
+  <description>Custom interface package for SceneControls service</description>
+  <maintainer email="zhangkaifeng@users.noreply.github.com">zkf</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>rosidl_default_runtime</depend>
+  
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/interface/srv/SceneControls.srv

@@ -0,0 +1,4 @@
+int32 start
+---
+int32 code
+string message

src/wheelchair_state_machine/CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.5)
+project(wheelchair_state_machine)
+
+# 默认使用 C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+# 查找 ROS2 包
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+# 1. 主节点可执行文件
+add_executable(wheelchair_state_machine_node
+  src/main.cpp
+  src/battery_manager.cpp
+  src/event_input.cpp
+  src/workflow.cpp
+  src/ipad_manager.cpp
+  src/lidascan_ctrl.cpp
+  src/recharge_tool.cpp
+  src/report.cpp
+  src/rotation_manager.cpp
+)
+
+# 设置头文件路径
+target_include_directories(wheelchair_state_machine_node PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${Eigen3_INCLUDE_DIRS}
+)
+
+# 设置 ROS2 依赖
+ament_target_dependencies(wheelchair_state_machine_node
+  rclcpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  visualization_msgs
+  tf2_geometry_msgs
+)
+
+# 安装规则
+install(TARGETS
+  wheelchair_state_machine_node
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+ament_package()

src/wheelchair_state_machine/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/wheelchair_state_machine/include/wheelchair_state_machine/battery_manager.hpp

@@ -0,0 +1,50 @@
+// battery_manager.hpp
+#ifndef BATTERY_MANAGER_HPP
+#define BATTERY_MANAGER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+#include "wheelchair_types.hpp"
+#include <functional>
+
+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,
+                   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::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();
+    std::string batteryStateToString(BatteryState state) const;
+};
+
+#endif // BATTERY_MANAGER_HPPs

src/wheelchair_state_machine/include/wheelchair_state_machine/error_code.hpp

@@ -0,0 +1,125 @@
+// error_code.hpp
+#ifndef ERROR_CODE_HPP
+#define ERROR_CODE_HPP
+
+#include <string>
+#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 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
+};
+
+// 错误码数据结构
+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
+    {
+        return static_cast<ErrorModule>((error_code >> 24) & 0xFF);
+    }
+
+    ErrorSubModule getSubModule() const
+    {
+        return static_cast<ErrorSubModule>((error_code >> 16) & 0xFF);
+    }
+
+    uint16_t getErrorNumber() const
+    {
+        return error_code & 0xFFFF;
+    }
+
+    // 判断是否为严重错误
+    bool isCritical() const
+    {
+        // 根据文档，硬件错误和部分功能错误为严重错误
+        auto sub = getSubModule();
+        auto module = getModule();
+
+        // 硬件错误总是严重
+        if (sub == ErrorSubModule::HARDWARE)
+            return true;
+
+        // 关键模块的通信错误
+        if (sub == ErrorSubModule::COMMUNICATION)
+        {
+            return (module == ErrorModule::IMU ||
+                    module == ErrorModule::LIDAR1 ||
+                    module == ErrorModule::DRIVER);
+        }
+
+        // 回充功能相关的错误
+        if (module == ErrorModule::RECHARGE)
+        {
+            uint16_t error_num = getErrorNumber();
+            // 根据文档，回充功能错误码 0x0001-0x0003 为严重错误
+            return (error_num >= 0x0001 && error_num <= 0x0003);
+        }
+
+        return false;
+    }
+};
+
+// 错误码消息类型（用于ROS topic）
+struct ErrorCodeMsg
+{
+    uint32_t error_code;
+    uint64_t timestamp;
+    uint8_t error_level; // 0: INFO, 1: WARNING, 2: ERROR, 3: CRITICAL
+    std::string description;
+};
+
+#endif // ERROR_CODE_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/event_input.hpp

@@ -0,0 +1,89 @@
+// event_input.hpp
+#ifndef EVENT_INPUT_HANDLER_HPP
+#define EVENT_INPUT_HANDLER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <functional>
+#include <map>
+#include <memory>
+#include "wheelchair_types.hpp"
+#include "battery_manager.hpp"
+#include "lidascan_ctrl.hpp"
+#include "ipad_manager.hpp"
+#include "workflow.hpp"
+#include "report.hpp"
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/string.hpp>
+
+class EventInputHandler
+{
+public:
+    using EventCallback = std::function<void(const std::string &)>;
+
+    EventInputHandler(rclcpp::Node *node);
+
+    // 初始化接口
+    void initializeModules();
+
+    // 事件注册接口
+    void registerEvent(const std::string &event_name, EventCallback callback);
+
+    // 事件触发接口
+    void triggerEvent(const std::string &event_name);
+    void triggerEvent(const std::string &event_name, const std::string &data);
+
+    // 特定事件触发（仅外部输入事件）
+    void triggerIpadStart();
+    void triggerIpadCancel();
+    void triggerBluetoothDisconnected();
+    void triggerBluetoothConnected();
+    void triggerBaseStationPowerOff();
+    void triggerLowBattery(BatteryState state, float percentage);
+    void triggerLockVehicle();
+    void triggerUnlockVehicle();
+    void triggerJoystickPullBack();
+    void triggerJoystickStop();
+    void triggerPushStart();
+    void triggerPushStop();
+    void triggerBatteryStartCharging();
+    void triggerBatteryStopCharging();
+    void triggerBatteryFull();
+    void triggerBaseStationLost();
+    void triggerSearchTimeout();
+
+    // 系统状态查询
+    std::string getCurrentState() const;
+    bool isRechargeActive() const;
+
+    // 系统状态显示
+    void displaySystemStatus();
+
+private:
+    rclcpp::Node *node_;
+
+    // 模块化组件
+    std::unique_ptr<BatteryManager> battery_manager_;
+    std::unique_ptr<LidarScanController> lidar_controller_;
+    std::unique_ptr<IpadManager> ipad_manager_;
+    std::unique_ptr<WorkflowManager> workflow_manager_;
+    std::unique_ptr<ReportManager> report_manager_;
+
+    // ROS订阅者
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr cmd_enable_sub_;
+
+    // 事件回调映射
+    std::map<std::string, EventCallback> event_callbacks_;
+
+    // 状态标志
+    bool interface_active_ = false;
+
+    // 私有方法
+    void setupModuleCallbacks();
+    void setupSubscriptions();
+    void startNavigationControl();
+    void stopNavigationControl();
+    void logEvent(const std::string &event_name, const std::string &details = "");
+};
+
+#endif // EVENT_INPUT_HANDLER_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/ipad_manager.hpp

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

src/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -0,0 +1,131 @@
+// lidascan_ctrl.hpp
+#ifndef LIDAR_SCAN_CONTROLLER_HPP
+#define LIDAR_SCAN_CONTROLLER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+#include "wheelchair_types.hpp"
+#include "recharge_tool.hpp"
+#include <map>
+#include <memory>
+#include <tuple>
+#include <std_msgs/msg/bool.hpp>
+
+class LidarScanController
+{
+public:
+    using LaserScan = sensor_msgs::msg::LaserScan;
+    using LaserScanSharedPtr = sensor_msgs::msg::LaserScan::SharedPtr;
+
+    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_; };
+
+    void publishStationDetected(bool detected);
+
+    void resetStationDetection();
+
+private:
+    rclcpp::Node *node_;
+
+    // ROS2 订阅者和发布者
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_ctrl_;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_rviz_;
+    rclcpp::TimerBase::SharedPtr navigation_timer_;
+    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
+    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_station_detected_; // 新增
+
+    // 状态变量
+    bool active_;
+    ChargingState charging_state_;
+    geometry_msgs::msg::Twist ctrl_twist_;
+    rclcpp::Time ctrl_twist_time_;
+    rclcpp::Time detect_station_time_;
+
+    bool station_detected_;
+    rclcpp::Time last_station_publish_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 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>> laserScanToXY1(int begin_index, int end_index,
+                                                            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);
+
+    // 可视化
+    void publishRechargeVisualization(const RechargeResult &result);
+
+    // ========== 辅助函数 ==========
+    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 getFixed() const { return 0; }
+    Eigen::MatrixXd getPointList(const std::vector<IndexPoint> &points);
+
+    // 定时器回调
+    void navigationTimerCallback();
+
+    // 控制发布
+    void publishControl();
+};
+
+#endif // LIDAR_SCAN_CONTROLLER_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/recharge_tool.hpp

@@ -0,0 +1,29 @@
+// recharge_tool.hpp
+#ifndef RECHARGE_TOOL_HPP
+#define RECHARGE_TOOL_HPP
+
+#include "wheelchair_types.hpp"
+#include <vector>
+#include <Eigen/Dense>
+
+class RechargeTool
+{
+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);
+};
+
+#endif // RECHARGE_TOOL_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/report.hpp

@@ -0,0 +1,113 @@
+// report.hpp
+#ifndef REPORT_MANAGER_HPP
+#define REPORT_MANAGER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <functional>
+#include <memory>
+#include <map>
+#include <vector>
+#include <mutex>
+#include "wheelchair_types.hpp"
+#include "error_code.hpp"
+#include <std_msgs/msg/string.hpp>
+#include <std_msgs/msg/u_int32.hpp>
+
+class ReportManager
+{
+public:
+    using StatusReportCallback = std::function<void(const std::string &)>;
+    using ErrorCallback = std::function<void(const ErrorInfo &)>;
+
+    ReportManager(rclcpp::Node *node);
+
+    // 报告生成接口
+    std::string generateSystemStatusReport(
+        WheelchairState wheelchair_state,
+        BatteryState battery_state,
+        float battery_percentage,
+        ChargingState charging_state,
+        bool charging_voltage_detected,
+        bool recharge_active,
+        const std::string &control_mode);
+
+    // 状态报告发布
+    void publishStatusReport(const std::string &report);
+
+    // 特定状态报告
+    void reportSystemStatus(
+        WheelchairState wheelchair_state,
+        BatteryState battery_state,
+        float battery_percentage,
+        ChargingState charging_state,
+        bool charging_voltage_detected,
+        bool recharge_active,
+        const std::string &control_mode);
+
+    void reportBatteryStatus(BatteryState state, float percentage);
+    void reportChargingStatus(ChargingState state);
+    void reportNavigationStatus(const std::string &status);
+    void reportError(const std::string &error_message);
+    void reportWarning(const std::string &warning_message);
+    void reportInfo(const std::string &info_message);
+
+    // 错误处理接口
+    void processErrorCode(uint32_t error_code);
+    void processErrorCode(const ErrorInfo &error_info);
+    void notifyErrorRecovered(uint32_t error_code);
+
+    // 错误查询接口
+    bool hasCriticalErrors() const;
+    bool hasError(uint32_t error_code) const;
+    std::vector<ErrorInfo> getActiveErrors() const;
+    std::vector<ErrorInfo> getCriticalErrors() const;
+
+    // 回调设置
+    void setStatusReportCallback(StatusReportCallback callback);
+    void setErrorCallback(ErrorCallback callback);
+    void setCriticalErrorCallback(ErrorCallback callback);
+    void setRecoveryCallback(ErrorCallback callback);
+
+private:
+    rclcpp::Node *node_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr report_publisher_;
+
+    // 错误订阅者
+    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr error_sub_;
+
+    StatusReportCallback status_report_callback_;
+
+    // 错误管理相关
+    std::map<uint32_t, ErrorInfo> active_errors_;
+    mutable std::mutex error_mutex_;
+
+    ErrorCallback error_callback_;
+    ErrorCallback critical_error_callback_;
+    ErrorCallback recovery_callback_;
+
+    // 错误码映射
+    static std::map<uint32_t, std::string> error_descriptions_;
+    static std::map<ErrorModule, std::string> module_names_;
+    static std::map<ErrorSubModule, std::string> sub_module_names_;
+
+    // 辅助函数
+    std::string wheelchairStateToString(WheelchairState state) const;
+    std::string batteryStateToString(BatteryState state) const;
+    std::string chargingStateToString(ChargingState state) const;
+
+    // 错误处理函数
+    void errorCodeCallback(const std_msgs::msg::UInt32::SharedPtr msg);
+    ErrorInfo parseErrorCode(uint32_t error_code);
+    void handleNewError(const ErrorInfo &error_info);
+    void handleErrorRecovery(const ErrorInfo &error_info);
+
+    static std::string getErrorDescription(uint32_t error_code);
+    static std::string getModuleName(ErrorModule module);
+    static std::string getSubModuleName(ErrorSubModule sub_module);
+    static void initializeErrorMaps();
+
+    void logReport(const std::string &category, const std::string &message);
+};
+
+#endif // REPORT_MANAGER_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/rotation_manager.hpp

@@ -0,0 +1,57 @@
+// 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

src/wheelchair_state_machine/include/wheelchair_state_machine/wheelchair_types.hpp

@@ -0,0 +1,83 @@
+// wheelchair_types.hpp
+#ifndef WHEELCHAIR_TYPES_HPP
+#define WHEELCHAIR_TYPES_HPP
+
+#include <vector>
+#include <string>
+#include <Eigen/Dense>
+
+// 轮椅状态枚举
+enum class WheelchairState
+{
+    READY,     // 就绪中
+    WALKING,   // 行走中
+    ROTATING,  // 旋转中
+    SEARCHING, // 搜索中
+    CHARGING   // 充电中
+};
+
+// 充电状态枚举
+enum class ChargingState
+{
+    IDLE,
+    NAVIGATING,
+    CHARGING,
+    UNAVAILABLE,
+    COMPLETED
+};
+
+// 电池状态枚举
+enum class BatteryState
+{
+    NORMAL,   // 正常
+    LOW,      // 低电量
+    CRITICAL, // 严重低电量
+    CHARGING, // 充电中
+    FULL      // 充满
+};
+
+// ==================== 数据结构 ====================
+struct IndexPoint
+{
+    double x;
+    double y;
+    int index;
+
+    IndexPoint(double x = 0.0, double y = 0.0, int index = 0)
+        : x(x), y(y), index(index) {}
+};
+
+struct StraightLine
+{
+    double k;
+    double b;
+
+    StraightLine(double k = 0.0, double b = 0.0) : k(k), b(b) {}
+};
+
+struct ScanFilterParam
+{
+    int min_index = 292;
+    int max_index = 382;
+    double min_intensities = 120.0;
+    double max_intensities = 220.0;
+    double min_distance = 0.28;
+    double max_distance = 3.5;
+    int min_count = 4;
+    double dock_length = 0.42;
+    int baselink_back_index = 337;
+};
+
+struct RechargeResult
+{
+    int frameid;
+    std::vector<IndexPoint> points;
+    std::vector<IndexPoint> left_points;
+    std::vector<IndexPoint> right_points;
+    IndexPoint middle_point;
+    StraightLine middle_line;
+    StraightLine left_line;
+    StraightLine right_line;
+};
+
+#endif // WHEELCHAIR_TYPES_HPP

src/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -0,0 +1,158 @@
+// workflow.hpp
+#ifndef WORKFLOW_HPP
+#define WORKFLOW_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#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 "rotation_manager.hpp" // 新增头文件
+#include <std_msgs/msg/bool.hpp>
+
+class WorkflowManager
+{
+public:
+    using StateUpdateCallback = std::function<void(const std::string &, const std::string &)>;
+
+    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 transitionToRotating();
+    void transitionToCharging();
+    void transitionToReady();
+
+    // 紧急状态转移
+    void emergencyTransitionToReady(const std::string &reason = "未知原因");
+
+    // ==================== 旋转管理接口 ====================
+    void startRotationTimer();      // 新增
+    void checkRotationTransition(); // 新增
+
+    // ==================== 回调设置接口 ====================
+
+    void setStateUpdateCallback(StateUpdateCallback callback);
+    void setRechargeStatusCallback(StateUpdateCallback callback);
+    void setRechargeErrorCallback(std::function<void(const ErrorInfo &)> callback);
+
+    // ==================== 定时器管理 ====================
+
+    void startSearchTimeoutTimer();
+    void stopSearchTimeoutTimer();
+    void stopRotationTimer();
+
+    // ==================== 事件处理接口 ====================
+
+    // 外部事件处理
+    void handleLowBatteryEvent(BatteryState state, float percentage);
+    void handleChargingStartEvent();
+    void handleChargingStopEvent();
+    void handleChargingFullEvent();
+    void handleBaseStationLostEvent();
+    void handleSearchTimeoutEvent();
+    void handleRechargeFailure(const std::string &reason);
+
+    // 内部事件触发
+    void triggerEvent(const std::string &event_name);
+
+    void startRotationTimeoutTimer();
+    void stopRotationTimeoutTimer();
+
+private:
+    rclcpp::Node *node_;
+
+    // 状态机状态
+    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::Subscription<std_msgs::msg::Bool>::SharedPtr station_detected_sub_; // 新增
+
+    // 定时器
+    rclcpp::TimerBase::SharedPtr search_timeout_timer_;
+    rclcpp::TimerBase::SharedPtr rotation_check_timer_; // 新增
+    rclcpp::Time search_start_time_;                    // 新增
+
+    // 回调函数
+    StateUpdateCallback state_update_callback_;
+    StateUpdateCallback recharge_status_callback_;
+    std::function<void(const ErrorInfo &)> recharge_error_callback_;
+
+    // ==================== 私有方法 ====================
+
+    // 初始化方法
+    void initializeTransitionTable();
+    void initializeRotationManager();
+    void initializeSubscriptions(); // 新增
+
+    // 事件权限检查
+    bool checkEventPermission(const std::string &event);
+
+    // 事件执行
+    void executeEvent(const std::string &event);
+    void processStateTransition(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);
+
+    // ==================== 具体事件处理函数 ====================
+
+    // 外部事件处理
+    void processIpadStartEvent();
+    void processIpadCancelEvent();
+    void processBluetoothDisconnected();
+    void processBluetoothConnected();
+    void processBaseStationPowerOff();
+    void processLockVehicle();
+    void processUnlockVehicle();
+    void processJoystickPullBack();
+    void processJoystickStop();
+    void processPushStart();
+    void processPushStop();
+
+    // 错误码处理（保留接口）
+    void processErrorCode();
+
+    // ==================== 异常处理函数 ====================
+
+    void handleException(const std::string &context, const std::exception &e);
+
+    // ==================== 定时器回调 ====================
+
+    void searchTimeoutCallback();
+    void rotationCheckCallback();
+
+    void stationDetectedCallback(const std_msgs::msg::Bool::SharedPtr msg);
+};
+#endif

src/wheelchair_state_machine/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>wheelchair_state_machine</name>
+  <version>0.0.0</version>
+  <description>智能轮椅自主回充系统状态机</description>
+  <maintainer email="zhangkaifeng@users.noreply.github.com">zkf</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>interface</depend>
+  
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/wheelchair_state_machine/src/battery_manager.cpp

@@ -0,0 +1,190 @@
+// battery_manager.cpp
+#include "wheelchair_state_machine/battery_manager.hpp"
+#include <rclcpp/logging.hpp>
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+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)
+{
+}
+
+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_);
+}
+
+void BatteryManager::batteryStateCallback(const sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+    updateFromMessage(msg);
+}
+
+void BatteryManager::updateFromMessage(const sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+    // 更新电池百分比
+    if (msg->percentage >= 0.0 && msg->percentage <= 1.0)
+    {
+        battery_percentage_ = msg->percentage * 100.0;
+    }
+    else if (msg->voltage > 0.0)
+    {
+        // 如果percentage无效，根据电压估算（示例）
+        float voltage = msg->voltage;
+        // 简单线性估算：假设12V为满电，10V为没电
+        battery_percentage_ = std::clamp((voltage - 10.0f) / 2.0f * 100.0f, 0.0f, 100.0f);
+    }
+    else
+    {
+        battery_percentage_ = 100.0f; // 默认值
+    }
+
+    // 检查充电状态
+    if (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING)
+    {
+        // 如果正在充电，更新状态
+        battery_state_ = BatteryState::CHARGING;
+        if (battery_state_callback_)
+            battery_state_callback_(battery_state_, battery_percentage_);
+    }
+    else if (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL)
+    {
+        // 电池已充满
+        battery_state_ = BatteryState::FULL;
+        if (battery_state_callback_)
+            battery_state_callback_(battery_state_, battery_percentage_);
+    }
+    else
+    {
+        // 更新电池状态
+        updateBatteryState(battery_percentage_);
+    }
+
+    // 检查电池阈值并触发相应动作
+    checkBatteryThresholds();
+
+    // 定期记录电池状态（每10%记录一次）
+    static int last_logged_percentage = -1;
+    int current_10percent = static_cast<int>(battery_percentage_ / 10.0);
+    if (current_10percent != last_logged_percentage)
+    {
+        RCLCPP_INFO(node_->get_logger(), "电池状态: %.1f%% [%s]",
+                    battery_percentage_, stateToString().c_str());
+        last_logged_percentage = current_10percent;
+    }
+}
+
+void BatteryManager::updateBatteryState(float percentage)
+{
+    BatteryState old_state = battery_state_;
+
+    if (percentage <= critical_battery_threshold_)
+    {
+        battery_state_ = BatteryState::CRITICAL;
+    }
+    else if (percentage <= low_battery_threshold_)
+    {
+        battery_state_ = BatteryState::LOW;
+    }
+    else
+    {
+        battery_state_ = BatteryState::NORMAL;
+    }
+
+    // 状态变化时记录
+    if (old_state != battery_state_)
+    {
+        RCLCPP_WARN(node_->get_logger(), "电池状态变化: %s -> %s (%.1f%%)",
+                    batteryStateToString(old_state).c_str(),
+                    batteryStateToString(battery_state_).c_str(),
+                    percentage);
+
+        if (battery_state_callback_)
+            battery_state_callback_(battery_state_, battery_percentage_);
+    }
+}
+
+void BatteryManager::checkBatteryThresholds()
+{
+    static bool low_battery_triggered = false;
+    static bool critical_battery_triggered = false;
+
+    // 检查低电量警告
+    if (battery_state_ == BatteryState::LOW && !low_battery_triggered)
+    {
+        RCLCPP_WARN(node_->get_logger(), "低电量警告: %.1f%% (阈值: %.1f%%)",
+                    battery_percentage_, low_battery_threshold_);
+        if (low_battery_callback_)
+            low_battery_callback_(battery_state_, battery_percentage_);
+        low_battery_triggered = true;
+        critical_battery_triggered = false; // 重置严重低电量触发
+    }
+
+    // 检查严重低电量警告
+    if (battery_state_ == BatteryState::CRITICAL && !critical_battery_triggered)
+    {
+        RCLCPP_ERROR(node_->get_logger(), "严重低电量警告: %.1f%% (阈值: %.1f%%)",
+                     battery_percentage_, critical_battery_threshold_);
+        if (low_battery_callback_)
+            low_battery_callback_(battery_state_, battery_percentage_);
+        critical_battery_triggered = true;
+    }
+
+    // 如果电池恢复到正常水平，重置触发标志
+    if (battery_state_ == BatteryState::NORMAL)
+    {
+        low_battery_triggered = false;
+        critical_battery_triggered = false;
+    }
+}
+
+void BatteryManager::setBatteryStateCallback(BatteryStateCallback callback)
+{
+    battery_state_callback_ = callback;
+}
+
+void BatteryManager::setLowBatteryCallback(LowBatteryCallback callback)
+{
+    low_battery_callback_ = callback;
+}
+
+std::string BatteryManager::stateToString() const
+{
+    return batteryStateToString(battery_state_);
+}
+
+std::string BatteryManager::batteryStateToString(BatteryState state) const
+{
+    switch (state)
+    {
+    case BatteryState::NORMAL:
+        return "正常";
+    case BatteryState::LOW:
+        return "低电量";
+    case BatteryState::CRITICAL:
+        return "严重低电量";
+    case BatteryState::CHARGING:
+        return "充电中";
+    case BatteryState::FULL:
+        return "充满";
+    default:
+        return "未知";
+    }
+}

src/wheelchair_state_machine/src/event_input.cpp

@@ -0,0 +1,458 @@
+// 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)
+{
+    RCLCPP_INFO(node_->get_logger(), "事件输入处理器已初始化");
+}
+
+void EventInputHandler::initializeModules()
+{
+    // 初始化电池管理器
+    battery_manager_ = std::make_unique<BatteryManager>(node_, 20.0f, 10.0f);
+    battery_manager_->initialize();
+
+    // 初始化激光控制器
+    lidar_controller_ = std::make_unique<LidarScanController>(node_);
+    lidar_controller_->initialize();
+
+    // 初始化iPad管理器
+    ipad_manager_ = std::make_unique<IpadManager>(node_);
+
+    // 初始化工作流管理器
+    workflow_manager_ = std::make_unique<WorkflowManager>(node_);
+    // 初始化报告管理器
+    report_manager_ = std::make_unique<ReportManager>(node_);
+
+    setupModuleCallbacks();
+    setupSubscriptions();
+
+    RCLCPP_INFO(node_->get_logger(), "所有模块初始化完成");
+}
+
+void EventInputHandler::setupModuleCallbacks()
+{
+    // 1. 设置电池管理器回调
+    battery_manager_->setBatteryStateCallback(
+        [this](BatteryState state, float percentage)
+        {
+            // 报告电池状态
+            report_manager_->reportBatteryStatus(state, percentage);
+
+            // 触发电池状态事件
+            if (state == BatteryState::CHARGING)
+            {
+                triggerEvent("battery_start_charging");
+            }
+            else if (state == BatteryState::FULL)
+            {
+                triggerEvent("battery_full");
+            }
+        });
+
+    battery_manager_->setLowBatteryCallback(
+        [this](BatteryState state, float percentage)
+        {
+            // 触发低电量事件
+            triggerLowBattery(state, percentage);
+        });
+
+    // 2. 设置iPad管理器回调
+    ipad_manager_->setRechargeStartCallback(
+        [this]()
+        {
+            RCLCPP_INFO(node_->get_logger(), "iPad启动回充");
+            if (lidar_controller_)
+            {
+                lidar_controller_->resetStationDetection();
+                RCLCPP_INFO(node_->get_logger(), "已重置基站检测状态为false");
+            }
+            triggerEvent("ipad_phone_interface_start");
+        });
+
+    ipad_manager_->setRechargeCancelCallback(
+        [this]()
+        {
+            RCLCPP_INFO(node_->get_logger(), "iPad取消回充");
+            triggerEvent("ipad_phone_interface_cancel");
+        });
+
+    // 3. 设置工作流管理器状态更新回调
+    workflow_manager_->setStateUpdateCallback(
+        [this](const std::string &state_str, const std::string &message)
+        {
+            // 报告状态更新
+            report_manager_->reportInfo(state_str + ": " + message);
+
+            // 根据状态变化控制导航
+            WheelchairState current_state = workflow_manager_->getCurrentWheelchairState();
+            if (current_state == WheelchairState::SEARCHING)
+            {
+                startNavigationControl();
+            }
+            else if (current_state == WheelchairState::ROTATING) // 新增：旋转状态
+            {
+                // 旋转状态由旋转管理器控制，不需要启动导航控制
+                // 可以添加其他处理逻辑
+                RCLCPP_INFO(node_->get_logger(), "进入旋转搜索状态");
+            }
+            else if (current_state == WheelchairState::CHARGING)
+            {
+                lidar_controller_->stopMotion();
+                lidar_controller_->setChargingVoltage(true);
+            }
+            else if (current_state == WheelchairState::READY)
+            {
+                stopNavigationControl();
+            }
+            else if (current_state == WheelchairState::WALKING)
+            {
+                RCLCPP_INFO(node_->get_logger(), "进入行走对接状态");
+            }
+        });
+
+    // 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")
+            {
+                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);
+            }
+        });
+
+    // 5. 设置事件处理器回调（仅外部输入事件）
+    registerEvent("ipad_phone_interface_start",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理iPad启动事件");
+                      workflow_manager_->handleEvent("ipad_phone_interface_start");
+                  });
+
+    registerEvent("ipad_phone_interface_cancel",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理iPad取消事件");
+                      workflow_manager_->handleEvent("ipad_phone_interface_cancel");
+                  });
+
+    registerEvent("low_battery_warning",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理低电量事件");
+                      workflow_manager_->handleEvent("low_battery_warning");
+                  });
+
+    registerEvent("battery_start_charging",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理开始充电事件");
+                      workflow_manager_->handleEvent("battery_start_charging");
+                  });
+
+    registerEvent("battery_stop_charging",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理停止充电事件");
+                      workflow_manager_->handleEvent("battery_stop_charging");
+                  });
+
+    registerEvent("battery_full",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理电池充满事件");
+                      workflow_manager_->handleEvent("battery_full");
+                  });
+
+    registerEvent("bluetooth_disconnected",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理蓝牙断开事件");
+                      workflow_manager_->handleEvent("bluetooth_disconnected");
+                  });
+
+    registerEvent("bluetooth_connected",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理蓝牙连接事件");
+                      workflow_manager_->handleEvent("bluetooth_connected");
+                  });
+
+    registerEvent("base_station_power_off",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理基站断电事件");
+                      workflow_manager_->handleEvent("base_station_power_off");
+                  });
+
+    registerEvent("lock_vehicle",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理锁车事件");
+                      workflow_manager_->handleEvent("lock_vehicle");
+                  });
+
+    registerEvent("unlock_vehicle",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理解锁事件");
+                      workflow_manager_->handleEvent("unlock_vehicle");
+                  });
+
+    registerEvent("joystick_pull_back",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理摇杆后拉事件");
+                      workflow_manager_->handleEvent("joystick_pull_back");
+                  });
+
+    registerEvent("joystick_stop",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理摇杆停止事件");
+                      workflow_manager_->handleEvent("joystick_stop");
+                  });
+
+    registerEvent("push_start",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理推行启动事件");
+                      workflow_manager_->handleEvent("push_start");
+                  });
+
+    registerEvent("push_stop",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理推行停止事件");
+                      workflow_manager_->handleEvent("push_stop");
+                  });
+
+    registerEvent("base_station_lost",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理基站丢失事件");
+                      workflow_manager_->handleEvent("base_station_lost");
+                  });
+
+    registerEvent("search_30s_timeout",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理搜索超时事件");
+                      workflow_manager_->handleEvent("search_30s_timeout");
+                  });
+
+    RCLCPP_INFO(node_->get_logger(), "模块回调设置完成");
+}
+
+void EventInputHandler::setupSubscriptions()
+{
+    // 创建命令使能订阅者
+    cmd_enable_sub_ = node_->create_subscription<std_msgs::msg::String>(
+        "/scene_services/mode", 10,
+        [this](const std_msgs::msg::String::SharedPtr msg)
+        {
+            this->ipad_manager_->onCmdEnable(msg);
+        });
+
+    RCLCPP_INFO(node_->get_logger(), "订阅器设置完成");
+}
+
+void EventInputHandler::startNavigationControl()
+{
+    if (!interface_active_)
+    {
+        lidar_controller_->startNavigation();
+        interface_active_ = true;
+        RCLCPP_INFO(node_->get_logger(), "启动导航控制");
+    }
+}
+
+void EventInputHandler::stopNavigationControl()
+{
+    if (interface_active_)
+    {
+        lidar_controller_->stopNavigation();
+        lidar_controller_->stopMotion();
+        interface_active_ = false;
+        RCLCPP_INFO(node_->get_logger(), "停止导航控制");
+    }
+}
+
+void EventInputHandler::registerEvent(const std::string &event_name, EventCallback callback)
+{
+    event_callbacks_[event_name] = callback;
+    RCLCPP_INFO(node_->get_logger(), "已注册事件: %s", event_name.c_str());
+}
+
+void EventInputHandler::triggerEvent(const std::string &event_name)
+{
+    triggerEvent(event_name, "");
+}
+
+void EventInputHandler::triggerEvent(const std::string &event_name, const std::string &data)
+{
+    logEvent(event_name, data);
+
+    if (event_callbacks_.find(event_name) != event_callbacks_.end())
+    {
+        event_callbacks_[event_name](data);
+    }
+    else
+    {
+        RCLCPP_WARN(node_->get_logger(), "未注册的事件: %s", event_name.c_str());
+    }
+}
+
+void EventInputHandler::triggerIpadStart()
+{
+    triggerEvent("ipad_phone_interface_start", "iPad启动回充");
+}
+
+void EventInputHandler::triggerIpadCancel()
+{
+    triggerEvent("ipad_phone_interface_cancel", "iPad取消回充");
+}
+
+void EventInputHandler::triggerBluetoothDisconnected()
+{
+    triggerEvent("bluetooth_disconnected", "蓝牙断开连接");
+}
+
+void EventInputHandler::triggerBluetoothConnected()
+{
+    triggerEvent("bluetooth_connected", "蓝牙已连接");
+}
+
+void EventInputHandler::triggerBaseStationPowerOff()
+{
+    triggerEvent("base_station_power_off", "基站断电");
+}
+
+void EventInputHandler::triggerLowBattery(BatteryState state, float percentage)
+{
+    std::string data = "低电量警告 - 状态: ";
+    data += std::to_string(static_cast<int>(state));
+    data += ", 电量: " + std::to_string(percentage) + "%";
+    triggerEvent("low_battery_warning", data);
+}
+
+void EventInputHandler::triggerLockVehicle()
+{
+    triggerEvent("lock_vehicle", "锁车操作");
+}
+
+void EventInputHandler::triggerUnlockVehicle()
+{
+    triggerEvent("unlock_vehicle", "解锁操作");
+}
+
+void EventInputHandler::triggerJoystickPullBack()
+{
+    triggerEvent("joystick_pull_back", "摇杆后拉");
+}
+
+void EventInputHandler::triggerJoystickStop()
+{
+    triggerEvent("joystick_stop", "摇杆停止");
+}
+
+void EventInputHandler::triggerPushStart()
+{
+    triggerEvent("push_start", "推行启动");
+}
+
+void EventInputHandler::triggerPushStop()
+{
+    triggerEvent("push_stop", "推行停止");
+}
+
+void EventInputHandler::triggerBatteryStartCharging()
+{
+    triggerEvent("battery_start_charging", "开始充电");
+}
+
+void EventInputHandler::triggerBatteryStopCharging()
+{
+    triggerEvent("battery_stop_charging", "停止充电");
+}
+
+void EventInputHandler::triggerBatteryFull()
+{
+    triggerEvent("battery_full", "电池充满");
+}
+
+void EventInputHandler::triggerBaseStationLost()
+{
+    triggerEvent("base_station_lost", "基站检测丢失");
+}
+
+void EventInputHandler::triggerSearchTimeout()
+{
+    triggerEvent("search_30s_timeout", "搜索30秒超时");
+}
+
+void EventInputHandler::logEvent(const std::string &event_name, const std::string &details)
+{
+    if (details.empty())
+    {
+        RCLCPP_INFO(node_->get_logger(), "触发事件: %s", event_name.c_str());
+    }
+    else
+    {
+        RCLCPP_INFO(node_->get_logger(), "触发事件: %s - %s",
+                    event_name.c_str(), details.c_str());
+    }
+}
+
+std::string EventInputHandler::getCurrentState() const
+{
+    if (workflow_manager_)
+        return workflow_manager_->getCurrentState();
+    return "未初始化";
+}
+
+bool EventInputHandler::isRechargeActive() const
+{
+    return interface_active_;
+}
+
+void EventInputHandler::displaySystemStatus()
+{
+    if (!workflow_manager_ || !battery_manager_ || !lidar_controller_ || !report_manager_)
+    {
+        RCLCPP_ERROR(node_->get_logger(), "系统未初始化");
+        return;
+    }
+
+    std::string control_mode = interface_active_ ? "iPad控制" : "本地控制";
+
+    report_manager_->reportSystemStatus(
+        workflow_manager_->getCurrentWheelchairState(),
+        battery_manager_->getBatteryState(),
+        battery_manager_->getBatteryPercentage(),
+        lidar_controller_->getChargingState(),
+        lidar_controller_->getChargingVoltage(),
+        isRechargeActive(),
+        control_mode);
+}

src/wheelchair_state_machine/src/ipad_manager.cpp

@@ -0,0 +1,97 @@
+// ipad_manager.cpp
+#include "wheelchair_state_machine/ipad_manager.hpp"
+#include <rclcpp/logging.hpp>
+
+using namespace std::chrono_literals;
+
+IpadManager::IpadManager(rclcpp::Node *node)
+    : node_(node), recharge_active_(false), cmd_enable_("auxiliary")
+{
+    logInfo("iPad管理器初始化完成");
+}
+
+void IpadManager::onCmdEnable(const std_msgs::msg::String::SharedPtr msg)
+{
+    std::string new_enable = msg->data;
+    logInfo("收到命令使能: " + new_enable);
+    // 状态变化检查
+    if (cmd_enable_ == "auxiliary" && new_enable == "recharge")
+    {
+        cmd_enable_ = "recharge";
+        startRecharge();
+    }
+    else if (cmd_enable_ == "recharge" && new_enable == "auxiliary")
+    {
+        cmd_enable_ = "auxiliary";
+        cancelRecharge();
+    }
+    else
+    {
+        // 状态未变化
+        logInfo("命令使能状态未变化");
+    }
+}
+
+void IpadManager::startRecharge()
+{
+    if (!recharge_active_)
+    {
+        recharge_active_ = true;
+        logInfo("启动回充功能");
+
+        if (recharge_start_callback_)
+        {
+            recharge_start_callback_();
+        }
+        else
+        {
+            logInfo("回充启动回调未设置");
+        }
+    }
+    else
+    {
+        logInfo("回充功能已在运行中");
+    }
+}
+
+void IpadManager::cancelRecharge()
+{
+    if (recharge_active_)
+    {
+        recharge_active_ = false;
+        logInfo("取消回充功能");
+
+        if (recharge_cancel_callback_)
+        {
+            recharge_cancel_callback_();
+        }
+        else
+        {
+            logInfo("回充取消回调未设置");
+        }
+    }
+    else
+    {
+        logInfo("回充功能未激活，无需取消");
+    }
+}
+
+void IpadManager::setRechargeStartCallback(RechargeStartCallback callback)
+{
+    recharge_start_callback_ = callback;
+    logInfo("已设置回充启动回调");
+}
+
+void IpadManager::setRechargeCancelCallback(RechargeCancelCallback callback)
+{
+    recharge_cancel_callback_ = callback;
+    logInfo("已设置回充取消回调");
+}
+
+void IpadManager::logInfo(const std::string &message)
+{
+    if (node_)
+    {
+        RCLCPP_INFO(node_->get_logger(), "[IpadManager] %s", message.c_str());
+    }
+}

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -0,0 +1,1122 @@
+// lidascan_ctrl.cpp
+#include "wheelchair_state_machine/lidascan_ctrl.hpp"
+#include <rclcpp/logging.hpp>
+#include <rclcpp/qos.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <cmath>
+#include <limits>
+#include <deque>
+#include <numeric>
+#include <thread>
+#include <Eigen/Dense>
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+LidarScanController::LidarScanController(rclcpp::Node *node)
+    : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0), station_detected_(false)
+{
+}
+
+LidarScanController::~LidarScanController()
+{
+}
+
+void LidarScanController::initialize()
+{
+    // 声明参数
+    node_->declare_parameter<double>("distance_threshold", 0.5);
+    node_->declare_parameter<int>("fit_window_size", 10);
+    node_->declare_parameter<double>("fit_residual_threshold", 0.09);
+
+    // 获取参数
+    distance_threshold_ = node_->get_parameter("distance_threshold").as_double();
+    fit_window_size_ = node_->get_parameter("fit_window_size").as_int();
+    fit_residual_threshold_ = node_->get_parameter("fit_residual_threshold").as_double();
+
+    // QoS配置
+    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10))
+                           .best_effort()
+                           .durability_volatile();
+
+    // 创建激光雷达订阅者
+    scan_sub_ = node_->create_subscription<sensor_msgs::msg::LaserScan>(
+        "/scan/back", qos_profile,
+        [this](const LaserScanSharedPtr msg)
+        {
+            if (active_)
+                processLaserScan(msg);
+        });
+
+    // 创建控制发布者
+    pub_ctrl_ = node_->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_raw", 10);
+
+    // 创建可视化发布者
+    pub_rviz_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>(
+        "/line_marker_array", 10);
+
+    // 创建过滤后的激光数据发布者
+    pub_filtered_ = node_->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out", 10);
+    pub_line_ = node_->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out_line", 10);
+
+    // 创建基站检测状态发布者（使用 transient_local 确保新订阅者能收到最后一条消息）
+    pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
+        "station/detected",
+        rclcpp::QoS(10).transient_local()); // 修改为 transient_local
+
+    // 初始化控制消息
+    ctrl_twist_.linear.x = 0.0;
+    ctrl_twist_.angular.z = 0.0;
+    ctrl_twist_time_ = node_->now();
+
+    // 创建导航定时器
+    navigation_timer_ = node_->create_wall_timer(
+        100ms, std::bind(&LidarScanController::navigationTimerCallback, this));
+
+    detect_station_time_ = node_->now() - rclcpp::Duration(100.0, 0);
+    detect_charging_voltage_time_ = node_->now();
+
+    // 初始化基站检测状态
+    station_detected_ = false;
+    last_station_publish_time_ = node_->now();
+
+    RCLCPP_INFO(node_->get_logger(), "激光扫描控制器已初始化");
+}
+
+void LidarScanController::resetStationDetection()
+{
+    // 重置内部状态
+    station_detected_ = false;
+    detect_charging_voltage_ = false;
+
+    // 发布false信号
+    auto false_msg = std_msgs::msg::Bool();
+    false_msg.data = false;
+    pub_station_detected_->publish(false_msg);
+
+    // 重置时间戳
+    last_station_publish_time_ = node_->now();
+
+    RCLCPP_INFO(node_->get_logger(), "基站检测状态已重置: false");
+}
+
+void LidarScanController::startNavigation()
+{
+    active_ = true;
+    charging_state_ = ChargingState::NAVIGATING;
+    station_detected_ = false;
+    RCLCPP_INFO(node_->get_logger(), "导航功能已启动");
+}
+
+void LidarScanController::stopNavigation()
+{
+    active_ = false;
+    charging_state_ = ChargingState::IDLE;
+    stopMotion();
+    RCLCPP_INFO(node_->get_logger(), "导航功能已停止");
+}
+
+void LidarScanController::publishStationDetected(bool detected)
+{
+    auto now = node_->now();
+    auto time_diff = (now - last_station_publish_time_).seconds();
+    // if (station_detected_ == false && detected == false)
+    // {
+    //     auto msg = std_msgs::msg::Bool();
+    //     msg.data = detected;
+    //     pub_station_detected_->publish(msg);
+    // }
+    // 防止频繁发布，至少间隔0.5秒
+    if (station_detected_ != detected)
+    {
+        auto msg = std_msgs::msg::Bool();
+        msg.data = detected;
+        pub_station_detected_->publish(msg);
+        station_detected_ = detected;
+    }
+    last_station_publish_time_ = now;
+
+    if (detected)
+    {
+        RCLCPP_INFO(node_->get_logger(), "发布基站检测成功信号: true");
+    }
+    else
+    {
+        RCLCPP_DEBUG(node_->get_logger(), "发布基站检测信号: false");
+    }
+}
+
+void LidarScanController::stopMotion()
+{
+    ctrl_twist_.linear.x = 0.0;
+    ctrl_twist_.angular.z = 0.0;
+    ctrl_twist_time_ = node_->now();
+    pub_ctrl_->publish(ctrl_twist_);
+    RCLCPP_INFO(node_->get_logger(), "停止运动");
+}
+
+void LidarScanController::navigationTimerCallback()
+{
+    if (!active_)
+        return;
+
+    auto now = node_->now();
+    auto time_diff = (now - ctrl_twist_time_).seconds();
+
+    if (time_diff < 2.0)
+    {
+        pub_ctrl_->publish(ctrl_twist_);
+    }
+}
+
+void LidarScanController::processLaserScan(const LaserScanSharedPtr &msg)
+{
+    if (!active_)
+        return;
+
+    // 应用过滤算法
+    filterScanModify0(msg);
+    filterScanModify1(msg);
+    filterScanModify2(msg);
+    filterScanModify3(msg);
+
+    auto [begin_index, end_index] = filterScanModify4(msg);
+
+    if (end_index - begin_index < 2)
+    {
+        RCLCPP_DEBUG(node_->get_logger(), "没有可连续性的点");
+        return;
+    }
+
+    // 处理分段数据
+    if (processSegmentData(msg, begin_index, end_index))
+    {
+        detect_station_time_ = node_->now();
+    }
+}
+
+bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
+                                             int begin_index, int end_index)
+{
+    // 转换到XY坐标系
+    auto xy_list = laserScanToXY1(begin_index, end_index, msg);
+    if (xy_list.size() < 2)
+    {
+        RCLCPP_WARN(node_->get_logger(), "XY转换结果点数不足: %zu", xy_list.size());
+        return false;
+    }
+
+    // 获取分段
+    auto sections = getSections(begin_index, end_index, msg, xy_list);
+    if (sections.empty())
+    {
+        RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
+        return false;
+    }
+
+    // 选择最近的分段
+    int nearest_index = -1;
+    double nearest_x = 100.0;
+
+    for (size_t i = 0; i < sections.size(); i++)
+    {
+        int section_start = sections[i].first;
+        int section_end = sections[i].second;
+
+        if (section_end - section_start < 10)
+            continue;
+
+        double avg_total = 0.0;
+        int avg_count = 0;
+
+        for (int index = section_start; index <= section_end; index++)
+        {
+            if (xy_list.find(index) == xy_list.end())
+                continue;
+
+            double x = msg->ranges[index];
+            avg_count++;
+            avg_total += x;
+        }
+
+        if (avg_count == 0)
+            continue;
+
+        double avg_value = avg_total / avg_count;
+
+        if (nearest_x > avg_value)
+        {
+            nearest_x = avg_value;
+            nearest_index = i;
+        }
+    }
+
+    if (nearest_index < 0)
+    {
+        return false;
+    }
+
+    begin_index = sections[nearest_index].first;
+    end_index = sections[nearest_index].second;
+
+    // 重置激光数据
+    auto filtered_msg = resetLaserData(msg, begin_index, end_index);
+
+    // 发布过滤后的数据
+    publishFilteredScan(filtered_msg, filtered_msg->ranges);
+
+    // 进一步处理
+    auto xy_list_full = laserScanToXY(filtered_msg);
+    if (xy_list_full.size() < 2)
+    {
+        // publishStationDetected(false);
+        RCLCPP_ERROR(node_->get_logger(), "xy坐标系没有转换成功");
+        return false;
+    }
+
+    // 获取分段
+    auto turn_segments = getSegments(xy_list_full);
+    auto filtered_segments = filterSegments(turn_segments);
+
+    if (filtered_segments.empty())
+    {
+        // publishStationDetected(false);
+        RCLCPP_DEBUG(node_->get_logger(), "未检测到有效的分段");
+        return false;
+    }
+
+    // 创建结果对象
+    RechargeResult result;
+    result.frameid = frame_index_++;
+
+    // 处理左右点
+    int segment_index = 0;
+    for (const auto &seg : filtered_segments)
+    {
+        const auto &info = seg.second;
+        int start_idx = std::get<0>(info);
+        int end_idx = std::get<1>(info);
+
+        for (int index = start_idx; index <= end_idx; index++)
+        {
+            if (xy_list_full.find(index) == xy_list_full.end())
+                continue;
+
+            double x = xy_list_full[index].first;
+            double y = xy_list_full[index].second;
+            IndexPoint item(x, y, index);
+
+            if (segment_index == 0)
+            {
+                result.left_points.push_back(item);
+            }
+            else if (segment_index == 1)
+            {
+                result.right_points.push_back(item);
+            }
+        }
+        segment_index++;
+
+        if (segment_index >= 2)
+            break; // 只需要两个分段
+    }
+
+    // 如果成功获取左右点，进行控制
+    if (!result.left_points.empty() && !result.right_points.empty())
+    {
+        procData(result);
+        return true;
+    }
+    // publishStationDetected(false);
+    return false;
+}
+
+void LidarScanController::procData(RechargeResult &result)
+{
+    if (result.left_points.size() < 2 || result.right_points.size() < 2)
+    {
+        RCLCPP_WARN(node_->get_logger(), "点数量不足，跳过拟合");
+        return;
+    }
+
+    // 拟合左边直线
+    auto [k1, b1] = RechargeTool::simpleFitLine(result.left_points);
+    RCLCPP_DEBUG(node_->get_logger(), "左直线拟合: y = %.4fx + %.4f", k1, b1);
+
+    // 拟合右边直线
+    auto [k2, b2] = RechargeTool::simpleFitLine(result.right_points);
+    RCLCPP_DEBUG(node_->get_logger(), "右直线拟合: y = %.4fx + %.4f", k2, b2);
+
+    // 检查是否平行
+    if (fabs(k1 - k2) < 1e-10)
+    {
+        RCLCPP_WARN(node_->get_logger(), "两直线平行，无交点");
+        result.middle_point = IndexPoint(0.0, 0.0, 0);
+        result.middle_line = StraightLine(0.0, 0.0);
+        result.left_line = StraightLine(k1, b1);
+        result.right_line = StraightLine(k2, b2);
+        return;
+    }
+
+    // 计算交点
+    double x = (b2 - b1) / (k1 - k2);
+    double y = k1 * x + b1;
+    RCLCPP_DEBUG(node_->get_logger(), "交点: (%.4f, %.4f)", x, y);
+
+    // 更新中间点
+    result.middle_point = IndexPoint(x, y, 0);
+
+    // 计算角平分线
+    auto [k3, b3] = RechargeTool::calculateAngleBisector(k1, b1, k2, b2);
+    RCLCPP_DEBUG(node_->get_logger(), "角平分线: y = %.4fx + %.4f", k3, b3);
+
+    // 更新结果
+    result.middle_line = StraightLine(k3, b3);
+    result.left_line = StraightLine(k1, b1);
+    result.right_line = StraightLine(k2, b2);
+
+    // 运动控制
+    controlRechargeMotion(result);
+
+    // 可视化
+    publishRechargeVisualization(result);
+}
+
+void LidarScanController::controlRechargeMotion(const RechargeResult &result)
+{
+    publishStationDetected(true);
+    // 如果检测到充电电压，停止移动
+    if (getChargingVoltage())
+    {
+        RCLCPP_INFO(node_->get_logger(), "检测到充电电压，停止移动");
+        ctrl_twist_.linear.x = 0.0;
+        ctrl_twist_.angular.z = 0.0;
+        ctrl_twist_time_ = node_->now();
+        return;
+    }
+
+    double dx = result.middle_point.x;
+    double dy = result.middle_point.y;
+    double k = result.middle_line.k;
+
+    RCLCPP_DEBUG(node_->get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);
+
+    // 调整 dy
+    dy = dy - 0.10;
+    RCLCPP_DEBUG(node_->get_logger(), "调整后 dy: %.4f", dy);
+
+    // 计算偏航角误差
+    double yaw_error = atan(k);
+    RCLCPP_DEBUG(node_->get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
+                 yaw_error, yaw_error * 180.0 / M_PI);
+
+    // 计算控制速度
+    auto [linear_velocity, angular_velocity] = RechargeTool::computeDockingVelocity(dx - 0.3, dy, yaw_error);
+    RCLCPP_DEBUG(node_->get_logger(), "基础控制速度 - 线速度: %.4f, 角速度: %.4f",
+                 linear_velocity, angular_velocity);
+
+    // 限制角速度
+    if (angular_velocity > 0.0)
+    {
+        angular_velocity = std::min(fabs(angular_velocity), 0.09);
+    }
+    else
+    {
+        angular_velocity = -std::min(fabs(angular_velocity), 0.09);
+    }
+
+    // 根据距离调整速度系数
+    double radio_x = 1.0;
+    double radio_z = 1.0;
+
+    if (dx > 1.0)
+    {
+        // 保持默认
+    }
+    else if (dx > 0.8)
+    {
+        radio_x = 0.7;
+    }
+    else if (dx > 0.7)
+    {
+        radio_x = 0.5;
+    }
+    else if (dx > 0.68)
+    {
+        radio_x = 0.3;
+        radio_z = 1.0;
+    }
+    else if (dx > 0.65)
+    {
+        radio_x = 0.2;
+        radio_z = 1.0;
+    }
+    else if (dx > 0.62)
+    {
+        radio_x = 0.1;
+        radio_z = 1.0;
+    }
+    else if (dx > 0.2)
+    {
+        radio_x = 0.15;
+        radio_z = 1.0;
+    }
+
+    RCLCPP_DEBUG(node_->get_logger(), "距离调整系数 - radio_x: %.2f, radio_z: %.2f", radio_x, radio_z);
+
+    // 设置控制命令
+    if (dx > 0.54)
+    {
+        double final_linear_vel = linear_velocity * radio_x;
+        if (final_linear_vel > -0.04)
+        {
+            final_linear_vel = -0.04;
+        }
+
+        ctrl_twist_.linear.x = final_linear_vel;
+        ctrl_twist_.angular.z = angular_velocity * radio_z;
+        ctrl_twist_time_ = node_->now();
+
+        RCLCPP_DEBUG(node_->get_logger(), "控制命令 - 线速度: %.4f, 角速度: %.4f",
+                     ctrl_twist_.linear.x, ctrl_twist_.angular.z);
+    }
+    else
+    {
+        // 到达目标位置
+        RCLCPP_INFO(node_->get_logger(), "到达目标位置 (dx=%.4f <= 0.54), 停止移动", dx);
+        ctrl_twist_.linear.x = 0.0;
+        ctrl_twist_.angular.z = 0.0;
+        ctrl_twist_time_ = node_->now();
+
+        // 模拟检测到充电电压
+        setChargingVoltage(true);
+    }
+}
+
+// ==================== 过滤函数实现 ====================
+void LidarScanController::filterScanModify0(LaserScanSharedPtr msg)
+{
+    int fixed = getFixed();
+    double diff_yaw = msg->angle_increment * fixed;
+    msg->angle_min -= diff_yaw;
+    msg->angle_max -= diff_yaw;
+
+    std::vector<float> ranges = msg->ranges;
+    for (size_t i = 0; i < msg->ranges.size(); i++)
+    {
+        int index = getFixedIndex(msg, i);
+        ranges[index] = msg->ranges[i];
+    }
+    msg->ranges = ranges;
+}
+
+void LidarScanController::filterScanModify1(LaserScanSharedPtr msg)
+{
+    // 过滤角度，只接受后面90度
+    int min_index = param_.min_index;
+    int max_index = param_.max_index;
+
+    for (size_t i = 0; i < msg->ranges.size(); i++)
+    {
+        if (i < min_index || i > max_index)
+        {
+            msg->ranges[i] = std::numeric_limits<float>::infinity();
+        }
+    }
+}
+
+void LidarScanController::filterScanModify2(LaserScanSharedPtr msg)
+{
+    // 过滤距离
+    for (size_t i = 0; i < msg->ranges.size(); i++)
+    {
+        if (!isValid(msg->ranges[i]))
+            continue;
+
+        if (msg->ranges[i] < param_.min_distance ||
+            msg->ranges[i] > param_.max_distance)
+        {
+            msg->ranges[i] = std::numeric_limits<float>::infinity();
+        }
+    }
+}
+
+void LidarScanController::filterScanModify3(LaserScanSharedPtr msg)
+{
+    // 过滤强度
+    for (size_t i = 0; i < msg->ranges.size(); i++)
+    {
+        if (!isValid(msg->ranges[i]))
+            continue;
+
+        float value = msg->intensities[i];
+        if (value < param_.min_intensities || value > param_.max_intensities)
+        {
+            msg->ranges[i] = std::numeric_limits<float>::infinity();
+        }
+
+        // 再次检查距离
+        if (msg->ranges[i] < param_.min_distance ||
+            msg->ranges[i] > param_.max_distance)
+        {
+            msg->ranges[i] = std::numeric_limits<float>::infinity();
+        }
+    }
+}
+
+std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr msg)
+{
+    // 过滤连续性
+    int begin_index = 0;
+    int end_index = 0;
+    std::vector<float> ranges = msg->ranges;
+
+    // 顺序找起点
+    for (int i = param_.min_index + 1; i < param_.max_index; i++)
+    {
+        if (!isValid(ranges[i]))
+            continue;
+        if (!isValid(ranges[i - 1]))
+        {
+            begin_index = i;
+            break;
+        }
+    }
+
+    if (static_cast<int>(msg->ranges.size()) - begin_index < 10)
+    {
+        RCLCPP_ERROR(node_->get_logger(), "数据太少无法继续处理");
+        return {0, 0};
+    }
+
+    // 倒序找终点
+    for (int i = param_.max_index - 2; i > 0; i--)
+    {
+        if (!isValid(ranges[i]))
+            continue;
+        if (!isValid(ranges[i + 1]))
+        {
+            end_index = i;
+            break;
+        }
+    }
+
+    if (end_index < 10)
+    {
+        return {0, 0};
+    }
+
+    // 顺序检查连续性
+    for (int i = begin_index + 1; i < end_index - 1; i++)
+    {
+        float prev = ranges[i - 1];
+        float curr = ranges[i];
+        float next = ranges[i + 1];
+
+        if (!isValid(prev) && !isValid(curr) && !isValid(next))
+        {
+            if (i - begin_index < param_.min_count)
+            {
+                begin_index = i + 1;
+            }
+            else
+            {
+                break;
+            }
+        }
+        else
+        {
+            float low = 100.0;
+            float high = 0.0;
+            for (int j = i - 1; j <= i + 1; j++)
+            {
+                if (isValid(ranges[j]))
+                {
+                    if (ranges[j] > 0.03 && ranges[j] < low)
+                    {
+                        low = ranges[j];
+                    }
+                    if (ranges[j] > high)
+                    {
+                        high = ranges[j];
+                    }
+                }
+            }
+            if (high - low > 0.08)
+            {
+                if (i - begin_index < param_.min_count)
+                {
+                    begin_index = i + 1;
+                }
+                else
+                {
+                    break;
+                }
+            }
+        }
+    }
+
+    // 倒序检查连续性
+    for (int i = end_index - 1; i > begin_index + 1; i--)
+    {
+        float prev = ranges[i - 1];
+        float curr = ranges[i];
+        float next = ranges[i + 1];
+
+        if (!isValid(prev) && !isValid(curr) && !isValid(next))
+        {
+            if (end_index - i < param_.min_count)
+            {
+                end_index = i - 1;
+            }
+            else
+            {
+                break;
+            }
+        }
+        else
+        {
+            if (isValid(next) && isValid(prev))
+            {
+                if (next - curr > 0.07)
+                {
+                    if (end_index - i < param_.min_count)
+                    {
+                        end_index = i - 1;
+                    }
+                    else
+                    {
+                        break;
+                    }
+                }
+            }
+        }
+    }
+
+    // 设置无效区域
+    for (int i = param_.min_index; i < begin_index - 1; i++)
+    {
+        msg->ranges[i] = std::numeric_limits<float>::infinity();
+    }
+    for (int i = end_index + 1; i <= param_.max_index; i++)
+    {
+        msg->ranges[i] = std::numeric_limits<float>::infinity();
+    }
+
+    return {begin_index, end_index};
+}
+
+// ==================== 坐标转换函数 ====================
+std::map<int, std::pair<double, double>> LidarScanController::laserScanToXY(
+    const LaserScanSharedPtr &scan_msg)
+{
+    std::map<int, std::pair<double, double>> xy_points_map;
+    double angle = scan_msg->angle_min;
+
+    for (size_t i = 0; i < scan_msg->ranges.size(); i++)
+    {
+        float range_val = scan_msg->ranges[i];
+
+        // 跳过无效测量值
+        if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
+        {
+            // 极坐标转笛卡尔坐标
+            double x1 = range_val * cos(angle);
+            double y1 = range_val * sin(angle);
+            double x = -y1;
+            double y = x1;
+            xy_points_map[i] = {x, y};
+        }
+        angle += scan_msg->angle_increment;
+    }
+    return xy_points_map;
+}
+
+std::map<int, std::pair<double, double>> LidarScanController::laserScanToXY1(
+    int begin_index, int end_index,
+    const LaserScanSharedPtr &scan_msg)
+{
+    std::map<int, std::pair<double, double>> xy_points_map;
+    double angle = scan_msg->angle_min;
+
+    for (int i = begin_index - 1; i <= end_index; i++)
+    {
+        float range_val = scan_msg->ranges[i];
+
+        if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
+        {
+            double x1 = range_val * cos(angle);
+            double y1 = range_val * sin(angle);
+            double x = -y1;
+            double y = x1;
+            xy_points_map[i] = {x, y};
+        }
+        angle += scan_msg->angle_increment;
+    }
+    return xy_points_map;
+}
+
+// ==================== 分段处理函数 ====================
+std::vector<std::pair<int, int>> LidarScanController::getSections(
+    int begin_index, int end_index,
+    const LaserScanSharedPtr &msg,
+    const std::map<int, std::pair<double, double>> &xy_list)
+{
+    std::vector<std::pair<int, int>> sections;
+
+    int section_start = begin_index + 1;
+    int section_end = 0;
+
+    int last_index = -1;
+    double last_x = 0.0, last_y = 0.0;
+
+    for (int i = begin_index + 1; i < end_index - 1; i++)
+    {
+        if (xy_list.find(i) == xy_list.end())
+        {
+            continue;
+        }
+
+        auto curr = xy_list.at(i);
+        if (last_index < 0)
+        {
+            last_index = i;
+            last_x = curr.first;
+            last_y = curr.second;
+            continue;
+        }
+
+        double dx = curr.first - last_x;
+        double dy = curr.second - last_y;
+        double dist = sqrt(dx * dx + dy * dy);
+
+        if (dist > 0.05 || (i - last_index) > 2)
+        {
+            section_end = i;
+            if ((section_end - section_start) >= param_.min_count)
+            {
+                sections.emplace_back(section_start, section_end);
+            }
+            section_start = i;
+        }
+
+        last_index = i;
+        last_x = curr.first;
+        last_y = curr.second;
+    }
+
+    if (last_index > section_end + 10)
+    {
+        section_end = last_index;
+        if ((section_end - section_start) > param_.min_count)
+        {
+            sections.emplace_back(section_start, section_end);
+        }
+    }
+
+    return sections;
+}
+
+std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
+LidarScanController::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>> turn_segments;
+
+    if (xy_points_map.empty())
+    {
+        return turn_segments;
+    }
+
+    // 将点转换为路径
+    std::vector<IndexPoint> path;
+    for (const auto &point : xy_points_map)
+    {
+        path.emplace_back(point.second.first, point.second.second, point.first);
+    }
+
+    // 使用相同的参数进行分段
+    auto segments = RechargeTool::splitPointsToSegments(path, 24.0, 4);
+
+    // 处理每个分段
+    for (size_t i = 0; i < segments.size(); i++)
+    {
+        if (segments[i].size() < 2)
+            continue;
+
+        // 获取分段的前两个点
+        const auto &s_prev = segments[i][0];
+        const auto &s_next = segments[i][1];
+
+        // 计算距离和角度
+        double dx = s_prev.x - s_next.x;
+        double dy = s_prev.y - s_next.y;
+        double distance = sqrt(dx * dx + dy * dy);
+        double line_angle = atan2(dy, dx);
+
+        int index_start = s_prev.index;
+        int index_end = s_next.index;
+
+        // 应用过滤条件
+        if (distance > 0.14 && (index_end - index_start) > 3)
+        {
+            turn_segments[index_start] = std::make_tuple(
+                index_start, // 0: 开始索引
+                index_end,   // 1: 结束索引
+                0.0,         // 2: 0.0
+                s_prev.x,    // 3: x坐标
+                s_prev.y,    // 4: y坐标
+                distance,    // 5: 距离
+                line_angle,  // 6: 线角度
+                1            // 7: 1
+            );
+        }
+    }
+
+    return turn_segments;
+}
+
+std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
+LidarScanController::filterSegments(
+    const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments)
+{
+    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> new_segments;
+
+    if (turn_segments.empty() || turn_segments.size() < 2)
+    {
+        return new_segments;
+    }
+
+    bool last_line_angle_valid = false;
+    double last_line_angle = 0.0;
+    std::tuple<int, int, double, double, double, double, double, int> last_seg;
+    int last_key = -1;
+
+    // 遍历所有分段
+    for (const auto &seg : turn_segments)
+    {
+        const auto &info = seg.second;
+        double line_angle = std::get<6>(info);
+
+        if (last_line_angle_valid)
+        {
+            // 计算角度差
+            double diff_angle = line_angle - last_line_angle;
+
+            // 目标角度计算
+            double target_angle = 0.5 * M_PI * (120.0 / 90.0);
+            target_angle = M_PI - target_angle;
+            double tolerance = 0.2;
+
+            // 角度条件判断
+            if (diff_angle > (target_angle - tolerance) && diff_angle < (target_angle + tolerance))
+            {
+                // 索引检查
+                int right_start_idx = std::get<0>(info);
+                int left_end_idx = std::get<1>(last_seg);
+
+                if (right_start_idx - left_end_idx < 3)
+                {
+                    // 添加到结果
+                    new_segments[last_key] = last_seg;
+                    new_segments[seg.first] = info;
+                    break; // 找到一对就跳出循环
+                }
+            }
+        }
+
+        // 更新上一个分段信息
+        last_line_angle = line_angle;
+        last_line_angle_valid = true;
+        last_seg = info;
+        last_key = seg.first;
+    }
+
+    return new_segments;
+}
+
+// ==================== 辅助函数实现 ====================
+void LidarScanController::publishRechargeVisualization(const RechargeResult &result)
+{
+    auto marker_array = std::make_shared<visualization_msgs::msg::MarkerArray>();
+
+    // 添加中线标记
+    visualization_msgs::msg::Marker line_marker;
+    line_marker.header.frame_id = "laser_frame_back1";
+    line_marker.header.stamp = node_->now();
+    line_marker.ns = "rotating_line";
+    line_marker.id = 0;
+    line_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+    line_marker.action = visualization_msgs::msg::Marker::ADD;
+    line_marker.pose.orientation.w = 1.0;
+    line_marker.scale.x = 0.02;
+    line_marker.color.r = 1.0;
+    line_marker.color.a = 1.0;
+
+    // 计算线段端点
+    double k = result.middle_line.k;
+    double b = result.middle_line.b;
+    double x1 = result.middle_point.x;
+    double x2 = x1 - 1.0;
+
+    if (x1 > 10.0 || x1 < -10.0)
+    {
+        x1 = 0.5;
+        x2 = -0.5;
+    }
+
+    geometry_msgs::msg::Point p1, p2;
+    p1.x = x1;
+    p1.y = k * x1 + b;
+    p1.z = 0.0;
+    p2.x = x2;
+    p2.y = k * x2 + b;
+    p2.z = 0.0;
+
+    line_marker.points.push_back(p1);
+    line_marker.points.push_back(p2);
+    marker_array->markers.push_back(line_marker);
+
+    // 添加中间点标记
+    visualization_msgs::msg::Marker middle_marker;
+    middle_marker.header.frame_id = "laser_frame_back1";
+    middle_marker.header.stamp = node_->now();
+    middle_marker.ns = "middle_point";
+    middle_marker.id = 1;
+    middle_marker.type = visualization_msgs::msg::Marker::SPHERE;
+    middle_marker.action = visualization_msgs::msg::Marker::ADD;
+    middle_marker.pose.orientation.w = 1.0;
+    middle_marker.scale.x = 0.03;
+    middle_marker.scale.y = 0.03;
+    middle_marker.color.b = 1.0;
+    middle_marker.color.a = 1.0;
+
+    middle_marker.pose.position.x = result.middle_point.x;
+    middle_marker.pose.position.y = result.middle_point.y;
+    middle_marker.pose.position.z = 0.0;
+
+    marker_array->markers.push_back(middle_marker);
+
+    // 发布标记数组
+    pub_rviz_->publish(*marker_array);
+}
+
+Eigen::MatrixXd LidarScanController::getPointList(const std::vector<IndexPoint> &points)
+{
+    Eigen::MatrixXd pts(points.size(), 2);
+
+    for (size_t i = 0; i < points.size(); i++)
+    {
+        pts(i, 0) = points[i].x;
+        pts(i, 1) = points[i].y;
+    }
+
+    return pts;
+}
+
+void LidarScanController::publishFilteredScan(
+    const LaserScanSharedPtr &original_msg,
+    const std::vector<float> &filtered_ranges)
+{
+    auto msg = std::make_shared<sensor_msgs::msg::LaserScan>();
+    msg->header = original_msg->header;
+    msg->angle_min = original_msg->angle_min;
+    msg->angle_max = original_msg->angle_max;
+    msg->angle_increment = original_msg->angle_increment;
+    msg->time_increment = original_msg->time_increment;
+    msg->scan_time = original_msg->scan_time;
+    msg->range_min = original_msg->range_min;
+    msg->range_max = original_msg->range_max;
+    msg->ranges = filtered_ranges;
+    msg->intensities = original_msg->intensities;
+
+    pub_filtered_->publish(*msg);
+    RCLCPP_DEBUG(node_->get_logger(), "已发布过滤后的激光数据");
+}
+
+LidarScanController::LaserScanSharedPtr LidarScanController::resetLaserData(
+    LaserScanSharedPtr msg,
+    int b_index, int e_index)
+{
+    std::vector<float> filtered_ranges = msg->ranges;
+    std::vector<float> intensities = msg->intensities;
+
+    for (int i = 0; i < b_index; i++)
+    {
+        filtered_ranges[i] = std::numeric_limits<float>::infinity();
+        intensities[i] = 0.0;
+    }
+
+    for (int i = e_index; i < static_cast<int>(msg->ranges.size()); i++)
+    {
+        filtered_ranges[i] = std::numeric_limits<float>::infinity();
+        intensities[i] = 0.0;
+    }
+
+    msg->ranges = filtered_ranges;
+    msg->intensities = intensities;
+
+    return msg;
+}
+
+bool LidarScanController::isValid(float value) const
+{
+    if (fabs(value) < 0.001)
+        return false;
+    return !std::isinf(value) && !std::isnan(value);
+}
+
+int LidarScanController::getFixedIndex(const LaserScanSharedPtr &msg, int i) const
+{
+    int fixed = getFixed();
+    if (fixed == 0)
+        return i;
+
+    int length = msg->ranges.size();
+    int index;
+
+    if (i >= length - fixed)
+    {
+        index = i - length + fixed;
+    }
+    else
+    {
+        index = i + fixed;
+    }
+    return index;
+}
+
+void LidarScanController::publishControl()
+{
+    if (active_)
+    {
+        pub_ctrl_->publish(ctrl_twist_);
+    }
+}
+
+void LidarScanController::setChargingVoltage(bool value)
+{
+    detect_charging_voltage_ = value;
+    detect_charging_voltage_time_ = node_->now();
+
+    if (value)
+    {
+        RCLCPP_INFO(node_->get_logger(), "检测到充电电压");
+    }
+    else
+    {
+        RCLCPP_INFO(node_->get_logger(), "充电电压已断开");
+    }
+}
+
+bool LidarScanController::getChargingVoltage() const
+{
+    auto now = node_->now();
+    auto diff = now - detect_charging_voltage_time_;
+
+    // 如果检测到充电电压且在0.1秒内，认为有效
+    if (detect_charging_voltage_ && diff.seconds() < 0.1)
+    {
+        return true;
+    }
+    return false;
+}

src/wheelchair_state_machine/src/main.cpp

@@ -0,0 +1,34 @@
+// main.cpp
+#include "wheelchair_state_machine/event_input.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <memory>
+
+class WheelchairStateMachineNode : public rclcpp::Node
+{
+public:
+    WheelchairStateMachineNode()
+        : Node("wheelchair_state_machine")
+    {
+        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_;
+};
+
+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

@@ -0,0 +1,197 @@
+// recharge_tool.cpp
+#include "wheelchair_state_machine/recharge_tool.hpp"
+#include <cmath>
+#include <algorithm>
+#include <numeric>
+#include <deque>
+
+double RechargeTool::angleBetween(const IndexPoint &p1, const IndexPoint &p2)
+{
+    return atan2(p2.y - p1.y, p2.x - p1.x);
+}
+
+double RechargeTool::angleDiff(double a1, double a2)
+{
+    double diff = fabs(a1 - a2);
+    return std::min(diff, 2 * M_PI - diff) * 180.0 / M_PI;
+}
+
+double RechargeTool::averageAngle(const std::vector<double> &angles)
+{
+    double sin_sum = 0.0, cos_sum = 0.0;
+    for (double a : angles)
+    {
+        sin_sum += sin(a);
+        cos_sum += cos(a);
+    }
+    return atan2(sin_sum, cos_sum);
+}
+
+std::vector<std::vector<IndexPoint>> RechargeTool::splitPointsToSegments(
+    const std::vector<IndexPoint> &points,
+    double angle_threshold_deg,
+    int history_window_size)
+{
+    std::vector<std::vector<IndexPoint>> segments;
+    if (points.size() < 2)
+    {
+        segments.push_back(points);
+        return segments;
+    }
+
+    std::vector<IndexPoint> current_segment;
+    std::deque<double> angle_history;
+
+    current_segment.push_back(points[0]);
+
+    for (size_t i = 1; i < points.size(); i++)
+    {
+        const IndexPoint &p_prev = points[i - 1];
+        const IndexPoint &p_curr = points[i];
+        double curr_angle = angleBetween(p_prev, p_curr);
+
+        // 更新方向历史
+        angle_history.push_back(curr_angle);
+        if (angle_history.size() > history_window_size)
+        {
+            angle_history.pop_front();
+        }
+
+        // 若历史方向足够，开始判断角度差
+        if (angle_history.size() >= history_window_size)
+        {
+            // 计算历史方向平均值（排除当前角度）
+            std::vector<double> hist(angle_history.begin(), angle_history.end() - 1);
+            double avg_angle = averageAngle(hist);
+            double diff = angleDiff(avg_angle, curr_angle);
+
+            if (diff > angle_threshold_deg)
+            {
+                // 发生显著拐弯，分段
+                current_segment.push_back(p_prev);
+                segments.push_back(current_segment);
+
+                // 开始新分段，以当前点为起点
+                current_segment.clear();
+                current_segment.push_back(p_curr);
+
+                // 重置方向历史，仅保留当前角度
+                angle_history.clear();
+                angle_history.push_back(curr_angle);
+            }
+        }
+    }
+
+    current_segment.push_back(points.back());
+    segments.push_back(current_segment);
+
+    return segments;
+}
+
+std::pair<double, double> RechargeTool::calculateAngleBisector(double k1, double b1, double k2, double b2)
+{
+    // 计算两条直线的夹角
+    double angle1 = atan(k1);
+    double angle2 = atan(k2);
+
+    // 计算两条直线之间的夹角
+    double delta_angle = fabs(angle1 - angle2);
+
+    // 计算中线角度
+    double bisector_angle1;
+    if (angle1 < angle2)
+    {
+        bisector_angle1 = angle1 + delta_angle / 2;
+    }
+    else
+    {
+        bisector_angle1 = angle2 + delta_angle / 2;
+    }
+
+    // 处理平行线（无交点）的情况
+    if (fabs(k1 - k2) < 1e-10)
+    {
+        // 平行线没有交点，返回原直线
+        return {k1, b1};
+    }
+
+    // 计算交点坐标
+    double x_intersect = (b2 - b1) / (k1 - k2);
+    double y_intersect = k1 * x_intersect + b1;
+
+    // 计算平分线的斜率
+    double k3 = tan(bisector_angle1);
+    double b3 = y_intersect - k3 * x_intersect;
+
+    return {k3, b3};
+}
+
+std::pair<double, double> RechargeTool::fitLineLeastSquares(const Eigen::MatrixXd &points)
+{
+    int n = points.rows();
+    Eigen::VectorXd x = points.col(0);
+    Eigen::VectorXd y = points.col(1);
+
+    // 最小二乘法拟合 y = kx + b
+    double x_mean = x.mean();
+    double y_mean = y.mean();
+
+    double numerator = 0.0;
+    double denominator = 0.0;
+
+    for (int i = 0; i < n; i++)
+    {
+        numerator += (x(i) - x_mean) * (y(i) - y_mean);
+        denominator += (x(i) - x_mean) * (x(i) - x_mean);
+    }
+
+    double k = numerator / denominator;
+    double b = y_mean - k * x_mean;
+
+    return {k, b};
+}
+
+std::pair<double, double> RechargeTool::computeDockingVelocity(double dx, double dy, double yaw_error)
+{
+    double max_linear_speed = 0.12;
+    double max_angular_speed = 0.16;
+
+    double Kp_linear = 0.5;
+    double Kp_angular = 0.5;
+    double Kp_lateral = 0.6;
+
+    // 线速度控制
+    double linear_velocity = Kp_linear * std::max(0.1, dx - 0.6);
+    linear_velocity = std::clamp(linear_velocity, -max_linear_speed, max_linear_speed);
+    linear_velocity = std::max(0.08, linear_velocity);
+
+    // 角速度控制
+    double angular_velocity = Kp_angular * yaw_error + Kp_lateral * dy;
+    angular_velocity = std::clamp(angular_velocity, -max_angular_speed, max_angular_speed);
+
+    return {-linear_velocity, -angular_velocity};
+}
+
+std::pair<double, double> RechargeTool::simpleFitLine(const std::vector<IndexPoint> &points)
+{
+    if (points.size() < 2)
+    {
+        return {0.0, 0.0};
+    }
+
+    double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0;
+    int n = points.size();
+
+    for (const auto &p : points)
+    {
+        sum_x += p.x;
+        sum_y += p.y;
+        sum_xy += p.x * p.y;
+        sum_xx += p.x * p.x;
+    }
+
+    double k = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x);
+    double b = (sum_y - k * sum_x) / n;
+
+    return {k, b};
+}

src/wheelchair_state_machine/src/report.cpp

@@ -0,0 +1,552 @@
+// report.cpp
+#include "wheelchair_state_machine/report.hpp"
+#include <rclcpp/logging.hpp>
+#include <sstream>
+#include <iomanip>
+using namespace std::chrono_literals;
+
+std::map<uint32_t, std::string> ReportManager::error_descriptions_;
+std::map<ErrorModule, std::string> ReportManager::module_names_;
+std::map<ErrorSubModule, std::string> ReportManager::sub_module_names_;
+
+ReportManager::ReportManager(rclcpp::Node *node)
+    : node_(node)
+{
+    // 创建报告发布者
+    report_publisher_ = node_->create_publisher<std_msgs::msg::String>(
+        "wheelchair/report", 10);
+
+    // 初始化错误映射
+    initializeErrorMaps();
+    RCLCPP_INFO(node_->get_logger(), "报告管理器已初始化");
+}
+
+std::string ReportManager::generateSystemStatusReport(
+    WheelchairState wheelchair_state,
+    BatteryState battery_state,
+    float battery_percentage,
+    ChargingState charging_state,
+    bool charging_voltage_detected,
+    bool recharge_active,
+    const std::string &control_mode)
+{
+    std::stringstream report;
+
+    report << "=== 轮椅系统状态报告 ===\n";
+    report << "时间: " << node_->now().seconds() << "s\n\n";
+
+    // 轮椅状态
+    report << "1. 轮椅状态:\n";
+    report << "   - 当前状态: " << wheelchairStateToString(wheelchair_state) << "\n";
+    report << "   - 控制模式: " << control_mode << "\n";
+    report << "   - 回充状态: " << (recharge_active ? "激活" : "未激活") << "\n\n";
+
+    // 电池状态
+    report << "2. 电池状态:\n";
+    report << "   - 电量: " << battery_percentage << "%\n";
+    report << "   - 状态: " << batteryStateToString(battery_state) << "\n\n";
+
+    // 充电状态
+    report << "3. 充电状态:\n";
+    report << "   - 充电状态: " << chargingStateToString(charging_state) << "\n";
+    report << "   - 充电电压: " << (charging_voltage_detected ? "检测到" : "未检测") << "\n\n";
+
+    // 错误状态
+    report << "4. 错误状态:\n";
+    std::vector<ErrorInfo> active_errors = getActiveErrors();
+    if (active_errors.empty())
+    {
+        report << "   - 当前错误: 无\n";
+    }
+    else
+    {
+        report << "   - 当前错误: " << active_errors.size() << "个\n";
+        for (const auto &error : active_errors)
+        {
+            report << "     * " << error.module_name << ": " << error.description << "\n";
+        }
+    }
+
+    // 系统状态
+    report << "4. 系统状态:\n";
+    report << "   - 运行时间: " << "N/A" << "\n";
+    report << "   - 安全状态: 正常\n";
+    report << "   - 错误代码: 无\n";
+
+    return report.str();
+}
+
+void ReportManager::publishStatusReport(const std::string &report)
+{
+    auto msg = std_msgs::msg::String();
+    msg.data = report;
+    report_publisher_->publish(msg);
+
+    // 调用回调函数
+    if (status_report_callback_)
+    {
+        status_report_callback_(report);
+    }
+}
+
+void ReportManager::reportSystemStatus(
+    WheelchairState wheelchair_state,
+    BatteryState battery_state,
+    float battery_percentage,
+    ChargingState charging_state,
+    bool charging_voltage_detected,
+    bool recharge_active,
+    const std::string &control_mode)
+{
+    std::string report = generateSystemStatusReport(
+        wheelchair_state, battery_state, battery_percentage,
+        charging_state, charging_voltage_detected,
+        recharge_active, control_mode);
+
+    publishStatusReport(report);
+    logReport("SYSTEM_STATUS", "系统状态报告已生成");
+}
+
+void ReportManager::reportBatteryStatus(BatteryState state, float percentage)
+{
+    std::stringstream report;
+    report << "电池状态报告:\n";
+    report << "- 电量: " << percentage << "%\n";
+    report << "- 状态: " << batteryStateToString(state) << "\n";
+    report << "- 时间: " << node_->now().seconds() << "s";
+
+    publishStatusReport(report.str());
+    logReport("BATTERY_STATUS", "电池状态报告已生成");
+}
+
+void ReportManager::reportChargingStatus(ChargingState state)
+{
+    std::stringstream report;
+    report << "充电状态报告:\n";
+    report << "- 状态: " << chargingStateToString(state) << "\n";
+    report << "- 时间: " << node_->now().seconds() << "s";
+
+    publishStatusReport(report.str());
+    logReport("CHARGING_STATUS", "充电状态报告已生成");
+}
+
+void ReportManager::reportNavigationStatus(const std::string &status)
+{
+    std::stringstream report;
+    report << "导航状态报告:\n";
+    report << "- 状态: " << status << "\n";
+    report << "- 时间: " << node_->now().seconds() << "s";
+
+    publishStatusReport(report.str());
+    logReport("NAVIGATION_STATUS", "导航状态报告已生成");
+}
+
+void ReportManager::reportError(const std::string &error_message)
+{
+    std::stringstream report;
+    report << "错误报告:\n";
+    report << "- 错误信息: " << error_message << "\n";
+    report << "- 时间: " << node_->now().seconds() << "s";
+
+    publishStatusReport(report.str());
+    logReport("ERROR", error_message);
+}
+
+void ReportManager::reportWarning(const std::string &warning_message)
+{
+    std::stringstream report;
+    report << "警告报告:\n";
+    report << "- 警告信息: " << warning_message << "\n";
+    report << "- 时间: " << node_->now().seconds() << "s";
+
+    publishStatusReport(report.str());
+    logReport("WARNING", warning_message);
+}
+
+void ReportManager::reportInfo(const std::string &info_message)
+{
+    std::stringstream report;
+    report << "信息报告:\n";
+    report << "- 信息: " << info_message << "\n";
+    report << "- 时间: " << node_->now().seconds() << "s";
+
+    publishStatusReport(report.str());
+    logReport("INFO", info_message);
+}
+
+// ==================== 错误管理功能 ====================
+
+void ReportManager::errorCodeCallback(const std_msgs::msg::UInt32::SharedPtr msg)
+{
+    uint32_t error_code = msg->data;
+    RCLCPP_INFO(node_->get_logger(), "收到错误码: 0x%08X", error_code);
+    processErrorCode(error_code);
+}
+
+ErrorInfo ReportManager::parseErrorCode(uint32_t error_code)
+{
+    ErrorInfo info;
+    info.error_code = error_code;
+    info.timestamp = node_->now().nanoseconds() / 1000000; // 转换为毫秒
+    info.is_recovered = false;
+
+    // 解析模块和子模块名称
+    ErrorModule module = info.getModule();
+    ErrorSubModule sub_module = info.getSubModule();
+
+    if (module_names_.find(module) != module_names_.end())
+    {
+        info.module_name = module_names_[module];
+    }
+    else
+    {
+        info.module_name = "未知模块";
+    }
+
+    if (sub_module_names_.find(sub_module) != sub_module_names_.end())
+    {
+        info.sub_module_name = sub_module_names_[sub_module];
+    }
+    else
+    {
+        info.sub_module_name = "未知子模块";
+    }
+
+    // 获取错误描述
+    if (error_descriptions_.find(error_code) != error_descriptions_.end())
+    {
+        info.description = error_descriptions_[error_code];
+    }
+    else
+    {
+        info.description = getErrorDescription(error_code);
+    }
+
+    return info;
+}
+
+void ReportManager::processErrorCode(uint32_t error_code)
+{
+    // 仅记录到活跃错误列表用于报告
+    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_);
+    active_errors_[error_info.error_code] = error_info;
+}
+
+void ReportManager::handleNewError(const ErrorInfo &error_info)
+{
+    // 添加到活跃错误列表
+    active_errors_[error_info.error_code] = error_info;
+
+    // 生成报告
+    std::stringstream report;
+    report << "错误报告 - " << error_info.module_name << " - "
+           << 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 << "时间: " << std::dec << error_info.timestamp << "ms\n";
+    report << "严重程度: " << (error_info.isCritical() ? "严重" : "一般");
+
+    if (error_info.isCritical())
+    {
+        reportError(report.str());
+        RCLCPP_ERROR(node_->get_logger(), "检测到严重错误: %s - %s",
+                     error_info.module_name.c_str(),
+                     error_info.description.c_str());
+    }
+    else
+    {
+        reportWarning(report.str());
+        RCLCPP_WARN(node_->get_logger(), "检测到新错误: %s - %s",
+                    error_info.module_name.c_str(),
+                    error_info.description.c_str());
+    }
+
+    // 调用回调函数
+    if (error_callback_)
+    {
+        error_callback_(error_info);
+    }
+
+    if (error_info.isCritical() && critical_error_callback_)
+    {
+        critical_error_callback_(error_info);
+    }
+}
+
+void ReportManager::notifyErrorRecovered(uint32_t error_code)
+{
+    std::lock_guard<std::mutex> lock(error_mutex_);
+
+    if (active_errors_.find(error_code) != active_errors_.end())
+    {
+        ErrorInfo &error_info = active_errors_[error_code];
+        error_info.is_recovered = true;
+
+        // 生成恢复报告
+        std::stringstream report;
+        report << "错误恢复 - " << error_info.module_name << "\n";
+        report << "错误码: 0x" << std::hex << error_code << "\n";
+        report << "描述: " << error_info.description << "\n";
+        report << "恢复时间: " << std::dec << node_->now().nanoseconds() / 1000000 << "ms";
+
+        reportInfo(report.str());
+
+        // 调用恢复回调
+        if (recovery_callback_)
+        {
+            recovery_callback_(error_info);
+        }
+
+        RCLCPP_INFO(node_->get_logger(), "错误码 0x%08X 已恢复", error_code);
+
+        // 从活跃错误列表中移除
+        active_errors_.erase(error_code);
+    }
+}
+
+// 查询接口
+bool ReportManager::hasCriticalErrors() const
+{
+    std::lock_guard<std::mutex> lock(error_mutex_);
+
+    for (const auto &pair : active_errors_)
+    {
+        if (pair.second.isCritical() && !pair.second.is_recovered)
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+bool ReportManager::hasError(uint32_t error_code) const
+{
+    std::lock_guard<std::mutex> lock(error_mutex_);
+    return active_errors_.find(error_code) != active_errors_.end();
+}
+
+std::vector<ErrorInfo> ReportManager::getActiveErrors() const
+{
+    std::lock_guard<std::mutex> lock(error_mutex_);
+
+    std::vector<ErrorInfo> errors;
+    for (const auto &pair : active_errors_)
+    {
+        if (!pair.second.is_recovered)
+        {
+            errors.push_back(pair.second);
+        }
+    }
+    return errors;
+}
+
+std::vector<ErrorInfo> ReportManager::getCriticalErrors() const
+{
+    std::lock_guard<std::mutex> lock(error_mutex_);
+
+    std::vector<ErrorInfo> critical_errors;
+    for (const auto &pair : active_errors_)
+    {
+        if (pair.second.isCritical() && !pair.second.is_recovered)
+        {
+            critical_errors.push_back(pair.second);
+        }
+    }
+    return critical_errors;
+}
+
+// 回调设置
+void ReportManager::setStatusReportCallback(StatusReportCallback callback)
+{
+    status_report_callback_ = callback;
+}
+
+void ReportManager::setErrorCallback(ErrorCallback callback)
+{
+    error_callback_ = callback;
+}
+
+void ReportManager::setCriticalErrorCallback(ErrorCallback callback)
+{
+    critical_error_callback_ = callback;
+}
+
+void ReportManager::setRecoveryCallback(ErrorCallback callback)
+{
+    recovery_callback_ = callback;
+}
+
+// 错误描述获取
+std::string ReportManager::getErrorDescription(uint32_t error_code)
+{
+    ErrorModule module = static_cast<ErrorModule>((error_code >> 24) & 0xFF);
+    ErrorSubModule sub_module = static_cast<ErrorSubModule>((error_code >> 16) & 0xFF);
+    uint16_t error_num = error_code & 0xFFFF;
+
+    std::stringstream description;
+    description << getModuleName(module) << " - "
+                << getSubModuleName(sub_module) << " - 错误编号: 0x"
+                << std::hex << error_num;
+
+    return description.str();
+}
+
+std::string ReportManager::getModuleName(ErrorModule module)
+{
+    if (module_names_.find(module) != module_names_.end())
+    {
+        return module_names_[module];
+    }
+    return "未知模块";
+}
+
+std::string ReportManager::getSubModuleName(ErrorSubModule sub_module)
+{
+    if (sub_module_names_.find(sub_module) != sub_module_names_.end())
+    {
+        return sub_module_names_[sub_module];
+    }
+    return "未知子模块";
+}
+
+// 初始化静态映射
+void ReportManager::initializeErrorMaps()
+{
+    // 模块名称映射
+    module_names_[ErrorModule::SYSTEM] = "系统";
+    module_names_[ErrorModule::IMU] = "IMU";
+    module_names_[ErrorModule::GPS] = "GPS";
+    module_names_[ErrorModule::LIDAR1] = "左雷达";
+    module_names_[ErrorModule::LIDAR2] = "右雷达";
+    module_names_[ErrorModule::LIDAR3] = "左斜雷达";
+    module_names_[ErrorModule::LIDAR4] = "右斜雷达";
+    module_names_[ErrorModule::LIDAR5] = "后雷达";
+    module_names_[ErrorModule::CAN0] = "CAN0总线";
+    module_names_[ErrorModule::CAN1] = "CAN1总线";
+    module_names_[ErrorModule::CAN2] = "CAN2总线";
+    module_names_[ErrorModule::DRIVER] = "电机驱动器";
+    module_names_[ErrorModule::TOP] = "TOP驱动器";
+    module_names_[ErrorModule::OBSTACLE] = "避障功能";
+    module_names_[ErrorModule::RECHARGE] = "回充功能";
+
+    // 子模块名称映射
+    sub_module_names_[ErrorSubModule::HARDWARE] = "硬件";
+    sub_module_names_[ErrorSubModule::DATA] = "数据";
+    sub_module_names_[ErrorSubModule::TIMING] = "时间同步";
+    sub_module_names_[ErrorSubModule::CONFIG] = "配置";
+    sub_module_names_[ErrorSubModule::COMMUNICATION] = "通信";
+    sub_module_names_[ErrorSubModule::CALIBRATION] = "标定";
+    sub_module_names_[ErrorSubModule::PERFORMANCE] = "性能";
+    sub_module_names_[ErrorSubModule::POWER] = "电源";
+    sub_module_names_[ErrorSubModule::TEMPERATURE] = "温度";
+    sub_module_names_[ErrorSubModule::OTHER] = "其他";
+
+    // 根据文档初始化具体的错误描述
+    // 系统错误 (0x01)
+    error_descriptions_[0x01010101] = "系统硬件故障";
+    error_descriptions_[0x01020101] = "系统数据无效";
+    error_descriptions_[0x01030101] = "系统时间同步失败";
+
+    // IMU错误 (0x10)
+    error_descriptions_[0x10010101] = "IMU硬件故障";
+    error_descriptions_[0x10020102] = "IMU数据无效";
+    error_descriptions_[0x10030103] = "IMU数据超时";
+
+    // 雷达错误示例
+    error_descriptions_[0x12010101] = "左雷达硬件故障";
+    error_descriptions_[0x12020101] = "左雷达数据无效";
+    error_descriptions_[0x13010101] = "右雷达硬件故障";
+    error_descriptions_[0x16010101] = "后雷达硬件故障";
+
+    // CAN总线错误
+    error_descriptions_[0x02010101] = "CAN0硬件故障";
+    error_descriptions_[0x02020105] = "CAN0通信失败";
+    error_descriptions_[0x03010101] = "CAN1硬件故障";
+
+    // 电机驱动器错误
+    error_descriptions_[0x20010108] = "电机驱动器电源异常";
+    error_descriptions_[0x20010109] = "电机驱动器温度过高";
+
+    // 避障功能错误
+    error_descriptions_[0x50010101] = "避障功能硬件故障";
+    error_descriptions_[0x50020107] = "避障功能性能低下";
+
+    // 回充功能错误（根据文档）
+    error_descriptions_[0x51010101] = "回充检测失败（超时）";
+    error_descriptions_[0x51010102] = "回充对接失败";
+    error_descriptions_[0x51010103] = "回充电压异常";
+    error_descriptions_[0x51010104] = "回充过程中断";
+    error_descriptions_[0x51010105] = "回充电流异常";
+}
+
+// 辅助函数
+std::string ReportManager::wheelchairStateToString(WheelchairState state) const
+{
+    switch (state)
+    {
+    case WheelchairState::READY:
+        return "就绪中";
+    case WheelchairState::WALKING:
+        return "行走中";
+    case WheelchairState::ROTATING:
+        return "旋转中";
+    case WheelchairState::SEARCHING:
+        return "搜索中";
+    case WheelchairState::CHARGING:
+        return "充电中";
+    default:
+        return "未知状态";
+    }
+}
+
+std::string ReportManager::batteryStateToString(BatteryState state) const
+{
+    switch (state)
+    {
+    case BatteryState::NORMAL:
+        return "正常";
+    case BatteryState::LOW:
+        return "低电量";
+    case BatteryState::CRITICAL:
+        return "严重低电量";
+    case BatteryState::CHARGING:
+        return "充电中";
+    case BatteryState::FULL:
+        return "充满";
+    default:
+        return "未知";
+    }
+}
+
+std::string ReportManager::chargingStateToString(ChargingState state) const
+{
+    switch (state)
+    {
+    case ChargingState::IDLE:
+        return "空闲";
+    case ChargingState::NAVIGATING:
+        return "导航中";
+    case ChargingState::CHARGING:
+        return "充电中";
+    case ChargingState::UNAVAILABLE:
+        return "不可用";
+    case ChargingState::COMPLETED:
+        return "完成";
+    default:
+        return "未知";
+    }
+}
+
+void ReportManager::logReport(const std::string &category, const std::string &message)
+{
+    RCLCPP_INFO(node_->get_logger(), "[%s] %s", category.c_str(), message.c_str());
+}

src/wheelchair_state_machine/src/rotation_manager.cpp

@@ -0,0 +1,148 @@
+// 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.15) // 角速度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());
+}

src/wheelchair_state_machine/src/workflow.cpp

@@ -0,0 +1,1539 @@
+// workflow.cpp
+#include "wheelchair_state_machine/workflow.hpp"
+#include <rclcpp/logging.hpp>
+#include <chrono>
+#include <stdexcept>
+#include <sstream>
+
+using namespace std::chrono_literals;
+
+WorkflowManager::WorkflowManager(rclcpp::Node *node)
+    : node_(node), current_state_(WheelchairState::READY)
+{
+    // 初始化状态转移表
+    initializeTransitionTable();
+
+    // 初始化旋转管理器
+    initializeRotationManager();
+
+    initializeSubscriptions(); // 新增
+
+    // 创建状态发布者
+    state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
+        "wheelchair/state", 10);
+
+    // 创建回充超时报告发布者
+    recharge_timeout_publisher_ = node_->create_publisher<std_msgs::msg::String>(
+        "wheelchair/recharge_timeout", 10);
+
+    RCLCPP_INFO(node_->get_logger(), "工作流管理器已初始化");
+}
+void WorkflowManager::initializeSubscriptions()
+{
+    // 创建基站检测订阅者
+    station_detected_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
+        "station/detected", 10,
+        [this](const std_msgs::msg::Bool::SharedPtr msg)
+        {
+            this->stationDetectedCallback(msg);
+        });
+
+    RCLCPP_INFO(node_->get_logger(), "基站检测订阅者已初始化");
+}
+
+void WorkflowManager::stationDetectedCallback(const std_msgs::msg::Bool::SharedPtr msg)
+{
+    try
+    {
+        bool station_detected = msg->data;
+        std::string current_state_str = getCurrentState();
+
+        RCLCPP_DEBUG(node_->get_logger(),
+                     "收到基站检测信号: %s, 当前状态: %s",
+                     station_detected ? "true" : "false",
+                     current_state_str.c_str());
+
+        // 只有在搜索或旋转状态下，且检测到基站时，才考虑状态转移
+        if (station_detected &&
+            (current_state_ == WheelchairState::SEARCHING ||
+             current_state_ == WheelchairState::ROTATING))
+        {
+            RCLCPP_INFO(node_->get_logger(),
+                        "搜索中检测到基站，切换到行走状态进行对接");
+
+            // 从搜索/旋转状态切换到行走状态
+            transitionToWalking();
+
+            // 发布状态更新
+            publishState("STATION_DETECTED", "检测到基站，开始移动对接");
+
+            // 发布回充状态
+            publishRechargeStatus("STATION_DETECTED",
+                                  "检测到充电基站，开始导航对接");
+        }
+        else if (!station_detected)
+        {
+            // 基站丢失处理（如果之前正在行走对接，但现在基站丢失）
+            if (current_state_ == WheelchairState::WALKING)
+            {
+                // 可以根据需要处理基站丢失的情况
+                RCLCPP_DEBUG(node_->get_logger(), "行走对接过程中基站信号丢失");
+            }
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理基站检测回调", e);
+    }
+}
+
+void WorkflowManager::initializeRotationManager()
+{
+    rotation_manager_ = std::make_unique<RotationManager>(node_);
+    RCLCPP_INFO(node_->get_logger(), "旋转管理器已初始化");
+}
+
+void WorkflowManager::initializeTransitionTable()
+{
+    transition_table_.clear();
+
+    // 1. ipad&phone界面启动: 就绪中✅, 其他❌
+    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;
+
+    // 2. iPad&phone界面取消: 所有状态✅
+    std::map<WheelchairState, bool> ipad_cancel_perms = {
+        {WheelchairState::READY, true},
+        {WheelchairState::WALKING, true},
+        {WheelchairState::ROTATING, true}, // 旋转中❌
+        {WheelchairState::SEARCHING, true},
+        {WheelchairState::CHARGING, true}};
+    transition_table_["ipad_phone_interface_cancel"] = ipad_cancel_perms;
+
+    // 3. 蓝牙断开: 就绪中❌, 其他✅
+    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;
+
+    // 4. 蓝牙已连接: 就绪中✅, 其他❌
+    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;
+
+    // 5. 基站断电: 就绪中❌, 其他✅
+    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;
+
+    // 6. 低电量警告: 就绪中✅, 其他❌
+    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;
+
+    // 7. 锁车: 就绪中✅, 其他❌
+    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;
+
+    // 8. 解锁: 行走中✅, 其他❌
+    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;
+
+    // 9. 摇杆后拉: 搜索中✅, 其他❌
+    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;
+
+    // 10. 摇杆停止: 行走中✅, 搜索中✅, 其他❌
+    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;
+
+    // 11. 推行启动: 充电中✅, 其他❌
+    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;
+
+    // 12. 推行关闭: 行走中✅, 搜索中✅, 其他❌
+    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;
+
+    // 13. 电池-开始充电: 搜索中✅, 充电中✅, 其他❌
+    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;
+
+    // 14. 电池-断开充电: 充电中✅, 其他❌
+    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;
+
+    // 15. 电池-电量满: 充电中✅, 其他❌
+    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;
+
+    // 16. 错误码处理: 所有状态✅
+    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;
+
+    // 17. 基站检测丢失: 行走中✅, 其他❌
+    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;
+
+    // 18. 搜索30s超时: 搜索中✅, 其他❌
+    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;
+
+    std::map<WheelchairState, bool> station_detected_perms = {
+        {WheelchairState::READY, false},
+        {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, true},  // 旋转中✅
+        {WheelchairState::SEARCHING, true}, // 搜索中✅
+        {WheelchairState::CHARGING, false}};
+    transition_table_["station_detected"] = station_detected_perms;
+
+    RCLCPP_INFO(node_->get_logger(), "状态转移表已初始化完成");
+}
+
+bool WorkflowManager::handleEvent(const std::string &event)
+{
+    try
+    {
+        if (!checkEventPermission(event))
+        {
+            RCLCPP_WARN(node_->get_logger(),
+                        "事件 '%s' 在当前状态 '%s' 下不允许执行",
+                        event.c_str(),
+                        getCurrentState().c_str());
+            return false;
+        }
+
+        executeEvent(event);
+        return true;
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理事件 " + event, e);
+        return false;
+    }
+}
+
+bool WorkflowManager::checkEventPermission(const std::string &event)
+{
+    if (transition_table_.find(event) == transition_table_.end())
+    {
+        RCLCPP_WARN(node_->get_logger(), "未知事件: %s", event.c_str());
+        return false;
+    }
+
+    std::map<WheelchairState, bool> state_permissions = transition_table_[event];
+
+    if (state_permissions.find(current_state_) != state_permissions.end())
+    {
+        return state_permissions[current_state_];
+    }
+
+    return false;
+}
+
+void WorkflowManager::executeEvent(const std::string &event)
+{
+    RCLCPP_INFO(node_->get_logger(), "执行事件: %s", event.c_str());
+
+    // 根据事件类型执行相应处理
+    if (event == "ipad_phone_interface_start")
+    {
+        processIpadStartEvent();
+    }
+    else if (event == "ipad_phone_interface_cancel")
+    {
+        processIpadCancelEvent();
+    }
+    else if (event == "bluetooth_disconnected")
+    {
+        processBluetoothDisconnected();
+    }
+    else if (event == "bluetooth_connected")
+    {
+        processBluetoothConnected();
+    }
+    else if (event == "base_station_power_off")
+    {
+        processBaseStationPowerOff();
+    }
+    else if (event == "low_battery_warning")
+    {
+        handleLowBatteryEvent(BatteryState::LOW, 0.0f);
+    }
+    else if (event == "lock_vehicle")
+    {
+        processLockVehicle();
+    }
+    else if (event == "unlock_vehicle")
+    {
+        processUnlockVehicle();
+    }
+    else if (event == "joystick_pull_back")
+    {
+        processJoystickPullBack();
+    }
+    else if (event == "joystick_stop")
+    {
+        processJoystickStop();
+    }
+    else if (event == "push_start")
+    {
+        processPushStart();
+    }
+    else if (event == "push_stop")
+    {
+        processPushStop();
+    }
+    else if (event == "battery_start_charging")
+    {
+        handleChargingStartEvent();
+    }
+    else if (event == "battery_stop_charging")
+    {
+        handleChargingStopEvent();
+    }
+    else if (event == "battery_full")
+    {
+        handleChargingFullEvent();
+    }
+    else if (event == "base_station_lost")
+    {
+        handleBaseStationLostEvent();
+    }
+    else if (event == "search_30s_timeout")
+    {
+        handleSearchTimeoutEvent();
+    }
+    else if (event == "station_detected")
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理基站检测成功事件");
+        // 实际上这个事件是通过回调处理的，这里只做日志记录
+    }
+    else
+    {
+        RCLCPP_WARN(node_->get_logger(), "未实现的事件处理: %s", event.c_str());
+    }
+
+    // 根据事件触发状态转移
+    processStateTransition(event);
+
+    // 发布状态更新
+    publishState(event, "事件已处理");
+}
+
+void WorkflowManager::processStateTransition(const std::string &event)
+{
+    try
+    {
+        // 根据事件检查是否需要状态转移
+        switch (current_state_)
+        {
+        case WheelchairState::READY:
+            if (event == "low_battery_warning" || event == "ipad_phone_interface_start")
+            {
+                transitionToSearching();
+            }
+            else if (event == "unlock_vehicle")
+            {
+                transitionToWalking();
+            }
+            break;
+
+        case WheelchairState::WALKING:
+            if (event == "joystick_stop" || event == "push_stop" || event == "lock_vehicle")
+            {
+                transitionToReady();
+            }
+            else if (event == "base_station_lost")
+            {
+                transitionToSearching();
+            }
+            else if (event == "battery_start_charging")
+            {
+                transitionToCharging();
+            }
+            break;
+        case WheelchairState::ROTATING:
+            // 旋转中状态下，所有事件默认不处理，可以添加特定逻辑
+            if (event == "station_detected")
+            {
+                RCLCPP_INFO(node_->get_logger(), "旋转中检测到基站，转为行走对接");
+                transitionToWalking();
+            }
+            break;
+        case WheelchairState::SEARCHING:
+            if (event == "base_station_lost")
+            {
+                transitionToReady();
+            }
+            else if (event == "battery_start_charging")
+            {
+                transitionToCharging();
+            }
+            else if (event == "joystick_pull_back")
+            {
+                transitionToWalking();
+            }
+            else if (event == "station_detected")
+            {
+                RCLCPP_INFO(node_->get_logger(), "搜索中检测到基站，转为行走对接");
+                transitionToWalking();
+            }
+            // 搜索超时事件
+            else if (event == "search_30s_timeout")
+            {
+                RCLCPP_WARN(node_->get_logger(), "搜索30秒超时，回充失败");
+                // 触发回充失败处理
+                handleRechargeFailure("搜索30秒超时");
+            }
+            break;
+
+        case WheelchairState::CHARGING:
+            if (event == "battery_full" ||
+                event == "battery_stop_charging" ||
+                event == "base_station_power_off")
+            {
+                transitionToReady();
+            }
+            else if (event == "push_start")
+            {
+                transitionToWalking();
+            }
+            break;
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理状态转移", e);
+    }
+}
+
+// ==================== 旋转管理接口 ====================
+
+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();
+
+            // 关键修复：进入旋转状态时，停止搜索超时定时器
+            stopSearchTimeoutTimer();
+
+            RCLCPP_INFO(node_->get_logger(), "已停止搜索超时定时器，进入旋转搜索模式");
+        }
+        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::SEARCHING ||
+            current_state_ == WheelchairState::ROTATING)
+        {
+            setState(WheelchairState::WALKING);
+            // 停止所有定时器
+            stopSearchTimeoutTimer();
+            stopRotationTimer();
+            RCLCPP_INFO(node_->get_logger(), "轮椅开始行走");
+            publishState("WHEELCHAIR_WALKING");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("转换到行走状态", e);
+    }
+}
+
+void WorkflowManager::transitionToSearching()
+{
+    try
+    {
+        if (current_state_ == WheelchairState::READY ||
+            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", "回充功能已启动，正在搜索充电站");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("转换到搜索状态", e);
+    }
+}
+
+void WorkflowManager::transitionToCharging()
+{
+    try
+    {
+        if (current_state_ == WheelchairState::SEARCHING ||
+            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");
+
+            // 发布回充成功通知
+            publishRechargeStatus("RECHARGE_SUCCESS", "成功对接充电站，开始充电");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("转换到充电状态", e);
+    }
+}
+
+void WorkflowManager::transitionToReady()
+{
+    try
+    {
+        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");
+    }
+    catch (const std::exception &e)
+    {
+        emergencyTransitionToReady("正常转换失败: " + std::string(e.what()));
+    }
+}
+
+// ==================== 事件处理函数实现 ====================
+
+void WorkflowManager::processIpadStartEvent()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理iPad启动事件");
+
+        // 发布状态更新
+        publishState("IPAD_RECHARGE_STARTED", "iPad启动自主回充");
+
+        // 如果当前是就绪状态，自动转移到搜索状态
+        if (current_state_ == WheelchairState::READY)
+        {
+            transitionToSearching();
+        }
+        else
+        {
+            RCLCPP_WARN(node_->get_logger(),
+                        "当前状态 %s 无法启动回充", getCurrentState().c_str());
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理iPad启动事件", e);
+    }
+}
+
+void WorkflowManager::processIpadCancelEvent()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理iPad取消事件");
+
+        // 发布状态更新
+        publishState("IPAD_RECHARGE_CANCELLED", "iPad取消自主回充");
+
+        // 根据当前状态决定是否取消回充
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::WALKING || current_state_ == WheelchairState::ROTATING)
+        {
+            transitionToReady();
+            RCLCPP_INFO(node_->get_logger(), "iPad自主回充已取消，返回到就绪状态");
+
+            // 发布回充取消通知
+            publishRechargeStatus("RECHARGE_CANCELLED", "用户取消回充");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理iPad取消事件", e);
+    }
+}
+
+void WorkflowManager::processBluetoothDisconnected()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理蓝牙断开事件");
+
+        // 发布状态更新
+        publishState("BLUETOOTH_DISCONNECTED", "蓝牙连接已断开");
+
+        // 如果正在回充或充电，退出流程
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_WARN(node_->get_logger(), "蓝牙断开，退出回充流程");
+            handleRechargeFailure("蓝牙断开导致回充失败");
+            transitionToReady();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理蓝牙断开事件", e);
+    }
+}
+
+void WorkflowManager::processBluetoothConnected()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理蓝牙连接成功事件");
+
+        // 发布状态更新
+        publishState("BLUETOOTH_CONNECTED", "蓝牙连接成功");
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理蓝牙连接事件", e);
+    }
+}
+
+void WorkflowManager::processBaseStationPowerOff()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理基站断电事件");
+
+        // 发布状态更新
+        publishState("BASE_STATION_POWER_OFF", "充电基站断电");
+
+        // 如果正在回充或充电，退出流程
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_WARN(node_->get_logger(), "基站断电，退出回充流程");
+            handleRechargeFailure("基站断电导致回充失败");
+            transitionToReady();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理基站断电事件", e);
+    }
+}
+
+void WorkflowManager::handleLowBatteryEvent(BatteryState state, float percentage)
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理低电量事件");
+
+        // 根据电池状态采取行动
+        if (state == BatteryState::CRITICAL)
+        {
+            RCLCPP_ERROR(node_->get_logger(), "严重低电量! 自动启动回充搜索");
+            publishState("CRITICAL_BATTERY_EMERGENCY", "自动启动回充");
+
+            // 紧急情况下，无论当前状态如何都尝试启动回充
+            if (current_state_ == WheelchairState::READY)
+            {
+                transitionToSearching();
+            }
+            else if (current_state_ == WheelchairState::WALKING)
+            {
+                // 如果是行走状态，先停止行走再搜索
+                RCLCPP_WARN(node_->get_logger(), "严重低电量，停止行走并启动回充搜索");
+                transitionToReady();
+                // 短暂延迟后启动搜索
+                transitionToSearching();
+            }
+        }
+        else if (state == BatteryState::LOW)
+        {
+            RCLCPP_WARN(node_->get_logger(), "低电量警告，建议启动回充");
+            publishState("LOW_BATTERY_WARNING", "建议启动回充");
+
+            // 如果当前是就绪状态，自动启动回充
+            if (current_state_ == WheelchairState::READY)
+            {
+                RCLCPP_INFO(node_->get_logger(), "自动启动回充搜索");
+                transitionToSearching();
+            }
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理低电量事件", e);
+    }
+}
+
+void WorkflowManager::processLockVehicle()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理锁车操作");
+
+        // 发布状态更新
+        publishState("VEHICLE_LOCKED", "轮椅已锁定");
+
+        // 如果正在回充或充电，退出流程
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_WARN(node_->get_logger(), "锁车操作，退出回充流程");
+            transitionToReady();
+
+            // 发布回充取消通知
+            publishRechargeStatus("RECHARGE_CANCELLED", "锁车操作取消回充");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理锁车事件", e);
+    }
+}
+
+void WorkflowManager::processUnlockVehicle()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理解锁操作");
+
+        // 发布状态更新
+        publishState("VEHICLE_UNLOCKED", "轮椅已解锁");
+
+        // 根据状态处理解锁
+        if (current_state_ == WheelchairState::READY)
+        {
+            RCLCPP_INFO(node_->get_logger(), "解锁后准备行走");
+            transitionToWalking();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理解锁事件", e);
+    }
+}
+
+void WorkflowManager::processJoystickPullBack()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理摇杆后拉操作");
+
+        // 发布状态更新
+        publishState("JOYSTICK_PULL_BACK", "摇杆后拉");
+
+        // 如果正在回充或充电，退出流程
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_INFO(node_->get_logger(), "用户手动干预，退出回充流程");
+            transitionToReady();
+
+            // 发布回充取消通知
+            publishRechargeStatus("RECHARGE_CANCELLED", "用户手动中断回充");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理摇杆后拉事件", e);
+    }
+}
+
+void WorkflowManager::processJoystickStop()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理摇杆停止操作");
+
+        // 发布状态更新
+        publishState("JOYSTICK_STOP", "摇杆停止");
+
+        // 如果正在回充或充电，退出流程
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_INFO(node_->get_logger(), "用户停止操作，退出回充流程");
+            transitionToReady();
+
+            // 发布回充取消通知
+            publishRechargeStatus("RECHARGE_CANCELLED", "用户停止操作取消回充");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理摇杆停止事件", e);
+    }
+}
+
+void WorkflowManager::processPushStart()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理推行启动");
+
+        // 发布状态更新
+        publishState("PUSH_START", "推行模式启动");
+
+        // 如果正在充电，退出流程
+        if (current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_INFO(node_->get_logger(), "推行操作，退出充电流程");
+            transitionToReady();
+
+            // 发布充电中断通知
+            publishRechargeStatus("CHARGING_INTERRUPTED", "推行操作中断充电");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理推行启动事件", e);
+    }
+}
+
+void WorkflowManager::processPushStop()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理推行停止");
+
+        // 发布状态更新
+        publishState("PUSH_STOP", "推行模式停止");
+
+        // 根据状态处理推行停止
+        if (current_state_ == WheelchairState::WALKING)
+        {
+            RCLCPP_INFO(node_->get_logger(), "推行中停止，返回就绪状态");
+            transitionToReady();
+        }
+        else if (current_state_ == WheelchairState::SEARCHING)
+        {
+            RCLCPP_INFO(node_->get_logger(), "搜索中停止推行，返回就绪状态");
+            transitionToReady();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理推行停止事件", e);
+    }
+}
+
+void WorkflowManager::handleChargingStartEvent()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理充电开始事件");
+
+        // 发布状态更新
+        publishState("BATTERY_START_CHARGING", "开始充电");
+
+        // 如果当前在搜索状态，转移到充电状态
+        if (current_state_ == WheelchairState::SEARCHING)
+        {
+            transitionToCharging();
+        }
+        else if (current_state_ == WheelchairState::WALKING)
+        {
+            RCLCPP_WARN(node_->get_logger(), "行走中开始充电，切换到充电状态");
+            transitionToCharging();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理充电开始事件", e);
+    }
+}
+
+void WorkflowManager::handleChargingStopEvent()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理充电停止事件");
+
+        // 发布状态更新
+        publishState("BATTERY_STOP_CHARGING", "停止充电");
+
+        // 如果正在充电，退出流程
+        if (current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_WARN(node_->get_logger(), "充电停止，退出回充流程");
+            transitionToReady();
+
+            // 发布充电停止通知
+            publishRechargeStatus("CHARGING_STOPPED", "充电已停止");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理充电停止事件", e);
+    }
+}
+
+void WorkflowManager::handleChargingFullEvent()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理电池充满事件");
+
+        // 发布状态更新
+        publishState("BATTERY_FULL", "电池已充满");
+
+        // 如果正在充电，退出流程
+        if (current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_INFO(node_->get_logger(), "电池充满，退出回充流程");
+            transitionToReady();
+
+            // 发布充电完成通知
+            publishRechargeStatus("CHARGING_COMPLETED", "电池已充满，充电完成");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理电池充满事件", e);
+    }
+}
+
+void WorkflowManager::handleBaseStationLostEvent()
+{
+    try
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理基站丢失事件");
+
+        // 发布状态更新
+        publishState("CHARGING_STATION_LOST", "充电基站丢失");
+
+        // 如果正在回充或充电，退出流程
+        if (current_state_ == WheelchairState::SEARCHING ||
+            current_state_ == WheelchairState::CHARGING)
+        {
+            RCLCPP_WARN(node_->get_logger(), "基站丢失，退出回充流程");
+            transitionToReady();
+
+            // 发布回充失败通知
+            publishRechargeStatus("RECHARGE_FAILED", "基站丢失导致回充失败");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理基站丢失事件", e);
+    }
+}
+
+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秒超时，回充失败");
+
+        // 发布回充失败通知
+        publishRechargeStatus("RECHARGE_FAILED", "搜索充电站30秒超时");
+
+        // 退出回充流程
+        transitionToReady();
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理搜索超时事件", e);
+    }
+}
+
+void WorkflowManager::handleRechargeFailure(const std::string &reason)
+{
+    try
+    {
+        RCLCPP_WARN(node_->get_logger(), "回充失败: %s", reason.c_str());
+
+        // 发布回充失败通知
+        publishRechargeStatus("RECHARGE_FAILED", "回充失败: " + reason);
+
+        // 记录错误日志
+        std::stringstream error_log;
+        error_log << "回充流程异常终止\n";
+        error_log << "原因: " << reason << "\n";
+        error_log << "时间: " << node_->now().seconds() << "s";
+
+        RCLCPP_ERROR(node_->get_logger(), "%s", error_log.str().c_str());
+    }
+    catch (const std::exception &e)
+    {
+        RCLCPP_FATAL(node_->get_logger(),
+                     "处理回充失败时发生异常: %s (原失败原因: %s)",
+                     e.what(), reason.c_str());
+    }
+}
+
+// ==================== 异常处理函数 ====================
+
+void WorkflowManager::handleException(const std::string &context, const std::exception &e)
+{
+    std::string error_msg = context + " 异常: " + e.what();
+    RCLCPP_ERROR(node_->get_logger(), "%s", error_msg.c_str());
+
+    // 记录异常发生时的状态
+    std::string previous_state = getCurrentState();
+
+    try
+    {
+        // 任何异常都视为系统故障，尝试紧急恢复
+        emergencyTransitionToReady(error_msg);
+
+        // 记录异常详细信息
+        std::stringstream log;
+        log << "异常处理记录:\n";
+        log << "异常位置: " << context << "\n";
+        log << "异常信息: " << e.what() << "\n";
+        log << "前状态: " << previous_state << "\n";
+        log << "处理时间: " << node_->now().seconds() << "s";
+
+        RCLCPP_ERROR(node_->get_logger(), "%s", log.str().c_str());
+    }
+    catch (const std::exception &inner_e)
+    {
+        // 如果异常处理也失败，记录致命错误
+        RCLCPP_FATAL(node_->get_logger(),
+                     "异常处理过程中再次发生异常: %s (原异常: %s)",
+                     inner_e.what(), error_msg.c_str());
+
+        // 尝试最后的恢复
+        try
+        {
+            current_state_ = WheelchairState::READY;
+            RCLCPP_INFO(node_->get_logger(), "强制设置状态为就绪");
+        }
+        catch (...)
+        {
+            RCLCPP_FATAL(node_->get_logger(), "无法恢复系统状态");
+        }
+    }
+}
+
+void WorkflowManager::emergencyTransitionToReady(const std::string &reason)
+{
+    try
+    {
+        std::string previous_state = getCurrentState();
+
+        // 停止所有定时器
+        if (search_timeout_timer_)
+        {
+            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;
+
+        // 发布紧急状态
+        auto msg = std_msgs::msg::String();
+        msg.data = "EMERGENCY_READY:系统异常后强制就绪 - " + reason;
+        state_publisher_->publish(msg);
+
+        // 如果之前是回充相关状态，发布回充失败
+        if (previous_state == "搜索中" || previous_state == "充电中" || previous_state == "旋转中")
+        {
+            publishRechargeStatus("RECHARGE_EMERGENCY_FAILURE",
+                                  "系统异常导致回充紧急终止: " + reason);
+        }
+
+        RCLCPP_WARN(node_->get_logger(), "紧急状态转换到就绪 (原因: %s)", reason.c_str());
+    }
+    catch (const std::exception &e)
+    {
+        RCLCPP_ERROR(node_->get_logger(),
+                     "紧急状态转换失败: %s", e.what());
+        // 仍然尝试设置状态
+        current_state_ = WheelchairState::READY;
+    }
+}
+
+// ==================== 回充状态管理 ====================
+
+void WorkflowManager::publishRechargeStatus(const std::string &status, const std::string &message)
+{
+    try
+    {
+        // 发布到专用话题
+        auto msg = std_msgs::msg::String();
+        msg.data = status + ": " + message;
+        recharge_timeout_publisher_->publish(msg);
+
+        // 同时记录日志
+        RCLCPP_INFO(node_->get_logger(), "[回充状态] %s: %s", status.c_str(), message.c_str());
+
+        // 调用外部回调（如果有）
+        if (recharge_status_callback_)
+        {
+            recharge_status_callback_(status, message);
+        }
+    }
+    catch (const std::exception &e)
+    {
+        RCLCPP_ERROR(node_->get_logger(),
+                     "发布回充状态时发生异常: %s (状态: %s, 消息: %s)",
+                     e.what(), status.c_str(), message.c_str());
+    }
+}
+
+// ==================== 定时器管理 ====================
+
+void WorkflowManager::startSearchTimeoutTimer()
+{
+    try
+    {
+        // 如果已经有定时器，先取消
+        if (search_timeout_timer_)
+        {
+            search_timeout_timer_->cancel();
+        }
+
+        // 创建30秒定时器
+        search_timeout_timer_ = node_->create_wall_timer(
+            std::chrono::seconds(30),
+            [this]()
+            {
+                this->searchTimeoutCallback();
+            });
+
+        RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已启动 (30秒)");
+    }
+    catch (const std::exception &e)
+    {
+        handleException("启动搜索超时定时器", e);
+    }
+}
+
+void WorkflowManager::stopSearchTimeoutTimer()
+{
+    try
+    {
+        if (search_timeout_timer_)
+        {
+            search_timeout_timer_->cancel();
+            search_timeout_timer_.reset();
+            RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已停止");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("停止搜索超时定时器", e);
+    }
+}
+
+// ==================== 定时器回调 ====================
+
+void WorkflowManager::rotationCheckCallback()
+{
+    try
+    {
+        if (current_state_ == WheelchairState::SEARCHING)
+        {
+            checkRotationTransition();
+        }
+        else
+        {
+            RCLCPP_DEBUG(node_->get_logger(),
+                         "旋转检查回调触发，但当前不在搜索状态");
+            stopRotationTimer();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("旋转检查定时器回调", e);
+    }
+}
+
+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);
+            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(),
+                         "搜索超时定时器触发，但当前不在搜索状态: %s",
+                         getCurrentState().c_str());
+            stopSearchTimeoutTimer();
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("搜索超时定时器回调", e);
+    }
+}
+
+// ==================== 辅助函数 ====================
+
+std::string WorkflowManager::getCurrentState() const
+{
+    return stateToString(current_state_);
+}
+
+void WorkflowManager::setState(WheelchairState new_state)
+{
+    try
+    {
+        std::string old_state = getCurrentState();
+        current_state_ = new_state;
+
+        RCLCPP_INFO(node_->get_logger(),
+                    "状态转移: %s -> %s",
+                    old_state.c_str(),
+                    getCurrentState().c_str());
+        // 增强状态发布：包含时间戳和前后状态
+        std::stringstream enhanced_state;
+        enhanced_state << "[" << node_->now().seconds() << "] "
+                       << old_state << " -> " << getCurrentState();
+
+        // 发布状态到ROS话题
+        auto msg = std_msgs::msg::String();
+        msg.data = enhanced_state.str();
+        state_publisher_->publish(msg);
+
+        // 调用外部回调
+        if (state_update_callback_)
+        {
+            state_update_callback_(getCurrentState(), "状态已更新");
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("设置状态", e);
+    }
+}
+
+std::string WorkflowManager::stateToString(WheelchairState state) const
+{
+    switch (state)
+    {
+    case WheelchairState::READY:
+        return "就绪中";
+    case WheelchairState::WALKING:
+        return "行走中";
+    case WheelchairState::ROTATING: // 新增
+        return "旋转中";
+    case WheelchairState::SEARCHING:
+        return "搜索中";
+    case WheelchairState::CHARGING:
+        return "充电中";
+    default:
+        return "未知状态";
+    }
+}
+
+void WorkflowManager::publishState(const std::string &state_str, const std::string &message)
+{
+    try
+    {
+        auto msg = std_msgs::msg::String();
+        if (message.empty())
+        {
+            msg.data = state_str;
+        }
+        else
+        {
+            msg.data = state_str + ":" + message;
+        }
+        state_publisher_->publish(msg);
+
+        // 调用外部回调
+        if (state_update_callback_)
+        {
+            state_update_callback_(state_str, message);
+        }
+    }
+    catch (const std::exception &e)
+    {
+        RCLCPP_ERROR(node_->get_logger(),
+                     "发布状态时发生异常: %s (状态: %s, 消息: %s)",
+                     e.what(), state_str.c_str(), message.c_str());
+    }
+}
+
+void WorkflowManager::setStateUpdateCallback(StateUpdateCallback callback)
+{
+    state_update_callback_ = callback;
+    RCLCPP_INFO(node_->get_logger(), "已设置状态更新回调");
+}
+
+void WorkflowManager::setRechargeStatusCallback(StateUpdateCallback callback)
+{
+    recharge_status_callback_ = callback;
+    RCLCPP_INFO(node_->get_logger(), "已设置回充状态回调");
+}
+
+void WorkflowManager::setRechargeErrorCallback(std::function<void(const ErrorInfo &)> callback)
+{
+    recharge_error_callback_ = callback;
+    RCLCPP_INFO(node_->get_logger(), "已设置回充错误回调");
+}
+
+void WorkflowManager::triggerEvent(const std::string &event_name)
+{
+    try
+    {
+        // 检查事件权限
+        if (!checkEventPermission(event_name))
+        {
+            RCLCPP_WARN(node_->get_logger(),
+                        "内部触发事件 '%s' 在当前状态 '%s' 下不允许执行",
+                        event_name.c_str(),
+                        getCurrentState().c_str());
+            return;
+        }
+
+        // 执行事件
+        executeEvent(event_name);
+    }
+    catch (const std::exception &e)
+    {
+        handleException("内部触发事件 " + event_name, e);
+    }
+}