wheelchair_recharge

src2/interface/CMakeLists.txt

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

src2/interface/msg/Empty.msg

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

src2/interface/package.xml

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

src2/interface/srv/SceneControls.srv

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

src2/wheelchair_state_machine/CMakeLists.txt

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

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

src2/wheelchair_state_machine/include/wheelchair_state_machine/battery_manager.hpp

@@ -0,0 +1,50 @@
+// battery_manager.hpp
+#ifndef BATTERY_MANAGER_HPP
+#define BATTERY_MANAGER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+#include "wheelchair_types.hpp"
+#include <functional>
+
+class BatteryManager
+{
+public:
+    using BatteryStateCallback = std::function<void(BatteryState, float)>;
+    using LowBatteryCallback = std::function<void(BatteryState, float)>;
+    
+    BatteryManager(rclcpp::Node* node, 
+                   float low_threshold = 20.0f, 
+                   float critical_threshold = 10.0f);
+    
+    void initialize();
+    void updateFromMessage(const sensor_msgs::msg::BatteryState::SharedPtr msg);
+    void setBatteryStateCallback(BatteryStateCallback callback);
+    void setLowBatteryCallback(LowBatteryCallback callback);
+    
+    BatteryState getBatteryState() const { return battery_state_; }
+    float getBatteryPercentage() const { return battery_percentage_; }
+    float getLowThreshold() const { return low_battery_threshold_; }
+    float getCriticalThreshold() const { return critical_battery_threshold_; }
+    
+    std::string stateToString() const;
+    
+private:
+    rclcpp::Node* node_;
+    rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
+    
+    BatteryState battery_state_;
+    float battery_percentage_;
+    float low_battery_threshold_;
+    float critical_battery_threshold_;
+    
+    BatteryStateCallback battery_state_callback_;
+    LowBatteryCallback low_battery_callback_;
+    
+    void batteryStateCallback(const sensor_msgs::msg::BatteryState::SharedPtr msg);
+    void updateBatteryState(float percentage);
+    void checkBatteryThresholds();
+    std::string batteryStateToString(BatteryState state) const;
+};
+
+#endif // BATTERY_MANAGER_HPPs

src2/wheelchair_state_machine/include/wheelchair_state_machine/ipad_controller.hpp

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

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

@@ -0,0 +1,29 @@
+// recharge_tool.hpp
+#ifndef RECHARGE_TOOL_HPP
+#define RECHARGE_TOOL_HPP
+
+#include "wheelchair_types.hpp"
+#include <vector>
+#include <Eigen/Dense>
+
+class RechargeTool
+{
+public:
+    static double angleBetween(const IndexPoint &p1, const IndexPoint &p2);
+    static double angleDiff(double a1, double a2);
+    static double averageAngle(const std::vector<double> &angles);
+    
+    static std::vector<std::vector<IndexPoint>> splitPointsToSegments(
+        const std::vector<IndexPoint> &points,
+        double angle_threshold_deg = 15.0,
+        int history_window_size = 3);
+    
+    static std::pair<double, double> calculateAngleBisector(double k1, double b1, double k2, double b2);
+    static std::pair<double, double> fitLineLeastSquares(const Eigen::MatrixXd &points);
+    static std::pair<double, double> computeDockingVelocity(double dx, double dy, double yaw_error);
+    
+    // 直线拟合（简化版，用于基础功能）
+    static std::pair<double, double> simpleFitLine(const std::vector<IndexPoint> &points);
+};
+
+#endif // RECHARGE_TOOL_HPP

src2/wheelchair_state_machine/include/wheelchair_state_machine/wheelchair_state_machine.hpp

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

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

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

src2/wheelchair_state_machine/src/battery_manager.cpp

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

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

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

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

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

src2/wheelchair_state_machine/src/wheelchair_state_machine.cpp

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