wheelchair_state_machine

src2/interface/CMakeLists.txt

@@ -1,26 +0,0 @@
-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()

src2/interface/msg/Empty.msg

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

src2/interface/package.xml

@@ -1,24 +0,0 @@
-<?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>

src2/interface/srv/SceneControls.srv

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

src2/wheelchair_state_machine/CMakeLists.txt

@@ -1,70 +0,0 @@
-cmake_minimum_required(VERSION 3.16)
-project(wheelchair_state_machine)
-
-# 默认使用C++17
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# 查找依赖包
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(visualization_msgs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(interface REQUIRED)
-
-# 包含当前目录
-include_directories(
-  ${CMAKE_CURRENT_SOURCE_DIR}/include
-  ${Eigen3_INCLUDE_DIRS}
-)
-
-# 创建可执行文件
-add_executable(wheelchair_state_machine
-  src/battery_manager.cpp
-  src/recharge_controller.cpp
-  src/main.cpp
-  src/recharge_tool.cpp
-  src/wheelchair_state_machine.cpp
-)
-
-# 使用 ament_target_dependencies 自动处理ROS2依赖
-ament_target_dependencies(wheelchair_state_machine
-  rclcpp
-  std_msgs
-  geometry_msgs
-  sensor_msgs
-  visualization_msgs
-  tf2
-  tf2_geometry_msgs
-  interface  # 关键：这里添加interface依赖
-)
-
-# 链接Eigen3库（不是ROS2包，需要单独处理）
-target_include_directories(wheelchair_state_machine PRIVATE
-  ${Eigen3_INCLUDE_DIRS}
-)
-target_link_libraries(wheelchair_state_machine
-  Eigen3::Eigen
-)
-
-# 添加自定义头文件目录
-target_include_directories(wheelchair_state_machine PRIVATE
-  include
-)
-
-# 安装可执行文件
-install(TARGETS wheelchair_state_machine
-  RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-
-# 安装头文件（如果有需要）
-install(DIRECTORY include/
-  DESTINATION include/${PROJECT_NAME}
-)
-
-ament_package()

src2/wheelchair_state_machine/LICENSE

@@ -1,202 +0,0 @@
-
-                                 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.

src2/wheelchair_state_machine/include/wheelchair_state_machine/battery_manager.hpp

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

src2/wheelchair_state_machine/include/wheelchair_state_machine/ipad_controller.hpp

@@ -1,60 +0,0 @@
-// ipad_controller.hpp
-#ifndef IPAD_CONTROLLER_HPP
-#define IPAD_CONTROLLER_HPP
-
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <memory>
-#include <string>
-#include <functional>
-
-class IpadController
-{
-public:
-    // 回调函数类型
-    using RechargeStartCallback = std::function<void()>;
-    using RechargeCancelCallback = std::function<void()>;
-    using StateUpdateCallback = std::function<void(const std::string& state_str, const std::string& message)>;
-    
-    IpadController(rclcpp::Node* node);
-    
-    // 主要接口 - iPad事件处理
-    void handleIpadStartEvent();
-    void handleIpadCancelEvent();
-    
-    // ROS消息回调
-    void onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg);
-    
-    // 回调设置
-    void setRechargeStartCallback(RechargeStartCallback callback);
-    void setRechargeCancelCallback(RechargeCancelCallback callback);
-    void setStateUpdateCallback(StateUpdateCallback callback);
-    
-    // 状态查询
-    bool isRechargeActive() const { return recharge_active_; }
-    bool isCmdEnabled() const { return cmd_enable_; }
-    std::string getCurrentMode() const { return current_mode_; }
-    
-private:
-    rclcpp::Node* node_;
-    
-    // 回调函数
-    RechargeStartCallback recharge_start_callback_;
-    RechargeCancelCallback recharge_cancel_callback_;
-    StateUpdateCallback state_update_callback_;
-    
-    // 状态
-    bool recharge_active_;
-    bool cmd_enable_;
-    std::string current_mode_;
-    
-    // 私有方法
-    void startRecharge();
-    void cancelRecharge();
-    void updateState(const std::string& state, const std::string& message = "");
-    void logInfo(const std::string& message);
-    void logWarning(const std::string& message);
-};
-
-#endif // IPAD_CONTROLLER_HPP

src2/wheelchair_state_machine/include/wheelchair_state_machine/recharge_controller.hpp

@@ -1,120 +0,0 @@
-// recharge_controller.hpp
-#ifndef RECHARGE_CONTROLLER_HPP
-#define RECHARGE_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>
-
-class RechargeController
-{
-public:
-    using LaserScan = sensor_msgs::msg::LaserScan;
-    using LaserScanSharedPtr = sensor_msgs::msg::LaserScan::SharedPtr;
-    
-    RechargeController(rclcpp::Node* node);
-    ~RechargeController();
-    
-    void initialize();
-    
-    // 控制接口（由IpadController调用）
-    void startRecharge();
-    void cancelRecharge();
-    void stopMotion();
-    
-    // 充电电压检测
-    void setChargingVoltage(bool value);
-    bool getChargingVoltage() const;
-    
-    // 激光数据处理（由ROS回调调用）
-    void processLaserScan(const LaserScanSharedPtr& msg);
-    
-    // 状态查询
-    bool isActive() const { return active_; }
-    ChargingState getChargingState() const { return charging_state_; }
-    void setChargingState(ChargingState state) { charging_state_ = state; }
-    
-    // 控制发布
-    void publishControl();
-    
-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 recharge_timer_;
-    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
-    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;
-    
-    // 状态变量
-    bool active_;
-    ChargingState charging_state_;
-    geometry_msgs::msg::Twist ctrl_twist_;
-    rclcpp::Time ctrl_twist_time_;
-    rclcpp::Time detect_station_time_;
-    
-    // 参数
-    ScanFilterParam param_;
-    double distance_threshold_;
-    int fit_window_size_;
-    double fit_residual_threshold_;
-    int frame_index_;
-    
-    // 充电检测相关
-    bool detect_charging_voltage_;
-    rclcpp::Time detect_charging_voltage_time_;
-    
-    // 激光数据处理方法（保持原有算法逻辑）
-    bool processSegmentData(const LaserScanSharedPtr& msg, int begin_index, int end_index);
-    void controlRechargeMotion(const RechargeResult& result);
-    void publishRechargeVisualization(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::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);
-    
-    // 辅助函数
-    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);
-    
-    void procData(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; }
-    
-    // 定时器回调
-    void rechargeTimerCallback();
-    Eigen::MatrixXd getPointList(const std::vector<IndexPoint>& points);
-};
-
-#endif // RECHARGE_CONTROLLER_HPP

src2/wheelchair_state_machine/include/wheelchair_state_machine/recharge_tool.hpp

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

src2/wheelchair_state_machine/include/wheelchair_state_machine/wheelchair_state_machine.hpp

@@ -1,114 +0,0 @@
-// wheelchair_state_machine.hpp
-#ifndef WHEELCHAIR_STATE_MACHINE_HPP
-#define WHEELCHAIR_STATE_MACHINE_HPP
-
-#include <rclcpp/rclcpp.hpp>
-#include <string>
-#include <map>
-#include <memory>
-#include "wheelchair_types.hpp"
-#include "battery_manager.hpp"
-#include "recharge_controller.hpp"
-#include "ipad_controller.hpp"
-// 添加必要的ROS消息头文件
-#include <std_msgs/msg/string.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-#include <sensor_msgs/msg/battery_state.hpp>
-
-class WheelchairStateMachine : public rclcpp::Node
-{
-public:
-    // 构造函数
-    WheelchairStateMachine();
-    
-    // 状态机核心接口
-    bool handleEvent(const std::string &event);
-    std::string getCurrentState() const;
-    void setState(WheelchairState new_state);
-    
-    // 回充控制接口
-    void startRecharge();
-    void cancelRecharge();
-    bool isRechargeActive() const;
-    
-    // 充电检测接口
-    void setChargingVoltage(bool value);
-    bool getChargingVoltage() const;
-
-    void displaySystemStatus();
-    
-private:
-    // ==================== 状态机成员 ====================
-    WheelchairState current_state_;
-    std::map<std::string, std::map<WheelchairState, bool>> transition_table_;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr state_publisher_;
-    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
-    rclcpp::TimerBase::SharedPtr search_timeout_timer_;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr cmd_enable_sub_;
-    bool interface_active_;
-    
-    // ==================== 模块化组件 ====================
-    BatteryManager battery_manager_;
-    RechargeController recharge_controller_;
-    IpadController ipad_controller_;
-    
-    // 模式管理
-    std::string last_mode_;
-    
-    // ==================== 私有方法 ====================
-    
-    // 状态机初始化
-    void initializeTransitionTable();
-    void executeEvent(const std::string &event);
-    void checkStateTransition(const std::string &event);
-    
-    // 状态转移函数
-    void transitionToWalking();
-    void transitionToSearching();
-    void transitionToCharging();
-    void transitionToReady();
-    
-    // 定时器相关
-    void startSearchTimeoutTimer();
-    void stopSearchTimeoutTimer();
-    void searchTimeoutCallback();
-    
-    // 事件处理函数
-    void handleIpadPhoneStart();
-    void handleIpadPhoneCancel();
-    void handleBluetoothDisconnected();
-    void handleBluetoothConnected();
-    void handleBaseStationPowerOff();
-    void handleLowBatteryWarning();
-    void handleLockVehicle();
-    void handleUnlockVehicle();
-    void handleJoystickPullBack();
-    void handleJoystickStop();
-    void handlePushStart();
-    void handlePushStop();
-    void handleBatteryStartCharging();
-    void handleBatteryStopCharging();
-    void handleBatteryFull();
-    void handleErrorCode();
-    void handleBaseStationLost();
-    void handleSearchTimeout();
-    
-    // 电池回调处理
-    void onBatteryStateChanged(BatteryState state, float percentage);
-    void onLowBattery(BatteryState state, float percentage);
-    
-    // iPad回调处理
-    void onIpadStateUpdate(const std::string& state_str, const std::string& message);
-    // 辅助函数
-    std::string stateToString(WheelchairState state) const;
-    std::string chargingStateToString(ChargingState state) const;
-    std::string batteryStateToString(BatteryState state) const;
-    
-    void publishStateUpdate(const std::string &state_str, const std::string &message = "");
-    
-    // 模式管理
-    void setMode(const std::string &mode);
-    std::string getMode() const;
-};
-
-#endif // WHEELCHAIR_STATE_MACHINE_HPP

src2/wheelchair_state_machine/include/wheelchair_state_machine/wheelchair_types.hpp

@@ -1,82 +0,0 @@
-// wheelchair_types.hpp
-#ifndef WHEELCHAIR_TYPES_HPP
-#define WHEELCHAIR_TYPES_HPP
-
-#include <vector>
-#include <string>
-#include <Eigen/Dense>
-
-// 轮椅状态枚举
-enum class WheelchairState
-{
-    READY,     // 就绪中
-    WALKING,   // 行走中
-    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

src2/wheelchair_state_machine/package.xml

@@ -1,27 +0,0 @@
-<?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>

src2/wheelchair_state_machine/src/battery_manager.cpp

@@ -1,194 +0,0 @@
-// 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 "未知";
-    }
-}

src2/wheelchair_state_machine/src/ipad_controller.cpp

@@ -1,162 +0,0 @@
-// ipad_controller.cpp
-#include "wheelchair_state_machine/ipad_controller.hpp"
-#include <rclcpp/logging.hpp>
-
-using namespace std::chrono_literals;
-using std::placeholders::_1;
-
-IpadController::IpadController(rclcpp::Node* node)
-    : node_(node)
-    , recharge_active_(false)
-    , cmd_enable_(false)
-    , current_mode_("")
-{
-    logInfo("iPad控制器初始化完成");
-}
-
-void IpadController::handleIpadStartEvent()
-{
-    logInfo("处理iPad自主回充启动事件");
-    
-    if (!recharge_active_)
-    {
-        // 启动回充
-        startRecharge();
-        updateState("IPAD_RECHARGE_STARTED", "iPad启动自主回充");
-    }
-    else
-    {
-        logWarning("回充功能已在运行中");
-        updateState("IPAD_RECHARGE_ALREADY_ACTIVE", "回充功能已在运行");
-    }
-}
-
-void IpadController::handleIpadCancelEvent()
-{
-    logInfo("处理iPad自主回充取消事件");
-    
-    if (recharge_active_)
-    {
-        // 取消回充
-        cancelRecharge();
-        updateState("IPAD_RECHARGE_CANCELLED", "iPad取消自主回充");
-    }
-    else
-    {
-        logWarning("回充功能未激活，无需取消");
-        updateState("IPAD_RECHARGE_NOT_ACTIVE", "回充功能未激活");
-    }
-}
-
-void IpadController::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    bool new_enable = msg->data;
-    logInfo("收到命令使能: " + std::to_string(new_enable));
-    
-    if (!cmd_enable_ && new_enable)
-    {
-        // 命令使能从false变为true
-        cmd_enable_ = true;
-        
-        if (!recharge_active_)
-        {
-            // 启动回充
-            startRecharge();
-            updateState("CMD_ENABLE_RECHARGE_STARTED", "命令使能启动回充");
-        }
-        else
-        {
-            logInfo("回充已在运行中，仅更新使能状态");
-        }
-    }
-    else if (cmd_enable_ && !new_enable)
-    {
-        // 命令使能从true变为false
-        cmd_enable_ = false;
-        
-        if (recharge_active_)
-        {
-            // 取消回充
-            cancelRecharge();
-            updateState("CMD_ENABLE_RECHARGE_CANCELLED", "命令使能取消回充");
-        }
-    }
-    
-    logInfo("当前命令使能状态: " + std::string(cmd_enable_ ? "启用" : "禁用"));
-}
-
-void IpadController::startRecharge()
-{
-    logInfo("启动回充功能");
-    recharge_active_ = true;
-    current_mode_ = "recharge";
-    
-    // 调用回充启动回调
-    if (recharge_start_callback_)
-    {
-        recharge_start_callback_();
-    }
-    else
-    {
-        logWarning("回充启动回调未设置");
-    }
-}
-
-void IpadController::cancelRecharge()
-{
-    logInfo("取消回充功能");
-    recharge_active_ = false;
-    current_mode_ = "";
-    
-    // 调用回充取消回调
-    if (recharge_cancel_callback_)
-    {
-        recharge_cancel_callback_();
-    }
-    else
-    {
-        logWarning("回充取消回调未设置");
-    }
-}
-
-void IpadController::setRechargeStartCallback(RechargeStartCallback callback)
-{
-    recharge_start_callback_ = callback;
-    logInfo("已设置回充启动回调");
-}
-
-void IpadController::setRechargeCancelCallback(RechargeCancelCallback callback)
-{
-    recharge_cancel_callback_ = callback;
-    logInfo("已设置回充取消回调");
-}
-
-void IpadController::setStateUpdateCallback(StateUpdateCallback callback)
-{
-    state_update_callback_ = callback;
-    logInfo("已设置状态更新回调");
-}
-
-void IpadController::updateState(const std::string& state, const std::string& message)
-{
-    if (state_update_callback_)
-    {
-        state_update_callback_(state, message);
-    }
-}
-
-void IpadController::logInfo(const std::string& message)
-{
-    if (node_)
-    {
-        RCLCPP_INFO(node_->get_logger(), "[iPadController] %s", message.c_str());
-    }
-}
-
-void IpadController::logWarning(const std::string& message)
-{
-    if (node_)
-    {
-        RCLCPP_WARN(node_->get_logger(), "[iPadController] %s", message.c_str());
-    }
-}

src2/wheelchair_state_machine/src/main.cpp

@@ -1,20 +0,0 @@
-// main.cpp
-#include "wheelchair_state_machine/wheelchair_state_machine.hpp"
-#include <rclcpp/rclcpp.hpp>
-#include <memory>
-
-int main(int argc, char **argv)
-{
-    rclcpp::init(argc, argv);
-    
-    auto state_machine = std::make_shared<WheelchairStateMachine>();
-    
-    // 显示系统状态
-    state_machine->displaySystemStatus();
-    
-    // 保持节点运行
-    rclcpp::spin(state_machine);
-    rclcpp::shutdown();
-    
-    return 0;
-}

src2/wheelchair_state_machine/src/recharge_controller.cpp

@@ -1,1071 +0,0 @@
-// recharge_controller.cpp
-#include "wheelchair_state_machine/recharge_controller.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;
-
-RechargeController::RechargeController(rclcpp::Node* node)
-    : node_(node)
-    , active_(false)
-    , charging_state_(ChargingState::IDLE)
-    , detect_charging_voltage_(false)
-    , frame_index_(0)
-{
-}
-
-RechargeController::~RechargeController()
-{
-}
-
-void RechargeController::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);
-    
-    // 初始化控制消息
-    ctrl_twist_.linear.x = 0.0;
-    ctrl_twist_.angular.z = 0.0;
-    ctrl_twist_time_ = node_->now();
-    
-    // 创建回充定时器
-    recharge_timer_ = node_->create_wall_timer(
-        100ms, std::bind(&RechargeController::rechargeTimerCallback, this));
-    
-    detect_station_time_ = node_->now() - rclcpp::Duration(100.0, 0);
-    detect_charging_voltage_time_ = node_->now();
-    
-    RCLCPP_INFO(node_->get_logger(), "回充控制器已初始化");
-}
-
-void RechargeController::startRecharge()
-{
-    active_ = true;
-    charging_state_ = ChargingState::NAVIGATING;
-    RCLCPP_INFO(node_->get_logger(), "回充功能已启动");
-}
-
-void RechargeController::cancelRecharge()
-{
-    active_ = false;
-    charging_state_ = ChargingState::IDLE;
-    stopMotion();
-    RCLCPP_INFO(node_->get_logger(), "回充功能已取消");
-}
-
-void RechargeController::rechargeTimerCallback()
-{
-    // 如果不是回充模式，直接返回
-    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 RechargeController::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 RechargeController::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)
-    {
-        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())
-    {
-        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;
-    }
-    
-    return false;
-}
-
-void RechargeController::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 RechargeController::controlRechargeMotion(const RechargeResult &result)
-{
-    // 如果检测到充电电压，停止移动
-    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 RechargeController::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 RechargeController::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 RechargeController::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 RechargeController::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> RechargeController::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>> RechargeController::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>> RechargeController::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>> RechargeController::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>>
-RechargeController::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>>
-RechargeController::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 RechargeController::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 RechargeController::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 RechargeController::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(), "已发布过滤后的激光数据");
-}
-
-RechargeController::LaserScanSharedPtr RechargeController::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 RechargeController::isValid(float value) const
-{
-    if (fabs(value) < 0.001)
-        return false;
-    return !std::isinf(value) && !std::isnan(value);
-}
-
-int RechargeController::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 RechargeController::publishControl()
-{
-    if (active_)
-    {
-        pub_ctrl_->publish(ctrl_twist_);
-    }
-}
-
-void RechargeController::stopMotion()
-{
-    ctrl_twist_.linear.x = 0.0;
-    ctrl_twist_.angular.z = 0.0;
-    ctrl_twist_time_ = node_->now();
-    pub_ctrl_->publish(ctrl_twist_);
-}
-
-void RechargeController::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 RechargeController::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;
-}

src2/wheelchair_state_machine/src/recharge_tool.cpp

@@ -1,197 +0,0 @@
-// 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};
-}

src2/wheelchair_state_machine/src/wheelchair_state_machine.cpp

@@ -1,951 +0,0 @@
-// wheelchair_state_machine.cpp
-#include "wheelchair_state_machine/wheelchair_state_machine.hpp"
-#include <rclcpp/logging.hpp>
-#include <rclcpp/qos.hpp>
-#include <cmath>
-
-using namespace std::chrono_literals;
-using std::placeholders::_1;
-WheelchairStateMachine::WheelchairStateMachine()
-    : Node("wheelchair_state_machine")
-    , current_state_(WheelchairState::READY)
-    , battery_manager_(this, 20.0f, 10.0f)
-    , recharge_controller_(this)
-    , ipad_controller_(this)
-    , last_mode_("")
-{
-    // 初始化状态转移表
-    initializeTransitionTable();
-    
-    // 创建状态发布者
-    state_publisher_ = this->create_publisher<std_msgs::msg::String>(
-        "wheelchair/state", 10);
-    
-    
-    // 初始化电池管理器
-    battery_manager_.setBatteryStateCallback(
-        std::bind(&WheelchairStateMachine::onBatteryStateChanged, this, std::placeholders::_1, std::placeholders::_2));
-    battery_manager_.setLowBatteryCallback(
-        std::bind(&WheelchairStateMachine::onLowBattery, this, std::placeholders::_1, std::placeholders::_2));
-    battery_manager_.initialize();
-    
-    // 初始化回充控制器
-    recharge_controller_.initialize();
-
-    // 初始化iPad控制器
-    ipad_controller_.setRechargeStartCallback(
-        [this]() {
-            RCLCPP_INFO(this->get_logger(), "iPad控制器请求启动回充");
-            // 这里可以添加额外的逻辑，但主要状态转移由事件处理函数处理
-        });
-    
-    ipad_controller_.setRechargeCancelCallback(
-        [this]() {
-            RCLCPP_INFO(this->get_logger(), "iPad控制器请求取消回充");
-            recharge_controller_.cancelRecharge();
-        });
-    
-    ipad_controller_.setStateUpdateCallback(
-        std::bind(&WheelchairStateMachine::onIpadStateUpdate, this,
-                  std::placeholders::_1, std::placeholders::_2));
-    
-    // 创建命令使能订阅者（由iPad控制器处理）
-    cmd_enable_sub_ = this->create_subscription<std_msgs::msg::Bool>(
-        "/recharge/cmd_enable", 10,
-        [this](const std_msgs::msg::Bool::SharedPtr msg) {
-            this->ipad_controller_.onCmdEnable(msg);
-        });
-    
-    RCLCPP_INFO(this->get_logger(), "智能轮椅状态机（集成回充功能）已启动");
-    RCLCPP_INFO(this->get_logger(), "当前状态: %s", getCurrentState().c_str());
-}
-
-void WheelchairStateMachine::initializeTransitionTable()
-{
-    transition_table_.clear();
-    
-    // 1. ipad&phone界面启动: 就绪中✅, 其他❌
-    std::map<WheelchairState, bool> ipad_start_perms = {
-        {WheelchairState::READY, true},
-        {WheelchairState::WALKING, 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::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::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::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::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::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::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::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::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::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::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::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::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::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::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::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::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::SEARCHING, true},
-        {WheelchairState::CHARGING, false}};
-    transition_table_["search_30s_timeout"] = search_timeout_perms;
-    
-    RCLCPP_INFO(this->get_logger(), "状态转移表已初始化完成");
-}
-
-bool WheelchairStateMachine::handleEvent(const std::string &event)
-{
-    if (transition_table_.find(event) == transition_table_.end())
-    {
-        RCLCPP_WARN(this->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() &&
-        state_permissions[current_state_])
-    {
-        executeEvent(event);
-        return true;
-    }
-    else
-    {
-        RCLCPP_WARN(this->get_logger(),
-                    "事件 '%s' 在当前状态 '%s' 下不允许执行",
-                    event.c_str(),
-                    getCurrentState().c_str());
-        return false;
-    }
-}
-
-void WheelchairStateMachine::executeEvent(const std::string &event)
-{
-    RCLCPP_INFO(this->get_logger(), "执行事件: %s", event.c_str());
-    
-    if (event == "ipad_phone_interface_start")
-    {
-        handleIpadPhoneStart();
-    }
-    else if (event == "ipad_phone_interface_cancel")
-    {
-        handleIpadPhoneCancel();
-    }
-    else if (event == "bluetooth_disconnected")
-    {
-        handleBluetoothDisconnected();
-    }
-    else if (event == "bluetooth_connected")
-    {
-        handleBluetoothConnected();
-    }
-    else if (event == "base_station_power_off")
-    {
-        handleBaseStationPowerOff();
-    }
-    else if (event == "low_battery_warning")
-    {
-        handleLowBatteryWarning();
-    }
-    else if (event == "lock_vehicle")
-    {
-        handleLockVehicle();
-    }
-    else if (event == "unlock_vehicle")
-    {
-        handleUnlockVehicle();
-    }
-    else if (event == "joystick_pull_back")
-    {
-        handleJoystickPullBack();
-    }
-    else if (event == "joystick_stop")
-    {
-        handleJoystickStop();
-    }
-    else if (event == "push_start")
-    {
-        handlePushStart();
-    }
-    else if (event == "push_stop")
-    {
-        handlePushStop();
-    }
-    else if (event == "battery_start_charging")
-    {
-        handleBatteryStartCharging();
-    }
-    else if (event == "battery_stop_charging")
-    {
-        handleBatteryStopCharging();
-    }
-    else if (event == "battery_full")
-    {
-        handleBatteryFull();
-    }
-    else if (event == "error_code_handling")
-    {
-        handleErrorCode();
-    }
-    else if (event == "base_station_lost")
-    {
-        handleBaseStationLost();
-    }
-    else if (event == "search_30s_timeout")
-    {
-        handleSearchTimeout();
-    }
-    
-    // 根据事件可能触发状态转移
-    checkStateTransition(event);
-}
-
-void WheelchairStateMachine::checkStateTransition(const std::string &event)
-{
-    // 根据事件检查是否需要状态转移
-    switch (current_state_)
-    {
-    case WheelchairState::READY:
-        if (event == "low_battery_warning" || event == "ipad_phone_interface_start")
-        {
-            transitionToSearching();
-        }
-        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::SEARCHING:
-        if (event == "search_30s_timeout" || event == "base_station_lost")
-        {
-            transitionToReady();
-        }
-        else if (event == "battery_start_charging" || getChargingVoltage())
-        {
-            transitionToCharging();
-        }
-        break;
-        
-    case WheelchairState::CHARGING:
-        if (event == "battery_full" ||
-            event == "battery_stop_charging" ||
-            event == "base_station_power_off")
-        {
-            transitionToReady();
-        }
-        break;
-    }
-}
-
-std::string WheelchairStateMachine::getCurrentState() const
-{
-    return stateToString(current_state_);
-}
-
-void WheelchairStateMachine::setState(WheelchairState new_state)
-{
-    std::string old_state = getCurrentState();
-    current_state_ = new_state;
-    
-    RCLCPP_INFO(this->get_logger(),
-                "状态转移: %s -> %s",
-                old_state.c_str(),
-                getCurrentState().c_str());
-    
-    // 发布状态话题
-    auto msg = std_msgs::msg::String();
-    msg.data = getCurrentState();
-    state_publisher_->publish(msg);
-}
-
-// ==================== 状态转移函数 ====================
-
-void WheelchairStateMachine::transitionToWalking()
-{
-    if (current_state_ == WheelchairState::READY ||
-        current_state_ == WheelchairState::SEARCHING)
-    {
-        setState(WheelchairState::WALKING);
-        RCLCPP_INFO(this->get_logger(), "轮椅开始行走");
-        
-        // 发布状态通知
-        publishStateUpdate("WHEELCHAIR_WALKING");
-    }
-}
-
-void WheelchairStateMachine::transitionToSearching()
-{
-    if (current_state_ == WheelchairState::READY)
-    {
-        setState(WheelchairState::SEARCHING);
-        RCLCPP_INFO(this->get_logger(), "开始搜索充电站");
-        
-        // 激活回充系统
-        recharge_controller_.startRecharge();
-        last_mode_ = "recharge";
-        
-        // 发布搜索开始通知
-        publishStateUpdate("SEARCH_CHARGING_STATION_START");
-        
-        // 启动30秒超时定时器
-        startSearchTimeoutTimer();
-    }
-}
-
-void WheelchairStateMachine::transitionToCharging()
-{
-    if (current_state_ == WheelchairState::SEARCHING || current_state_ == WheelchairState::WALKING)
-    {
-        setState(WheelchairState::CHARGING);
-        RCLCPP_INFO(this->get_logger(), "开始充电");
-        
-        // 停止运动
-        recharge_controller_.stopMotion();
-        
-        // 停止搜索超时定时器
-        stopSearchTimeoutTimer();
-        
-        // 更新充电状态
-        recharge_controller_.setChargingState(ChargingState::CHARGING);
-        
-        // 设置充电电压检测
-        recharge_controller_.setChargingVoltage(true);
-        
-        // 发布充电开始通知
-        publishStateUpdate("CHARGING_START");
-    }
-}
-
-void WheelchairStateMachine::transitionToReady()
-{
-    // 可以从所有状态返回到就绪状态
-    std::string previous_state = getCurrentState();
-    
-    // 停止回充系统
-    recharge_controller_.cancelRecharge();
-    last_mode_ = "";
-    
-    // 停止所有运动
-    recharge_controller_.stopMotion();
-    
-    // 停止定时器
-    stopSearchTimeoutTimer();
-    
-    setState(WheelchairState::READY);
-    
-     RCLCPP_INFO(this->get_logger(), "返回到就绪状态 (前状态: %s)", 
-                previous_state.c_str());
-    
-    // 发布就绪状态通知
-    publishStateUpdate("WHEELCHAIR_READY");
-}
-
-// ==================== 定时器相关函数 ====================
-
-void WheelchairStateMachine::startSearchTimeoutTimer()
-{
-    // 如果已经有定时器，先取消
-    if (search_timeout_timer_)
-    {
-        search_timeout_timer_->cancel();
-    }
-    
-    // 创建30秒定时器
-    search_timeout_timer_ = this->create_wall_timer(
-        std::chrono::seconds(30),
-        [this]()
-        {
-            this->searchTimeoutCallback();
-        });
-    
-    RCLCPP_INFO(this->get_logger(), "搜索超时定时器已启动");
-}
-
-void WheelchairStateMachine::stopSearchTimeoutTimer()
-{
-    if (search_timeout_timer_)
-    {
-        search_timeout_timer_->cancel();
-        search_timeout_timer_.reset();
-        RCLCPP_INFO(this->get_logger(), "搜索超时定时器已停止");
-    }
-}
-
-void WheelchairStateMachine::searchTimeoutCallback()
-{
-    if (current_state_ == WheelchairState::SEARCHING)
-    {
-        RCLCPP_WARN(this->get_logger(), "搜索充电站超时30秒");
-        handleSearchTimeout();
-    }
-}
-
-// ==================== iPad事件处理 ====================
-
-void WheelchairStateMachine::handleIpadPhoneStart()
-{
-    RCLCPP_INFO(this->get_logger(), "处理iPad自主回充按钮点击");
-    
-    // 通过iPad控制器处理启动事件
-    ipad_controller_.handleIpadStartEvent();
-    // 根据当前状态决定是否启动回充
-    if (current_state_ == WheelchairState::READY)
-    {
-        if (ipad_controller_.isRechargeActive())
-        {
-            // 启动回充控制器
-            recharge_controller_.startRecharge();
-            
-            // 转移到搜索状态
-            transitionToSearching();
-            
-            RCLCPP_INFO(this->get_logger(), "iPad自主回充已启动，进入搜索状态");
-        }
-    }
-    else
-    {
-        RCLCPP_WARN(this->get_logger(), 
-                   "当前状态 %s 无法启动回充", getCurrentState().c_str());
-    }
-}
-
-void WheelchairStateMachine::handleIpadPhoneCancel()
-{
-    RCLCPP_INFO(this->get_logger(), "处理iPad自主回充取消");
-    
-    // 通过iPad控制器处理取消事件
-    ipad_controller_.handleIpadCancelEvent();
-    
-    // 根据当前状态决定是否取消回充
-    if (current_state_ == WheelchairState::SEARCHING || 
-        current_state_ == WheelchairState::CHARGING)
-    {
-        if (!ipad_controller_.isRechargeActive())
-        {
-            // 转移到就绪状态
-            transitionToReady();
-            
-            RCLCPP_INFO(this->get_logger(), "iPad自主回充已取消，返回到就绪状态");
-        }
-    }
-}
-
-void WheelchairStateMachine::onIpadStateUpdate(const std::string& state_str, 
-                                              const std::string& message)
-{
-    RCLCPP_INFO(this->get_logger(), "iPad状态更新: %s - %s", 
-                state_str.c_str(), message.c_str());
-    
-    // 发布状态更新
-    publishStateUpdate(state_str, message);
-    
-    // 根据iPad状态更新状态机
-    if (state_str == "IPAD_RECHARGE_STARTED" || 
-        state_str == "CMD_ENABLE_RECHARGE_STARTED")
-    {
-        // 如果当前是就绪状态，自动转移到搜索状态
-        if (current_state_ == WheelchairState::READY)
-        {
-            transitionToSearching();
-        }
-    }
-    else if (state_str == "IPAD_RECHARGE_CANCELLED" || 
-             state_str == "CMD_ENABLE_RECHARGE_CANCELLED")
-    {
-        // 如果当前是搜索或充电状态，返回到就绪状态
-        if (current_state_ == WheelchairState::SEARCHING || 
-            current_state_ == WheelchairState::CHARGING)
-        {
-            transitionToReady();
-        }
-    }
-}
-
-// ==================== 电池相关事件处理 ====================
-
-void WheelchairStateMachine::onBatteryStateChanged(BatteryState state, float percentage)
-{
-    // 处理电池状态变化
-    switch (state)
-    {
-    case BatteryState::CHARGING:
-        handleBatteryStartCharging();
-        break;
-    case BatteryState::FULL:
-        handleBatteryFull();
-        break;
-    default:
-        break;
-    }
-}
-
-void WheelchairStateMachine::onLowBattery(BatteryState state, float percentage)
-{
-    // 触发低电量警告
-    handleLowBatteryWarning();
-}
-
-void WheelchairStateMachine::handleLowBatteryWarning()
-{
-    BatteryState battery_state = battery_manager_.getBatteryState();
-    float battery_percentage = battery_manager_.getBatteryPercentage();
-    
-    RCLCPP_INFO(this->get_logger(), "处理低电量警告 - 当前电量: %.1f%%", battery_percentage);
-    
-    // 根据当前电池状态采取行动
-    if (battery_state == BatteryState::CRITICAL)
-    {
-        RCLCPP_ERROR(this->get_logger(), "严重低电量! 自动启动回充搜索");
-        publishStateUpdate("CRITICAL_BATTERY_EMERGENCY", "自动启动回充");
-        
-        // 紧急情况下，无论当前状态如何都尝试启动回充
-        if (current_state_ == WheelchairState::READY)
-        {
-            transitionToSearching();
-        }
-        else if (current_state_ == WheelchairState::WALKING)
-        {
-            // 如果是行走状态，先停止行走再搜索
-            RCLCPP_WARN(this->get_logger(), "严重低电量，停止行走并启动回充搜索");
-            transitionToReady();
-            rclcpp::sleep_for(std::chrono::seconds(1));
-            transitionToSearching();
-        }
-    }
-    else if (battery_state == BatteryState::LOW)
-    {
-        RCLCPP_WARN(this->get_logger(), "低电量警告，建议启动回充");
-        publishStateUpdate("LOW_BATTERY_WARNING", "建议启动回充");
-        
-        // 如果当前是就绪状态，自动启动回充
-        if (current_state_ == WheelchairState::READY)
-        {
-            RCLCPP_INFO(this->get_logger(), "自动启动回充搜索");
-            transitionToSearching();
-        }
-    }
-}
-
-void WheelchairStateMachine::handleBatteryStartCharging()
-{
-    float battery_percentage = battery_manager_.getBatteryPercentage();
-    RCLCPP_INFO(this->get_logger(), "开始充电 - 当前电量: %.1f%%", battery_percentage);
-    
-    // 设置充电电压检测
-    recharge_controller_.setChargingVoltage(true);
-    
-    // 如果当前在搜索状态，转移到充电状态
-    if (current_state_ == WheelchairState::SEARCHING)
-    {
-        transitionToCharging();
-    }
-    
-    // 发布充电开始通知
-    publishStateUpdate("BATTERY_START_CHARGING");
-}
-
-void WheelchairStateMachine::handleBatteryStopCharging()
-{
-    RCLCPP_INFO(this->get_logger(), "停止充电");
-    recharge_controller_.setChargingVoltage(false);
-}
-
-void WheelchairStateMachine::handleBatteryFull()
-{
-    float battery_percentage = battery_manager_.getBatteryPercentage();
-    RCLCPP_INFO(this->get_logger(), "电池已充满 - 电量: %.1f%%", battery_percentage);
-    
-    // 停止充电，返回就绪状态
-    if (current_state_ == WheelchairState::CHARGING)
-    {
-        transitionToReady();
-    }
-    
-    publishStateUpdate("BATTERY_FULL");
-}
-
-// ==================== 其他事件处理函数 ====================
-
-void WheelchairStateMachine::handleBluetoothDisconnected()
-{
-    RCLCPP_INFO(this->get_logger(), "处理蓝牙断开");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleBluetoothConnected()
-{
-    RCLCPP_INFO(this->get_logger(), "处理蓝牙连接成功");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleBaseStationPowerOff()
-{
-    RCLCPP_INFO(this->get_logger(), "处理基站断电");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleLockVehicle()
-{
-    RCLCPP_INFO(this->get_logger(), "处理锁车操作");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleUnlockVehicle()
-{
-    RCLCPP_INFO(this->get_logger(), "处理解锁操作");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleJoystickPullBack()
-{
-    RCLCPP_INFO(this->get_logger(), "处理摇杆后拉操作");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleJoystickStop()
-{
-    RCLCPP_INFO(this->get_logger(), "处理摇杆停止操作");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handlePushStart()
-{
-    RCLCPP_INFO(this->get_logger(), "处理推行启动");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handlePushStop()
-{
-    RCLCPP_INFO(this->get_logger(), "处理推行停止");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleErrorCode()
-{
-    RCLCPP_INFO(this->get_logger(), "处理错误码");
-    // 实现具体逻辑
-}
-
-void WheelchairStateMachine::handleBaseStationLost()
-{
-    RCLCPP_INFO(this->get_logger(), "基站检测丢失");
-    
-    // 发布基站丢失通知
-    publishStateUpdate("CHARGING_STATION_LOST");
-    
-    // 根据状态处理基站丢失
-    if (current_state_ == WheelchairState::WALKING)
-    {
-        // 行走中检测到基站丢失，进入搜索状态
-        transitionToSearching();
-    }
-    else if (current_state_ == WheelchairState::SEARCHING)
-    {
-        // 搜索中检测到基站丢失，返回就绪状态
-        transitionToReady();
-    }
-}
-
-void WheelchairStateMachine::handleSearchTimeout()
-{
-    RCLCPP_INFO(this->get_logger(), "搜索30秒超时");
-    
-    // 发布超时通知
-    publishStateUpdate("SEARCH_CHARGING_STATION_TIMEOUT");
-    
-    // 根据状态转移表，搜索超时应返回就绪状态
-    if (current_state_ == WheelchairState::SEARCHING)
-    {
-        transitionToReady();
-    }
-}
-
-// ==================== 辅助函数实现 ====================
-
-std::string WheelchairStateMachine::stateToString(WheelchairState state) const
-{
-    switch (state)
-    {
-    case WheelchairState::READY:
-        return "就绪中";
-    case WheelchairState::WALKING:
-        return "行走中";
-    case WheelchairState::SEARCHING:
-        return "搜索中";
-    case WheelchairState::CHARGING:
-        return "充电中";
-    default:
-        return "未知状态";
-    }
-}
-
-std::string WheelchairStateMachine::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 "未知";
-    }
-}
-
-std::string WheelchairStateMachine::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 "未知";
-    }
-}
-
-void WheelchairStateMachine::displaySystemStatus()
-{
-    std::string status_report = "系统状态报告:\n";
-    
-    // 轮椅状态
-    status_report += "  - 轮椅状态: " + getCurrentState() + "\n";
-    
-    // 电池状态
-    status_report += "  - 电池状态: " + battery_manager_.stateToString() +
-                     " (" + std::to_string(battery_manager_.getBatteryPercentage()) + "%)\n";
-    
-    // 充电状态
-    status_report += "  - 充电状态: " + chargingStateToString(recharge_controller_.getChargingState()) + "\n";
-    
-    // 回充状态
-    if (current_state_ == WheelchairState::SEARCHING)
-    {
-        status_report += "  - 回充状态: 正在搜索充电站\n";
-    }
-    else if (current_state_ == WheelchairState::CHARGING)
-    {
-        status_report += "  - 回充状态: 正在充电\n";
-    }
-    else
-    {
-        status_report += "  - 回充状态: 待命\n";
-    }
-    
-    // 控制模式
-    status_report += " - 控制模式: ";
-    status_report += (interface_active_ ? "iPad远程控制" : "本地控制");
-    status_report += "\n";
-    
-    // 充电电压检测
-    status_report += "  - 充电电压: " + std::string(getChargingVoltage() ? "检测到" : "未检测") + "\n";
-    
-    // 安全状态
-    status_report += "  - 安全状态: 正常\n";
-    
-    RCLCPP_INFO(this->get_logger(), "\n%s", status_report.c_str());
-    
-    // 发布到状态话题
-    auto status_msg = std_msgs::msg::String();
-    status_msg.data = status_report;
-    state_publisher_->publish(status_msg);
-}
-
-void WheelchairStateMachine::publishStateUpdate(const std::string &state_str, const std::string &message)
-{
-    auto msg = std_msgs::msg::String();
-    if (message.empty())
-    {
-        msg.data = state_str;
-    }
-    else
-    {
-        msg.data = state_str + ":" + message;
-    }
-    state_publisher_->publish(msg);
-}
-
-void WheelchairStateMachine::setMode(const std::string &mode)
-{
-    last_mode_ = mode;
-    if (last_mode_ != "recharge")
-    {
-        recharge_controller_.setChargingVoltage(false);
-    }
-}
-
-std::string WheelchairStateMachine::getMode() const
-{
-    return last_mode_;
-}
-
-void WheelchairStateMachine::setChargingVoltage(bool value)
-{
-    recharge_controller_.setChargingVoltage(value);
-}
-
-// ==================== 回充控制接口 ====================
-
-void WheelchairStateMachine::startRecharge()
-{
-    if (current_state_ == WheelchairState::READY)
-    {
-        handleIpadPhoneStart();
-    }
-    else
-    {
-        RCLCPP_WARN(this->get_logger(), "当前状态无法启动回充: %s", getCurrentState().c_str());
-    }
-}
-
-void WheelchairStateMachine::cancelRecharge()
-{
-    handleIpadPhoneCancel();
-}
-bool WheelchairStateMachine::isRechargeActive() const
-{
-    return recharge_controller_.isActive() || ipad_controller_.isRechargeActive();
-}

src3/wheelchair_state_machine/CMakeLists.txt

@@ -1,73 +1,68 @@
-cmake_minimum_required(VERSION 3.16)
+cmake_minimum_required(VERSION 3.5)
 project(wheelchair_state_machine)
 
-# 默认使用C++17
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
+# 默认使用 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(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
-find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(interface REQUIRED)
-
-# 包含当前目录
-include_directories(
-  ${CMAKE_CURRENT_SOURCE_DIR}/include
-  ${Eigen3_INCLUDE_DIRS}
-)
 
-# 创建可执行文件
-add_executable(wheelchair_state_machine
+# 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/report.cpp
-  src/main.cpp
   src/recharge_tool.cpp
-  src/ipad_manager.cpp
-  src/battery_manager.cpp
+  src/report.cpp
+)
+
+# 设置头文件路径
+target_include_directories(wheelchair_state_machine_node PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${Eigen3_INCLUDE_DIRS}
 )
 
-# 使用 ament_target_dependencies 自动处理ROS2依赖
-ament_target_dependencies(wheelchair_state_machine
+# 设置 ROS2 依赖
+ament_target_dependencies(wheelchair_state_machine_node
   rclcpp
   std_msgs
-  geometry_msgs
   sensor_msgs
+  geometry_msgs
   visualization_msgs
-  tf2
   tf2_geometry_msgs
-  interface  # 关键：这里添加interface依赖
 )
 
-# 链接Eigen3库（不是ROS2包，需要单独处理）
-target_include_directories(wheelchair_state_machine PRIVATE
-  ${Eigen3_INCLUDE_DIRS}
-)
-target_link_libraries(wheelchair_state_machine
-  Eigen3::Eigen
+# 2. 测试错误发布器
+add_executable(test_error_publisher
+  src/error_code_publisher.cpp
 )
 
-# 添加自定义头文件目录
-target_include_directories(wheelchair_state_machine PRIVATE
-  include
+ament_target_dependencies(test_error_publisher
+  rclcpp
+  std_msgs
 )
 
-# 安装可执行文件
-install(TARGETS wheelchair_state_machine
+# 安装规则
+install(TARGETS
+  wheelchair_state_machine_node
+  test_error_publisher
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-# 安装头文件（如果有需要）
 install(DIRECTORY include/
-  DESTINATION include/${PROJECT_NAME}
+  DESTINATION include/
 )
 
 ament_package()

src3/wheelchair_state_machine/include/wheelchair_state_machine/error_code.hpp

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

src3/wheelchair_state_machine/include/wheelchair_state_machine/event_input.hpp

@@ -14,24 +14,25 @@
 #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);
-    
+    using EventCallback = std::function<void(const std::string &)>;
+
+    EventInputHandler(rclcpp::Node *node);
+
     // 初始化接口
     void initializeModules();
-    
+
     // 事件注册接口
-    void registerEvent(const std::string& event_name, EventCallback callback);
-    
+    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 triggerEvent(const std::string &event_name);
+    void triggerEvent(const std::string &event_name, const std::string &data);
+
     // 特定事件触发
     void triggerIpadStart();
     void triggerIpadCancel();
@@ -51,39 +52,44 @@ public:
     void triggerErrorCode(int error_code);
     void triggerBaseStationLost();
     void triggerSearchTimeout();
-    
+
     // 系统状态查询
     std::string getCurrentState() const;
     bool isRechargeActive() const;
-    
+
     // 系统状态显示
     void displaySystemStatus();
-    
+
 private:
-    rclcpp::Node* node_;
-    
+    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::Bool>::SharedPtr cmd_enable_sub_;
-    
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr recharge_status_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 = "");
+    void logEvent(const std::string &event_name, const std::string &details = "");
+
+    // 错误处理相关
+    void handleCriticalError(const ErrorInfo &error_info);
+    void handleErrorRecovery(const ErrorInfo &error_info);
 };
 
 #endif // EVENT_INPUT_HANDLER_HPP

src3/wheelchair_state_machine/include/wheelchair_state_machine/report.hpp

@@ -6,16 +6,25 @@
 #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&)>;
-    
-    ReportManager(rclcpp::Node* node);
-    
+    using StatusReportCallback = std::function<void(const std::string &)>;
+    using ErrorCallback = std::function<void(const ErrorInfo &)>;
+
+    ReportManager(rclcpp::Node *node);
+
+    // 错误管理初始化
+    void initializeErrorManager();
+
     // 报告生成接口
     std::string generateSystemStatusReport(
         WheelchairState wheelchair_state,
@@ -24,11 +33,11 @@ public:
         ChargingState charging_state,
         bool charging_voltage_detected,
         bool recharge_active,
-        const std::string& control_mode);
-    
+        const std::string &control_mode);
+
     // 状态报告发布
-    void publishStatusReport(const std::string& report);
-    
+    void publishStatusReport(const std::string &report);
+
     // 特定状态报告
     void reportSystemStatus(
         WheelchairState wheelchair_state,
@@ -37,29 +46,71 @@ public:
         ChargingState charging_state,
         bool charging_voltage_detected,
         bool recharge_active,
-        const std::string& control_mode);
-    
+        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 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::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 logReport(const std::string& category, const std::string& message);
+
+    // 错误处理函数
+    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

src3/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -8,33 +8,36 @@
 #include <memory>
 #include "wheelchair_types.hpp"
 #include <std_msgs/msg/string.hpp>
+#include <std_msgs/msg/u_int32.hpp>
 #include <rclcpp/timer.hpp>
-
+#include "error_code.hpp"
 class WorkflowManager
 {
 public:
-    using StateUpdateCallback = std::function<void(const std::string&, const std::string&)>;
-    
-    WorkflowManager(rclcpp::Node* node);
-    
+    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;
     void setState(WheelchairState new_state);
-    
+
     // 状态转移控制
     void transitionToWalking();
     void transitionToSearching();
     void transitionToCharging();
     void transitionToReady();
-    
+
     // 回调设置
     void setStateUpdateCallback(StateUpdateCallback callback);
-    
+    void setRechargeStatusCallback(StateUpdateCallback callback);
+    void setRechargeErrorCallback(std::function<void(const ErrorInfo &)> callback);
+
     // 定时器管理
     void startSearchTimeoutTimer();
     void stopSearchTimeoutTimer();
-    
+
     // 事件处理接口
     void handleLowBatteryEvent(BatteryState state, float percentage);
     void handleChargingStartEvent();
@@ -42,35 +45,43 @@ public:
     void handleChargingFullEvent();
     void handleBaseStationLostEvent();
     void handleSearchTimeoutEvent();
-    
+    void handleRechargeFailure(const std::string &reason);
+
     // 查询接口
     WheelchairState getCurrentWheelchairState() const { return current_state_; }
-    
+
 private:
-    rclcpp::Node* node_;
-    
+    rclcpp::Node *node_;
+
     // 状态机状态
     WheelchairState current_state_;
     std::map<std::string, std::map<WheelchairState, bool>> transition_table_;
-    
+
     // ROS发布者
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr state_publisher_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr recharge_timeout_publisher_;
     rclcpp::TimerBase::SharedPtr search_timeout_timer_;
-    
+
     // 回调函数
     StateUpdateCallback state_update_callback_;
-    
+    StateUpdateCallback recharge_status_callback_;
+    std::function<void(const ErrorInfo &)> recharge_error_callback_;
+
     // 私有方法
     void initializeTransitionTable();
     bool checkEventPermission(const std::string &event);
     void executeEvent(const std::string &event);
-     void processStateTransition(const std::string& event);  
-    void triggerEvent(const std::string& event);            
+    void processStateTransition(const std::string &event);
+    void triggerEvent(const std::string &event);
     void publishState(const std::string &state_str, const std::string &message = "");
-    
+
+    // 回充状态管理
+    void publishRechargeStatus(const std::string &status, const std::string &message);
+    void publishRechargeErrorCode(uint32_t error_code);
+
     // 状态转换函数
     std::string stateToString(WheelchairState state) const;
-    
+
     // 具体事件处理函数
     void processIpadStartEvent();
     void processIpadCancelEvent();
@@ -84,9 +95,9 @@ private:
     void processPushStart();
     void processPushStop();
     void processErrorCode();
-    
+
     // 定时器回调
     void searchTimeoutCallback();
 };
 
-#endif // WORKFLOW_HPP
+#endif

src3/wheelchair_state_machine/src/error_code_publisher.cpp

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

src3/wheelchair_state_machine/src/event_input.cpp

@@ -1,16 +1,10 @@
 #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)
+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(), "事件输入处理器已初始化");
 }
@@ -20,23 +14,24 @@ 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_);
-    
+    report_manager_->initializeErrorManager(); // 初始化错误管理
+
     setupModuleCallbacks();
     setupSubscriptions();
-    
+
     RCLCPP_INFO(node_->get_logger(), "所有模块初始化完成");
 }
 
@@ -44,10 +39,11 @@ void EventInputHandler::setupModuleCallbacks()
 {
     // 1. 设置电池管理器回调
     battery_manager_->setBatteryStateCallback(
-        [this](BatteryState state, float percentage) {
+        [this](BatteryState state, float percentage)
+        {
             // 报告电池状态
             report_manager_->reportBatteryStatus(state, percentage);
-            
+
             // 触发电池状态事件
             if (state == BatteryState::CHARGING)
             {
@@ -58,32 +54,65 @@ void EventInputHandler::setupModuleCallbacks()
                 triggerEvent("battery_full");
             }
         });
-    
+
     battery_manager_->setLowBatteryCallback(
-        [this](BatteryState state, float percentage) {
+        [this](BatteryState state, float percentage)
+        {
             // 触发低电量事件
             triggerLowBattery(state, percentage);
         });
-    
+
     // 2. 设置iPad管理器回调 - 只处理开始和结束
     ipad_manager_->setRechargeStartCallback(
-        [this]() {
+        [this]()
+        {
             RCLCPP_INFO(node_->get_logger(), "iPad启动回充");
             triggerEvent("ipad_phone_interface_start");
         });
-    
+
     ipad_manager_->setRechargeCancelCallback(
-        [this]() {
+        [this]()
+        {
             RCLCPP_INFO(node_->get_logger(), "iPad取消回充");
             triggerEvent("ipad_phone_interface_cancel");
         });
-    
-    // 3. 设置工作流管理器回调
+
+    // 3. 设置报告管理器错误回调
+    report_manager_->setErrorCallback(
+        [this](const ErrorInfo &error_info)
+        {
+            // 所有错误都通过报告管理器反馈
+            std::stringstream msg;
+            msg << "系统错误: " << error_info.module_name
+                << " - " << error_info.description;
+            report_manager_->reportWarning(msg.str());
+
+            // 如果错误与回充相关，触发工作流事件
+            if (error_info.getModule() == ErrorModule::RECHARGE)
+            {
+                triggerEvent("error_code_handling", error_info.description);
+            }
+        });
+
+    report_manager_->setCriticalErrorCallback(
+        [this](const ErrorInfo &error_info)
+        {
+            handleCriticalError(error_info);
+        });
+
+    report_manager_->setRecoveryCallback(
+        [this](const ErrorInfo &error_info)
+        {
+            handleErrorRecovery(error_info);
+        });
+
+    // 4. 设置工作流管理器回调
     workflow_manager_->setStateUpdateCallback(
-        [this](const std::string& state_str, const std::string& message) {
+        [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)
@@ -100,116 +129,195 @@ void EventInputHandler::setupModuleCallbacks()
                 stopNavigationControl();
             }
         });
-    
-    // 4. 设置事件处理器回调 - 所有事件都转发给工作流管理器
-    registerEvent("ipad_phone_interface_start",
-        [this](const std::string& data) {
-            RCLCPP_INFO(node_->get_logger(), "处理iPad启动事件");
-            workflow_manager_->handleEvent("ipad_phone_interface_start");
+
+    // 5. 设置工作流管理器回充状态回调
+    workflow_manager_->setRechargeStatusCallback(
+        [this](const std::string &status, const std::string &message)
+        {
+            // 处理回充状态更新
+            RCLCPP_INFO(node_->get_logger(), "[回充状态] %s: %s", status.c_str(), message.c_str());
+
+            // 根据回充状态采取行动
+            if (status == "RECHARGE_FAILED")
+            {
+                // 回充失败，通过报告管理器反馈
+                std::string error_msg = "回充失败: " + message;
+                report_manager_->reportWarning(error_msg);
+
+                // 生成回充失败错误码
+                uint32_t error_code = 0x51010101; // 回充检测失败
+                report_manager_->processErrorCode(error_code);
+            }
+            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);
+            }
         });
-    
-    registerEvent("ipad_phone_interface_cancel",
-        [this](const std::string& data) {
-            RCLCPP_INFO(node_->get_logger(), "处理iPad取消事件");
-            workflow_manager_->handleEvent("ipad_phone_interface_cancel");
+
+    // 6. 设置工作流管理器回充错误回调
+    workflow_manager_->setRechargeErrorCallback(
+        [this](const ErrorInfo &error_info)
+        {
+            // 处理回充相关错误
+            RCLCPP_WARN(node_->get_logger(), "回充错误: %s - %s",
+                        error_info.module_name.c_str(),
+                        error_info.description.c_str());
+
+            // 通过报告管理器处理错误
+            report_manager_->processErrorCode(error_info);
+
+            // 如果错误严重，触发错误处理事件
+            if (error_info.isCritical())
+            {
+                triggerEvent("error_code_handling", error_info.description);
+            }
         });
-    
+
+    // 7. 设置事件处理器回调 - 所有事件都转发给工作流管理器
+    registerEvent("error_code_handling",
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理错误码事件: %s", data.c_str());
+                      workflow_manager_->handleEvent("error_code_handling");
+                  });
+
+    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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [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");
-        });
-    
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理推行停止事件");
+                      workflow_manager_->handleEvent("push_stop");
+                  });
+
     registerEvent("error_code_handling",
-        [this](const std::string& data) {
-            RCLCPP_INFO(node_->get_logger(), "处理错误码事件");
-            workflow_manager_->handleEvent("error_code_handling");
-        });
-    
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理错误码事件");
+                      workflow_manager_->handleEvent("error_code_handling");
+                  });
+
     registerEvent("base_station_lost",
-        [this](const std::string& data) {
-            RCLCPP_INFO(node_->get_logger(), "处理基站丢失事件");
-            workflow_manager_->handleEvent("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");
-        });
-    
+                  [this](const std::string &data)
+                  {
+                      RCLCPP_INFO(node_->get_logger(), "处理搜索超时事件");
+                      workflow_manager_->handleEvent("search_30s_timeout");
+                  });
+
     RCLCPP_INFO(node_->get_logger(), "模块回调设置完成");
 }
 
@@ -218,10 +326,26 @@ void EventInputHandler::setupSubscriptions()
     // 创建命令使能订阅者
     cmd_enable_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
         "/recharge/cmd_enable", 10,
-        [this](const std_msgs::msg::Bool::SharedPtr msg) {
+        [this](const std_msgs::msg::Bool::SharedPtr msg)
+        {
             this->ipad_manager_->onCmdEnable(msg);
         });
-    
+
+    // 创建回充状态订阅者（用于监控回充超时）
+    recharge_status_sub_ = node_->create_subscription<std_msgs::msg::String>(
+        "wheelchair/recharge_timeout", 10,
+        [this](const std_msgs::msg::String::SharedPtr msg)
+        {
+            RCLCPP_INFO(node_->get_logger(), "收到回充状态: %s", msg->data.c_str());
+
+            // 检查是否是回充失败消息
+            if (msg->data.find("RECHARGE_FAILED") != std::string::npos)
+            {
+                // 回充失败，记录到报告管理器
+                report_manager_->reportWarning("回充失败通知: " + msg->data);
+            }
+        });
+
     RCLCPP_INFO(node_->get_logger(), "订阅器设置完成");
 }
 
@@ -246,21 +370,21 @@ void EventInputHandler::stopNavigationControl()
     }
 }
 
-void EventInputHandler::registerEvent(const std::string& event_name, EventCallback callback)
+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)
+void EventInputHandler::triggerEvent(const std::string &event_name)
 {
     triggerEvent(event_name, "");
 }
 
-void EventInputHandler::triggerEvent(const std::string& event_name, const std::string& data)
+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);
@@ -349,12 +473,6 @@ void EventInputHandler::triggerBatteryFull()
     triggerEvent("battery_full", "电池充满");
 }
 
-void EventInputHandler::triggerErrorCode(int error_code)
-{
-    std::string data = "错误码: " + std::to_string(error_code);
-    triggerEvent("error_code_handling", data);
-}
-
 void EventInputHandler::triggerBaseStationLost()
 {
     triggerEvent("base_station_lost", "基站检测丢失");
@@ -365,7 +483,7 @@ void EventInputHandler::triggerSearchTimeout()
     triggerEvent("search_30s_timeout", "搜索30秒超时");
 }
 
-void EventInputHandler::logEvent(const std::string& event_name, const std::string& details)
+void EventInputHandler::logEvent(const std::string &event_name, const std::string &details)
 {
     if (details.empty())
     {
@@ -373,8 +491,8 @@ void EventInputHandler::logEvent(const std::string& event_name, const std::strin
     }
     else
     {
-        RCLCPP_INFO(node_->get_logger(), "触发事件: %s - %s", 
-                   event_name.c_str(), details.c_str());
+        RCLCPP_INFO(node_->get_logger(), "触发事件: %s - %s",
+                    event_name.c_str(), details.c_str());
     }
 }
 
@@ -397,9 +515,9 @@ void EventInputHandler::displaySystemStatus()
         RCLCPP_ERROR(node_->get_logger(), "系统未初始化");
         return;
     }
-    
+
     std::string control_mode = interface_active_ ? "iPad控制" : "本地控制";
-    
+
     report_manager_->reportSystemStatus(
         workflow_manager_->getCurrentWheelchairState(),
         battery_manager_->getBatteryState(),
@@ -407,6 +525,87 @@ void EventInputHandler::displaySystemStatus()
         lidar_controller_->getChargingState(),
         lidar_controller_->getChargingVoltage(),
         isRechargeActive(),
-        control_mode
-    );
+        control_mode);
+}
+
+void EventInputHandler::handleCriticalError(const ErrorInfo &error_info)
+{
+    RCLCPP_ERROR(node_->get_logger(), "检测到严重错误: %s - %s",
+                 error_info.module_name.c_str(),
+                 error_info.description.c_str());
+
+    // 通过报告管理器反馈严重错误
+    std::stringstream report;
+    report << "严重错误警报!\n";
+    report << "模块: " << error_info.module_name << "\n";
+    report << "子模块: " << error_info.sub_module_name << "\n";
+    report << "错误码: 0x" << std::hex << std::setw(8) << std::setfill('0')
+           << error_info.error_code << "\n";
+    report << "描述: " << error_info.description << "\n";
+    report << "建议: 立即检查相关硬件或联系技术支持";
+
+    report_manager_->reportError(report.str());
+
+    // 根据错误类型采取行动
+    ErrorModule module = error_info.getModule();
+
+    if (module == ErrorModule::LIDAR5)
+    { // 后雷达故障
+        RCLCPP_WARN(node_->get_logger(), "后雷达故障，停止回充功能");
+        if (isRechargeActive())
+        {
+            stopNavigationControl();
+            triggerEvent("error_code_handling", "雷达故障导致回充停止");
+        }
+    }
+    else if (module == ErrorModule::DRIVER)
+    { // 电机驱动器故障
+        RCLCPP_WARN(node_->get_logger(), "电机驱动器故障，停止所有运动");
+        lidar_controller_->stopMotion();
+        triggerEvent("error_code_handling", "驱动器故障");
+    }
+    else if (module == ErrorModule::RECHARGE)
+    { // 回充功能故障
+        RCLCPP_WARN(node_->get_logger(), "回充功能故障: %s",
+                    error_info.description.c_str());
+
+        // 如果是回充超时错误
+        if (error_info.error_code == 0x51010101)
+        {
+            RCLCPP_WARN(node_->get_logger(), "回充超时30秒，通知用户");
+            // 可以通过其他方式通知用户界面
+        }
+    }
+}
+
+void EventInputHandler::handleErrorRecovery(const ErrorInfo &error_info)
+{
+    RCLCPP_INFO(node_->get_logger(), "错误已恢复: %s - %s",
+                error_info.module_name.c_str(),
+                error_info.description.c_str());
+
+    // 报告错误恢复
+    std::stringstream report;
+    report << "错误恢复通知\n";
+    report << "模块: " << error_info.module_name << "\n";
+    report << "错误: " << error_info.description << "\n";
+    report << "状态: 已恢复正常";
+
+    report_manager_->reportInfo(report.str());
+
+    // 如果恢复了关键模块，可以重新启用相关功能
+    if (error_info.getModule() == ErrorModule::LIDAR5)
+    {
+        RCLCPP_INFO(node_->get_logger(), "后雷达已恢复，可以重新启用回充功能");
+    }
+}
+
+// 修改错误码触发函数
+void EventInputHandler::triggerErrorCode(int error_code)
+{
+    std::string data = "错误码: 0x" + std::to_string(static_cast<uint32_t>(error_code));
+    triggerEvent("error_code_handling", data);
+
+    // 同时传递给报告管理器处理
+    report_manager_->processErrorCode(static_cast<uint32_t>(error_code));
 }

src3/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -13,12 +13,8 @@
 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)
+LidarScanController::LidarScanController(rclcpp::Node *node)
+    : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0)
 {
 }
 
@@ -32,48 +28,49 @@ 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) {
+        [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);
-    
+
     // 初始化控制消息
     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();
-    
+
     RCLCPP_INFO(node_->get_logger(), "激光扫描控制器已初始化");
 }
 
@@ -103,35 +100,37 @@ void LidarScanController::stopMotion()
 
 void LidarScanController::navigationTimerCallback()
 {
-    if (!active_) return;
-    
+    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)
+void LidarScanController::processLaserScan(const LaserScanSharedPtr &msg)
 {
-    if (!active_) return;
-    
+    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))
     {
@@ -139,8 +138,8 @@ void LidarScanController::processLaserScan(const LaserScanSharedPtr& msg)
     }
 }
 
-bool LidarScanController::processSegmentData(const LaserScanSharedPtr& msg,
-                                            int begin_index, int end_index)
+bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
+                                             int begin_index, int end_index)
 {
     // 转换到XY坐标系
     auto xy_list = laserScanToXY1(begin_index, end_index, msg);
@@ -149,7 +148,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr& msg,
         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())
@@ -157,58 +156,58 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr& msg,
         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)
@@ -216,21 +215,21 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr& msg,
         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())
     {
         RCLCPP_DEBUG(node_->get_logger(), "未检测到有效的分段");
         return false;
     }
-    
+
     // 创建结果对象
     RechargeResult result;
     result.frameid = frame_index_++;
-    
+
     // 处理左右点
     int segment_index = 0;
     for (const auto &seg : filtered_segments)
@@ -238,16 +237,16 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr& msg,
         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);
@@ -258,18 +257,18 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr& msg,
             }
         }
         segment_index++;
-        
+
         if (segment_index >= 2)
             break; // 只需要两个分段
     }
-    
+
     // 如果成功获取左右点，进行控制
     if (!result.left_points.empty() && !result.right_points.empty())
     {
         procData(result);
         return true;
     }
-    
+
     return false;
 }
 
@@ -280,15 +279,15 @@ void LidarScanController::procData(RechargeResult &result)
         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)
     {
@@ -299,27 +298,27 @@ void LidarScanController::procData(RechargeResult &result)
         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);
 }
@@ -335,27 +334,27 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         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)
     {
@@ -365,11 +364,11 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
     {
         angular_velocity = -std::min(fabs(angular_velocity), 0.09);
     }
-    
+
     // 根据距离调整速度系数
     double radio_x = 1.0;
     double radio_z = 1.0;
-    
+
     if (dx > 1.0)
     {
         // 保持默认
@@ -402,9 +401,9 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         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)
     {
@@ -413,11 +412,11 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         {
             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);
     }
@@ -428,7 +427,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         ctrl_twist_.linear.x = 0.0;
         ctrl_twist_.angular.z = 0.0;
         ctrl_twist_time_ = node_->now();
-        
+
         // 模拟检测到充电电压
         setChargingVoltage(true);
     }
@@ -441,7 +440,7 @@ void LidarScanController::filterScanModify0(LaserScanSharedPtr msg)
     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++)
     {
@@ -456,7 +455,7 @@ 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)
@@ -473,7 +472,7 @@ void LidarScanController::filterScanModify2(LaserScanSharedPtr msg)
     {
         if (!isValid(msg->ranges[i]))
             continue;
-        
+
         if (msg->ranges[i] < param_.min_distance ||
             msg->ranges[i] > param_.max_distance)
         {
@@ -489,13 +488,13 @@ void LidarScanController::filterScanModify3(LaserScanSharedPtr msg)
     {
         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)
@@ -511,7 +510,7 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
     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++)
     {
@@ -523,13 +522,13 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
             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--)
     {
@@ -541,19 +540,19 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
             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)
@@ -596,14 +595,14 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
             }
         }
     }
-    
+
     // 倒序检查连续性
     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)
@@ -633,7 +632,7 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
             }
         }
     }
-    
+
     // 设置无效区域
     for (int i = param_.min_index; i < begin_index - 1; i++)
     {
@@ -643,21 +642,21 @@ std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr ms
     {
         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)
+    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)
         {
@@ -675,15 +674,15 @@ std::map<int, std::pair<double, double>> LidarScanController::laserScanToXY(
 
 std::map<int, std::pair<double, double>> LidarScanController::laserScanToXY1(
     int begin_index, int end_index,
-    const LaserScanSharedPtr& scan_msg)
+    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);
@@ -700,24 +699,24 @@ std::map<int, std::pair<double, double>> LidarScanController::laserScanToXY1(
 // ==================== 分段处理函数 ====================
 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)
+    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)
         {
@@ -726,11 +725,11 @@ std::vector<std::pair<int, int>> LidarScanController::getSections(
             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;
@@ -740,12 +739,12 @@ std::vector<std::pair<int, int>> LidarScanController::getSections(
             }
             section_start = i;
         }
-        
+
         last_index = i;
         last_x = curr.first;
         last_y = curr.second;
     }
-    
+
     if (last_index > section_end + 10)
     {
         section_end = last_index;
@@ -754,49 +753,49 @@ std::vector<std::pair<int, int>> LidarScanController::getSections(
             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)
+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)
+    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];
-        
+        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)
         {
@@ -812,49 +811,49 @@ LidarScanController::getSegments(const std::map<int, std::pair<double, double>>&
             );
         }
     }
-    
+
     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)
+    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)
+    for (const auto &seg : turn_segments)
     {
-        const auto& info = seg.second;
+        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)
                 {
                     // 添加到结果
@@ -864,22 +863,22 @@ LidarScanController::filterSegments(
                 }
             }
         }
-        
+
         // 更新上一个分段信息
         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)
+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";
@@ -892,19 +891,19 @@ void LidarScanController::publishRechargeVisualization(const RechargeResult& res
     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;
@@ -912,11 +911,11 @@ void LidarScanController::publishRechargeVisualization(const RechargeResult& res
     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";
@@ -930,33 +929,33 @@ void LidarScanController::publishRechargeVisualization(const RechargeResult& res
     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 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)
+    const LaserScanSharedPtr &original_msg,
+    const std::vector<float> &filtered_ranges)
 {
     auto msg = std::make_shared<sensor_msgs::msg::LaserScan>();
     msg->header = original_msg->header;
@@ -969,7 +968,7 @@ void LidarScanController::publishFilteredScan(
     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(), "已发布过滤后的激光数据");
 }
@@ -980,22 +979,22 @@ LidarScanController::LaserScanSharedPtr LidarScanController::resetLaserData(
 {
     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;
 }
 
@@ -1006,15 +1005,15 @@ bool LidarScanController::isValid(float value) const
     return !std::isinf(value) && !std::isnan(value);
 }
 
-int LidarScanController::getFixedIndex(const LaserScanSharedPtr& msg, int i) const
+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;
@@ -1038,7 +1037,7 @@ void LidarScanController::setChargingVoltage(bool value)
 {
     detect_charging_voltage_ = value;
     detect_charging_voltage_time_ = node_->now();
-    
+
     if (value)
     {
         RCLCPP_INFO(node_->get_logger(), "检测到充电电压");
@@ -1053,7 +1052,7 @@ 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)
     {

src3/wheelchair_state_machine/src/report.cpp

@@ -2,19 +2,38 @@
 #include "wheelchair_state_machine/report.hpp"
 #include <rclcpp/logging.hpp>
 #include <sstream>
-
+#include <iomanip>
 using namespace std::chrono_literals;
 
-ReportManager::ReportManager(rclcpp::Node* node)
+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(), "报告管理器已初始化");
 }
 
+void ReportManager::initializeErrorManager()
+{
+    // 订阅错误码topic
+    error_sub_ = node_->create_subscription<std_msgs::msg::UInt32>(
+        "/wheelchair/error_code", 10,
+        [this](const std_msgs::msg::UInt32::SharedPtr msg)
+        {
+            this->errorCodeCallback(msg);
+        });
+
+    RCLCPP_INFO(node_->get_logger(), "错误码订阅器已启动");
+}
+
 std::string ReportManager::generateSystemStatusReport(
     WheelchairState wheelchair_state,
     BatteryState battery_state,
@@ -22,44 +41,60 @@ std::string ReportManager::generateSystemStatusReport(
     ChargingState charging_state,
     bool charging_voltage_detected,
     bool recharge_active,
-    const std::string& control_mode)
+    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)
+void ReportManager::publishStatusReport(const std::string &report)
 {
     auto msg = std_msgs::msg::String();
     msg.data = report;
     report_publisher_->publish(msg);
-    
+
     // 调用回调函数
     if (status_report_callback_)
     {
@@ -74,13 +109,13 @@ void ReportManager::reportSystemStatus(
     ChargingState charging_state,
     bool charging_voltage_detected,
     bool recharge_active,
-    const std::string& control_mode)
+    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", "系统状态报告已生成");
 }
@@ -92,7 +127,7 @@ void ReportManager::reportBatteryStatus(BatteryState state, float percentage)
     report << "- 电量: " << percentage << "%\n";
     report << "- 状态: " << batteryStateToString(state) << "\n";
     report << "- 时间: " << node_->now().seconds() << "s";
-    
+
     publishStatusReport(report.str());
     logReport("BATTERY_STATUS", "电池状态报告已生成");
 }
@@ -103,55 +138,377 @@ void ReportManager::reportChargingStatus(ChargingState state)
     report << "充电状态报告:\n";
     report << "- 状态: " << chargingStateToString(state) << "\n";
     report << "- 时间: " << node_->now().seconds() << "s";
-    
+
     publishStatusReport(report.str());
     logReport("CHARGING_STATUS", "充电状态报告已生成");
 }
 
-void ReportManager::reportNavigationStatus(const std::string& 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)
+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)
+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)
+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)
+{
+    ErrorInfo error_info = parseErrorCode(error_code);
+    processErrorCode(error_info);
+}
+
+void ReportManager::processErrorCode(const ErrorInfo &error_info)
+{
+    std::lock_guard<std::mutex> lock(error_mutex_);
+
+    // 检查是否为新错误
+    if (active_errors_.find(error_info.error_code) == active_errors_.end())
+    {
+        // 新错误
+        handleNewError(error_info);
+    }
+    else
+    {
+        // 已有错误，更新时间戳
+        active_errors_[error_info.error_code].timestamp = error_info.timestamp;
+    }
+}
+
+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
 {
@@ -208,12 +565,7 @@ std::string ReportManager::chargingStateToString(ChargingState state) const
     }
 }
 
-void ReportManager::setStatusReportCallback(StatusReportCallback callback)
-{
-    status_report_callback_ = callback;
-}
-
-void ReportManager::logReport(const std::string& category, const std::string& message)
+void ReportManager::logReport(const std::string &category, const std::string &message)
 {
     RCLCPP_INFO(node_->get_logger(), "[%s] %s", category.c_str(), message.c_str());
 }

src3/wheelchair_state_machine/src/workflow.cpp

@@ -6,24 +6,27 @@
 using namespace std::chrono_literals;
 using std::placeholders::_1;
 
-WorkflowManager::WorkflowManager(rclcpp::Node* node)
-    : node_(node)
-    , current_state_(WheelchairState::READY)
+WorkflowManager::WorkflowManager(rclcpp::Node *node)
+    : node_(node), current_state_(WheelchairState::READY)
 {
     // 初始化状态转移表
     initializeTransitionTable();
-    
+
     // 创建状态发布者
     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::initializeTransitionTable()
 {
     transition_table_.clear();
-    
+
     // 1. ipad&phone界面启动: 就绪中✅, 其他❌
     std::map<WheelchairState, bool> ipad_start_perms = {
         {WheelchairState::READY, true},
@@ -31,7 +34,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -39,7 +42,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -47,7 +50,7 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, true}};
     transition_table_["bluetooth_disconnected"] = bluetooth_disconnect_perms;
-    
+
     // 4. 蓝牙已连接: 就绪中✅, 其他❌
     std::map<WheelchairState, bool> bluetooth_connect_perms = {
         {WheelchairState::READY, true},
@@ -55,7 +58,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -63,7 +66,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -71,7 +74,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -79,7 +82,7 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["lock_vehicle"] = lock_vehicle_perms;
-    
+
     // 8. 解锁: 行走中✅, 其他❌
     std::map<WheelchairState, bool> unlock_vehicle_perms = {
         {WheelchairState::READY, false},
@@ -87,7 +90,7 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, false}};
     transition_table_["unlock_vehicle"] = unlock_vehicle_perms;
-    
+
     // 9. 摇杆后拉: 搜索中✅, 其他❌
     std::map<WheelchairState, bool> joystick_pull_perms = {
         {WheelchairState::READY, false},
@@ -95,7 +98,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -103,7 +106,7 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, false}};
     transition_table_["joystick_stop"] = joystick_stop_perms;
-    
+
     // 11. 推行启动: 充电中✅, 其他❌
     std::map<WheelchairState, bool> push_start_perms = {
         {WheelchairState::READY, false},
@@ -111,7 +114,7 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, true}};
     transition_table_["push_start"] = push_start_perms;
-    
+
     // 12. 推行关闭: 行走中✅, 搜索中✅, 其他❌
     std::map<WheelchairState, bool> push_stop_perms = {
         {WheelchairState::READY, false},
@@ -119,7 +122,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -127,7 +130,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -135,7 +138,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -143,7 +146,7 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::SEARCHING, false},
         {WheelchairState::CHARGING, true}};
     transition_table_["battery_full"] = battery_full_perms;
-    
+
     // 16. 错误码处理: 所有状态✅
     std::map<WheelchairState, bool> error_code_perms = {
         {WheelchairState::READY, true},
@@ -151,7 +154,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -159,7 +162,7 @@ void WorkflowManager::initializeTransitionTable()
         {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},
@@ -167,7 +170,7 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, false}};
     transition_table_["search_30s_timeout"] = search_timeout_perms;
-    
+
     RCLCPP_INFO(node_->get_logger(), "状态转移表已初始化完成");
 }
 
@@ -181,7 +184,7 @@ bool WorkflowManager::handleEvent(const std::string &event)
                     getCurrentState().c_str());
         return false;
     }
-    
+
     executeEvent(event);
     return true;
 }
@@ -193,21 +196,21 @@ bool WorkflowManager::checkEventPermission(const std::string &event)
         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")
     {
@@ -279,12 +282,13 @@ void WorkflowManager::executeEvent(const std::string &event)
     }
     else if (event == "search_30s_timeout")
     {
-        // 由外部回调处理
+        // 搜索超时事件
+        handleSearchTimeoutEvent();
     }
-    
+
     // 根据事件触发状态转移
     processStateTransition(event);
-    
+
     // 发布状态更新
     publishState(event, "事件已处理");
 }
@@ -304,7 +308,7 @@ void WorkflowManager::processStateTransition(const std::string &event)
             transitionToWalking();
         }
         break;
-        
+
     case WheelchairState::WALKING:
         if (event == "joystick_stop" || event == "push_stop" || event == "lock_vehicle")
         {
@@ -319,9 +323,9 @@ void WorkflowManager::processStateTransition(const std::string &event)
             transitionToCharging();
         }
         break;
-        
+
     case WheelchairState::SEARCHING:
-        if (event == "search_30s_timeout" || event == "base_station_lost")
+        if (event == "base_station_lost")
         {
             transitionToReady();
         }
@@ -333,8 +337,15 @@ void WorkflowManager::processStateTransition(const std::string &event)
         {
             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" ||
@@ -371,18 +382,24 @@ void WorkflowManager::transitionToSearching()
         RCLCPP_INFO(node_->get_logger(), "开始搜索充电站");
         publishState("SEARCH_CHARGING_STATION_START");
         startSearchTimeoutTimer();
+
+        // 发布回充开始通知
+        publishRechargeStatus("RECHARGE_STARTED", "回充功能已启动，正在搜索充电站");
     }
 }
 
 void WorkflowManager::transitionToCharging()
 {
-    if (current_state_ == WheelchairState::SEARCHING || 
+    if (current_state_ == WheelchairState::SEARCHING ||
         current_state_ == WheelchairState::WALKING)
     {
         setState(WheelchairState::CHARGING);
         RCLCPP_INFO(node_->get_logger(), "开始充电");
         stopSearchTimeoutTimer();
         publishState("CHARGING_START");
+
+        // 发布回充成功通知
+        publishRechargeStatus("RECHARGE_SUCCESS", "成功对接充电站，开始充电");
     }
 }
 
@@ -391,7 +408,7 @@ void WorkflowManager::transitionToReady()
     std::string previous_state = getCurrentState();
     setState(WheelchairState::READY);
     stopSearchTimeoutTimer();
-    RCLCPP_INFO(node_->get_logger(), "返回到就绪状态 (前状态: %s)", 
+    RCLCPP_INFO(node_->get_logger(), "返回到就绪状态 (前状态: %s)",
                 previous_state.c_str());
     publishState("WHEELCHAIR_READY");
 }
@@ -401,10 +418,10 @@ void WorkflowManager::transitionToReady()
 void WorkflowManager::processIpadStartEvent()
 {
     RCLCPP_INFO(node_->get_logger(), "处理iPad启动事件");
-    
+
     // 发布状态更新
     publishState("IPAD_RECHARGE_STARTED", "iPad启动自主回充");
-    
+
     // 如果当前是就绪状态，自动转移到搜索状态
     if (current_state_ == WheelchairState::READY)
     {
@@ -412,34 +429,37 @@ void WorkflowManager::processIpadStartEvent()
     }
     else
     {
-        RCLCPP_WARN(node_->get_logger(), 
-                   "当前状态 %s 无法启动回充", getCurrentState().c_str());
+        RCLCPP_WARN(node_->get_logger(),
+                    "当前状态 %s 无法启动回充", getCurrentState().c_str());
     }
 }
 
 void WorkflowManager::processIpadCancelEvent()
 {
     RCLCPP_INFO(node_->get_logger(), "处理iPad取消事件");
-    
+
     // 发布状态更新
     publishState("IPAD_RECHARGE_CANCELLED", "iPad取消自主回充");
-    
+
     // 根据当前状态决定是否取消回充
-    if (current_state_ == WheelchairState::SEARCHING || 
+    if (current_state_ == WheelchairState::SEARCHING ||
         current_state_ == WheelchairState::CHARGING)
     {
         transitionToReady();
         RCLCPP_INFO(node_->get_logger(), "iPad自主回充已取消，返回到就绪状态");
+
+        // 发布回充取消通知
+        publishRechargeStatus("RECHARGE_CANCELLED", "用户取消回充");
     }
 }
 
 void WorkflowManager::processBluetoothDisconnected()
 {
     RCLCPP_INFO(node_->get_logger(), "处理蓝牙断开事件");
-    
+
     // 发布状态更新
     publishState("BLUETOOTH_DISCONNECTED", "蓝牙连接已断开");
-    
+
     // 根据状态处理蓝牙断开
     if (current_state_ == WheelchairState::WALKING ||
         current_state_ == WheelchairState::SEARCHING ||
@@ -447,16 +467,22 @@ void WorkflowManager::processBluetoothDisconnected()
     {
         RCLCPP_WARN(node_->get_logger(), "蓝牙断开，返回到就绪状态");
         transitionToReady();
+
+        // 如果正在回充，发布回充失败通知
+        if (current_state_ == WheelchairState::SEARCHING)
+        {
+            publishRechargeStatus("RECHARGE_FAILED", "蓝牙断开导致回充失败");
+        }
     }
 }
 
 void WorkflowManager::processBluetoothConnected()
 {
     RCLCPP_INFO(node_->get_logger(), "处理蓝牙连接成功事件");
-    
+
     // 发布状态更新
     publishState("BLUETOOTH_CONNECTED", "蓝牙连接成功");
-    
+
     // 蓝牙连接成功通常不需要改变当前状态
     // 但可以根据业务需求添加逻辑
 }
@@ -464,27 +490,33 @@ void WorkflowManager::processBluetoothConnected()
 void WorkflowManager::processBaseStationPowerOff()
 {
     RCLCPP_INFO(node_->get_logger(), "处理基站断电事件");
-    
+
     // 发布状态更新
     publishState("BASE_STATION_POWER_OFF", "充电基站断电");
-    
+
     // 根据状态处理基站断电
     if (current_state_ == WheelchairState::SEARCHING)
     {
         RCLCPP_WARN(node_->get_logger(), "基站断电，停止搜索");
         transitionToReady();
+
+        // 发布回充失败通知
+        publishRechargeStatus("RECHARGE_FAILED", "基站断电导致回充失败");
     }
     else if (current_state_ == WheelchairState::CHARGING)
     {
         RCLCPP_ERROR(node_->get_logger(), "充电中基站断电!");
         transitionToReady();
+
+        // 发布充电中断通知
+        publishRechargeStatus("CHARGING_INTERRUPTED", "基站断电导致充电中断");
     }
 }
 
 void WorkflowManager::handleLowBatteryEvent(BatteryState state, float percentage)
 {
     RCLCPP_INFO(node_->get_logger(), "处理低电量事件: %.1f%%", percentage);
-    
+
     // 发布相应的事件名称
     if (state == BatteryState::CRITICAL)
     {
@@ -494,13 +526,13 @@ void WorkflowManager::handleLowBatteryEvent(BatteryState state, float percentage
     {
         triggerEvent("low_battery_warning");
     }
-    
+
     // 根据电池状态采取行动
     if (state == BatteryState::CRITICAL)
     {
         RCLCPP_ERROR(node_->get_logger(), "严重低电量! 自动启动回充搜索");
         publishState("CRITICAL_BATTERY_EMERGENCY", "自动启动回充");
-        
+
         // 紧急情况下，无论当前状态如何都尝试启动回充
         if (current_state_ == WheelchairState::READY)
         {
@@ -512,7 +544,6 @@ void WorkflowManager::handleLowBatteryEvent(BatteryState state, float percentage
             RCLCPP_WARN(node_->get_logger(), "严重低电量，停止行走并启动回充搜索");
             transitionToReady();
             // 短暂延迟后启动搜索
-            // 注意：在实际应用中可能需要异步处理
             transitionToSearching();
         }
     }
@@ -520,7 +551,7 @@ void WorkflowManager::handleLowBatteryEvent(BatteryState state, float percentage
     {
         RCLCPP_WARN(node_->get_logger(), "低电量警告，建议启动回充");
         publishState("LOW_BATTERY_WARNING", "建议启动回充");
-        
+
         // 如果当前是就绪状态，自动启动回充
         if (current_state_ == WheelchairState::READY)
         {
@@ -533,10 +564,10 @@ void WorkflowManager::handleLowBatteryEvent(BatteryState state, float percentage
 void WorkflowManager::processLockVehicle()
 {
     RCLCPP_INFO(node_->get_logger(), "处理锁车操作");
-    
+
     // 发布状态更新
     publishState("VEHICLE_LOCKED", "轮椅已锁定");
-    
+
     // 根据状态处理锁车
     if (current_state_ == WheelchairState::WALKING)
     {
@@ -547,6 +578,9 @@ void WorkflowManager::processLockVehicle()
     {
         RCLCPP_WARN(node_->get_logger(), "搜索中锁车，取消搜索");
         transitionToReady();
+
+        // 发布回充取消通知
+        publishRechargeStatus("RECHARGE_CANCELLED", "锁车操作取消回充");
     }
     else if (current_state_ == WheelchairState::CHARGING)
     {
@@ -558,10 +592,10 @@ void WorkflowManager::processLockVehicle()
 void WorkflowManager::processUnlockVehicle()
 {
     RCLCPP_INFO(node_->get_logger(), "处理解锁操作");
-    
+
     // 发布状态更新
     publishState("VEHICLE_UNLOCKED", "轮椅已解锁");
-    
+
     // 根据状态处理解锁
     if (current_state_ == WheelchairState::READY)
     {
@@ -573,25 +607,28 @@ void WorkflowManager::processUnlockVehicle()
 void WorkflowManager::processJoystickPullBack()
 {
     RCLCPP_INFO(node_->get_logger(), "处理摇杆后拉操作");
-    
+
     // 发布状态更新
     publishState("JOYSTICK_PULL_BACK", "摇杆后拉");
-    
+
     // 根据状态处理摇杆后拉
     if (current_state_ == WheelchairState::SEARCHING)
     {
         RCLCPP_INFO(node_->get_logger(), "搜索中摇杆后拉，切换为行走模式");
         transitionToWalking();
+
+        // 发布回充取消通知
+        publishRechargeStatus("RECHARGE_CANCELLED", "用户手动中断回充");
     }
 }
 
 void WorkflowManager::processJoystickStop()
 {
     RCLCPP_INFO(node_->get_logger(), "处理摇杆停止操作");
-    
+
     // 发布状态更新
     publishState("JOYSTICK_STOP", "摇杆停止");
-    
+
     // 根据状态处理摇杆停止
     if (current_state_ == WheelchairState::WALKING)
     {
@@ -602,31 +639,37 @@ void WorkflowManager::processJoystickStop()
     {
         RCLCPP_INFO(node_->get_logger(), "搜索中摇杆停止，返回就绪状态");
         transitionToReady();
+
+        // 发布回充取消通知
+        publishRechargeStatus("RECHARGE_CANCELLED", "用户停止操作取消回充");
     }
 }
 
 void WorkflowManager::processPushStart()
 {
     RCLCPP_INFO(node_->get_logger(), "处理推行启动");
-    
+
     // 发布状态更新
     publishState("PUSH_START", "推行模式启动");
-    
+
     // 根据状态处理推行启动
     if (current_state_ == WheelchairState::CHARGING)
     {
         RCLCPP_INFO(node_->get_logger(), "充电中启动推行，切换到行走模式");
         transitionToWalking();
+
+        // 发布充电中断通知
+        publishRechargeStatus("CHARGING_INTERRUPTED", "推行操作中断充电");
     }
 }
 
 void WorkflowManager::processPushStop()
 {
     RCLCPP_INFO(node_->get_logger(), "处理推行停止");
-    
+
     // 发布状态更新
     publishState("PUSH_STOP", "推行模式停止");
-    
+
     // 根据状态处理推行停止
     if (current_state_ == WheelchairState::WALKING)
     {
@@ -643,13 +686,13 @@ void WorkflowManager::processPushStop()
 void WorkflowManager::handleChargingStartEvent()
 {
     RCLCPP_INFO(node_->get_logger(), "处理充电开始事件");
-    
+
     // 触发相应事件
     triggerEvent("battery_start_charging");
-    
+
     // 发布状态更新
     publishState("BATTERY_START_CHARGING", "开始充电");
-    
+
     // 如果当前在搜索状态，转移到充电状态
     if (current_state_ == WheelchairState::SEARCHING)
     {
@@ -665,66 +708,74 @@ void WorkflowManager::handleChargingStartEvent()
 void WorkflowManager::handleChargingStopEvent()
 {
     RCLCPP_INFO(node_->get_logger(), "处理充电停止事件");
-    
+
     // 触发相应事件
     triggerEvent("battery_stop_charging");
-    
+
     // 发布状态更新
     publishState("BATTERY_STOP_CHARGING", "停止充电");
-    
+
     // 如果当前在充电状态，返回就绪状态
     if (current_state_ == WheelchairState::CHARGING)
     {
         RCLCPP_WARN(node_->get_logger(), "充电停止，返回就绪状态");
         transitionToReady();
+
+        // 发布充电停止通知
+        publishRechargeStatus("CHARGING_STOPPED", "充电已停止");
     }
 }
 
 void WorkflowManager::handleChargingFullEvent()
 {
     RCLCPP_INFO(node_->get_logger(), "处理电池充满事件");
-    
+
     // 触发相应事件
     triggerEvent("battery_full");
-    
+
     // 发布状态更新
     publishState("BATTERY_FULL", "电池已充满");
-    
+
     // 停止充电，返回就绪状态
     if (current_state_ == WheelchairState::CHARGING)
     {
         transitionToReady();
+
+        // 发布充电完成通知
+        publishRechargeStatus("CHARGING_COMPLETED", "电池已充满，充电完成");
     }
 }
 
 void WorkflowManager::processErrorCode()
 {
     RCLCPP_INFO(node_->get_logger(), "处理错误码");
-    
+
     // 发布状态更新
     publishState("ERROR_CODE_HANDLING", "处理系统错误码");
-    
+
     // 根据错误码的严重程度处理
-    // 严重错误可能需要紧急停止并返回就绪状态
     bool critical_error = true; // 示例：假设是严重错误
-    
+
     if (critical_error)
     {
         RCLCPP_ERROR(node_->get_logger(), "检测到严重错误，紧急停止所有操作");
         transitionToReady();
+
+        // 发布回充失败通知
+        publishRechargeStatus("RECHARGE_FAILED", "系统错误导致回充失败");
     }
 }
 
 void WorkflowManager::handleBaseStationLostEvent()
 {
     RCLCPP_INFO(node_->get_logger(), "处理基站丢失事件");
-    
+
     // 触发相应事件
     triggerEvent("base_station_lost");
-    
+
     // 发布状态更新
     publishState("CHARGING_STATION_LOST", "充电基站丢失");
-    
+
     // 根据状态处理基站丢失
     if (current_state_ == WheelchairState::WALKING)
     {
@@ -734,29 +785,93 @@ void WorkflowManager::handleBaseStationLostEvent()
     else if (current_state_ == WheelchairState::SEARCHING)
     {
         // 搜索中检测到基站丢失，返回就绪状态
+        RCLCPP_WARN(node_->get_logger(), "搜索中基站丢失，返回就绪状态");
         transitionToReady();
+
+        // 发布回充失败通知
+        publishRechargeStatus("RECHARGE_FAILED", "基站丢失导致回充失败");
     }
     else if (current_state_ == WheelchairState::CHARGING)
     {
         RCLCPP_ERROR(node_->get_logger(), "充电中基站丢失，停止充电");
         transitionToReady();
+
+        // 发布充电中断通知
+        publishRechargeStatus("CHARGING_INTERRUPTED", "基站丢失导致充电中断");
     }
 }
 
 void WorkflowManager::handleSearchTimeoutEvent()
 {
     RCLCPP_INFO(node_->get_logger(), "处理搜索超时事件");
-    
-    // 触发相应事件
-    triggerEvent("search_30s_timeout");
-    
-    // 发布超时通知
-    publishState("SEARCH_CHARGING_STATION_TIMEOUT", "搜索充电站超时");
-    
-    // 根据状态转移表，搜索超时应返回就绪状态
-    if (current_state_ == WheelchairState::SEARCHING)
+
+    // 搜索超时30秒视为回充失败
+    RCLCPP_WARN(node_->get_logger(), "搜索30秒超时，回充失败");
+
+    // 发布回充失败通知
+    publishRechargeStatus("RECHARGE_FAILED", "搜索充电站30秒超时");
+
+    // 生成回充失败错误码
+    publishRechargeErrorCode(0x51010101); // 回充检测失败（超时）
+
+    // 触发基站丢失事件（保持向后兼容）
+    handleBaseStationLostEvent();
+
+    RCLCPP_INFO(node_->get_logger(), "搜索30秒超时，已作为回充失败事件处理");
+}
+
+void WorkflowManager::handleRechargeFailure(const std::string &reason)
+{
+    RCLCPP_WARN(node_->get_logger(), "回充失败: %s", reason.c_str());
+
+    // 发布回充失败通知
+    publishRechargeStatus("RECHARGE_FAILED", "回充失败: " + reason);
+
+    // 生成回充失败错误码
+    publishRechargeErrorCode(0x51010101); // 回充检测失败
+
+    // 返回就绪状态
+    transitionToReady();
+}
+
+// ==================== 回充状态管理 ====================
+
+void WorkflowManager::publishRechargeStatus(const std::string &status, const std::string &message)
+{
+    // 发布到专用话题
+    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_)
     {
-        transitionToReady();
+        recharge_status_callback_(status, message);
+    }
+}
+
+void WorkflowManager::publishRechargeErrorCode(uint32_t error_code)
+{
+    // 创建错误码消息
+    auto error_msg = std_msgs::msg::UInt32();
+    error_msg.data = error_code;
+
+    // 发布到错误码话题
+    // 注意：这里需要外部提供错误码发布者，或者通过回调传递
+
+    RCLCPP_WARN(node_->get_logger(), "生成回充错误码: 0x%08X", error_code);
+
+    // 调用外部错误处理回调（如果有）
+    if (recharge_error_callback_)
+    {
+        ErrorInfo error_info;
+        error_info.error_code = error_code;
+        error_info.description = "回充功能失败";
+        error_info.timestamp = node_->now().nanoseconds() / 1000000;
+        recharge_error_callback_(error_info);
     }
 }
 
@@ -769,7 +884,7 @@ void WorkflowManager::startSearchTimeoutTimer()
     {
         search_timeout_timer_->cancel();
     }
-    
+
     // 创建30秒定时器
     search_timeout_timer_ = node_->create_wall_timer(
         std::chrono::seconds(30),
@@ -777,8 +892,8 @@ void WorkflowManager::startSearchTimeoutTimer()
         {
             this->searchTimeoutCallback();
         });
-    
-    RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已启动");
+
+    RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已启动 (30秒)");
 }
 
 void WorkflowManager::stopSearchTimeoutTimer()
@@ -795,9 +910,14 @@ void WorkflowManager::searchTimeoutCallback()
 {
     if (current_state_ == WheelchairState::SEARCHING)
     {
-        RCLCPP_WARN(node_->get_logger(), "搜索充电站超时30秒");
+        RCLCPP_WARN(node_->get_logger(), "搜索充电站超时30秒，回充失败");
         handleSearchTimeoutEvent();
     }
+    else
+    {
+        RCLCPP_DEBUG(node_->get_logger(), "搜索超时定时器触发，但当前不在搜索状态");
+        stopSearchTimeoutTimer();
+    }
 }
 
 // ==================== 辅助函数 ====================
@@ -811,17 +931,17 @@ void WorkflowManager::setState(WheelchairState new_state)
 {
     std::string old_state = getCurrentState();
     current_state_ = new_state;
-    
+
     RCLCPP_INFO(node_->get_logger(),
                 "状态转移: %s -> %s",
                 old_state.c_str(),
                 getCurrentState().c_str());
-    
+
     // 发布状态到ROS话题
     auto msg = std_msgs::msg::String();
     msg.data = getCurrentState();
     state_publisher_->publish(msg);
-    
+
     // 调用外部回调
     if (state_update_callback_)
     {
@@ -858,7 +978,7 @@ void WorkflowManager::publishState(const std::string &state_str, const std::stri
         msg.data = state_str + ":" + message;
     }
     state_publisher_->publish(msg);
-    
+
     // 调用外部回调
     if (state_update_callback_)
     {
@@ -872,6 +992,20 @@ void WorkflowManager::setStateUpdateCallback(StateUpdateCallback 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)
@@ -885,7 +1019,7 @@ void WorkflowManager::triggerEvent(const std::string &event_name)
                     getCurrentState().c_str());
         return;
     }
-    
+
     // 执行事件
     executeEvent(event_name);
 }