wheelchair_recharge

src/interface/CMakeLists.txt

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

src/interface/msg/Empty.msg

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

src/interface/package.xml

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

src/interface/srv/SceneControls.srv

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

src/wheelchair_state_machine/CMakeLists.txt

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

src/wheelchair_state_machine/LICENSE

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

src/wheelchair_state_machine/include/wheelchair_state_machine/battery_manager.hpp

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

src/wheelchair_state_machine/include/wheelchair_state_machine/recharge_controller.hpp

@@ -1,117 +0,0 @@
-// recharge_controller.hpp
-#ifndef RECHARGE_CONTROLLER_HPP
-#define RECHARGE_CONTROLLER_HPP
-
-#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-#include <std_msgs/msg/bool.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();
-    void startRecharge();
-    void cancelRecharge();
-    void processLaserScan(const LaserScanSharedPtr& msg);
-    void onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg);
-    void setChargingVoltage(bool value);
-    bool getChargingVoltage() const;
-    
-    bool isActive() const { return active_; }
-    ChargingState getChargingState() const { return charging_state_; }
-    void setChargingState(ChargingState state) { charging_state_ = state; }
-    
-    // 控制相关
-    void publishControl();
-    void stopMotion();
-    
-private:
-    rclcpp::Node* node_;
-    
-    // ROS2 订阅者和发布者
-    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
-    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_ctrl_;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_cmd_enable_;
-    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_;
-    bool cmd_enable_;
-    
-    // 参数
-    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

src/wheelchair_state_machine/include/wheelchair_state_machine/recharge_tool.hpp

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

src/wheelchair_state_machine/include/wheelchair_state_machine/wheelchair_state_machine.hpp

@@ -1,109 +0,0 @@
-// wheelchair_state_machine.hpp
-#ifndef WHEELCHAIR_STATE_MACHINE_HPP
-#define WHEELCHAIR_STATE_MACHINE_HPP
-
-#include <rclcpp/rclcpp.hpp>
-#include <string>
-#include <map>
-#include <memory>
-#include "wheelchair_types.hpp"
-#include "battery_manager.hpp"
-#include "recharge_controller.hpp"
-// 添加必要的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 { return recharge_controller_.isActive(); }
-    
-    // 充电检测接口
-    void setChargingVoltage(bool value);
-    bool getChargingVoltage() const { return recharge_controller_.getChargingVoltage(); }
-
-    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_;
-    bool interface_active_;
-    
-    // ==================== 模块化组件 ====================
-    BatteryManager battery_manager_;
-    RechargeController recharge_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);
-    
-    // 辅助函数
-    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

src/wheelchair_state_machine/include/wheelchair_state_machine/wheelchair_types.hpp

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

src/wheelchair_state_machine/package.xml

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

src/wheelchair_state_machine/src/battery_manager.cpp

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

src/wheelchair_state_machine/src/main.cpp

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

src/wheelchair_state_machine/src/recharge_controller.cpp

@@ -1,1095 +0,0 @@
-// recharge_controller.cpp
-#include "wheelchair_state_machine/recharge_controller.hpp"
-#include <rclcpp/logging.hpp>
-#include <rclcpp/qos.hpp>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <cmath>
-#include <limits>
-#include <deque>
-#include <numeric>
-#include <thread>
-#include <Eigen/Dense>
-
-using namespace std::chrono_literals;
-using std::placeholders::_1;
-
-RechargeController::RechargeController(rclcpp::Node* node)
-    : node_(node)
-    , active_(false)
-    , charging_state_(ChargingState::IDLE)
-    , cmd_enable_(false)
-    , 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);
-    
-    // 创建命令使能订阅者
-    sub_cmd_enable_ = node_->create_subscription<std_msgs::msg::Bool>(
-        "/recharge/cmd_enable", 10,
-        std::bind(&RechargeController::onCmdEnable, this, _1));
-    
-    // 创建可视化发布者
-    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::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    bool new_enable = msg->data;
-    
-    // 如果从false变为true，启动回充
-    if (!cmd_enable_ && new_enable) {
-        RCLCPP_INFO(node_->get_logger(), "收到回充启动命令");
-        startRecharge();
-    }
-    // 如果从true变为false，取消回充
-    else if (cmd_enable_ && !new_enable) {
-        RCLCPP_INFO(node_->get_logger(), "收到回充取消命令");
-        cancelRecharge();
-    }
-    
-    cmd_enable_ = new_enable;
-    RCLCPP_INFO(node_->get_logger(), "回充命令使能: %s", cmd_enable_ ? "启用" : "禁用");
-}
-
-void RechargeController::rechargeTimerCallback()
-{
-    // 如果不是回充模式，直接返回
-    if (!active_)
-    {
-        return;
-    }
-    
-    auto now = node_->now();
-    auto time_diff = (now - ctrl_twist_time_).seconds();
-    
-    // 如果最近有控制命令且使能，发布控制命令
-    if (time_diff < 2.0 && cmd_enable_)
-    {
-        pub_ctrl_->publish(ctrl_twist_);
-    }
-}
-
-void RechargeController::processLaserScan(const LaserScanSharedPtr& msg)
-{
-    // 应用过滤
-    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_ && cmd_enable_)
-    {
-        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;
-}

src/wheelchair_state_machine/src/recharge_tool.cpp

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

src/wheelchair_state_machine/src/wheelchair_state_machine.cpp

@@ -1,935 +0,0 @@
-// wheelchair_state_machine.cpp
-#include "wheelchair_state_machine/wheelchair_state_machine.hpp"
-#include <rclcpp/logging.hpp>
-#include <rclcpp/qos.hpp>
-#include <cmath>
-
-using namespace std::chrono_literals;
-
-WheelchairStateMachine::WheelchairStateMachine()
-    : Node("wheelchair_state_machine")
-    , current_state_(WheelchairState::READY)
-    , interface_active_(false)
-    , battery_manager_(this, 20.0f, 10.0f)
-    , recharge_controller_(this)
-    , last_mode_("")
-{
-    // 初始化状态转移表
-    initializeTransitionTable();
-    
-    // 创建状态发布者
-    state_publisher_ = this->create_publisher<std_msgs::msg::String>(
-        "wheelchair/state", 10);
-    
-    cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
-        "cmd_vel", 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();
-    
-    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);
-    
-    // 根据之前的状态执行清理操作
-    if (previous_state == "搜索中")
-    {
-        RCLCPP_INFO(this->get_logger(), "停止搜索，返回到就绪状态");
-    }
-    else if (previous_state == "充电中")
-    {
-        RCLCPP_INFO(this->get_logger(), "停止充电，返回到就绪状态");
-        recharge_controller_.setChargingVoltage(false);
-    }
-    else if (previous_state == "行走中")
-    {
-        RCLCPP_INFO(this->get_logger(), "停止行走，返回到就绪状态");
-    }
-    
-    RCLCPP_INFO(this->get_logger(), "返回到就绪状态");
-    
-    // 发布就绪状态通知
-    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自主回充按钮点击");
-    
-    // 1. 检查当前状态
-    std::string current_state_str = getCurrentState();
-    RCLCPP_INFO(this->get_logger(), "当前状态: %s", current_state_str.c_str());
-    
-    // 2. 用户通过iPad点击了自主回充按钮
-    RCLCPP_INFO(this->get_logger(), "用户通过iPad启动自主回充功能");
-    
-    // 3. 发布自主回充启动通知
-    publishStateUpdate("AUTO_RECHARGE_INITIATED_BY_IPAD");
-    
-    // 4. 开始搜索充电站
-    if (current_state_ == WheelchairState::READY)
-    {
-        // 直接进入搜索状态
-        transitionToSearching();
-        
-        // 发布搜索开始的通知
-        publishStateUpdate("AUTO_RECHARGE_SEARCH_STARTED");
-        
-        RCLCPP_INFO(this->get_logger(), "iPad自主回充已启动，开始搜索充电站");
-        
-        // 显示系统状态
-        displaySystemStatus();
-    }
-    else
-    {
-        RCLCPP_WARN(this->get_logger(), "当前不是就绪状态(%s)，无法启动自主回充",
-                    current_state_str.c_str());
-        
-        // 发布错误通知
-        publishStateUpdate("AUTO_RECHARGE_FAILED_NOT_READY");
-    }
-}
-
-void WheelchairStateMachine::handleIpadPhoneCancel()
-{
-    RCLCPP_INFO(this->get_logger(), "处理iPad自主回充取消");
-    
-    // 记录取消前的状态
-    std::string previous_state = getCurrentState();
-    
-    // 根据当前状态进行不同的取消处理
-    switch (current_state_)
-    {
-    case WheelchairState::SEARCHING:
-    {
-        RCLCPP_INFO(this->get_logger(), "取消自主回充搜索");
-        
-        // 停止搜索，返回到就绪状态
-        transitionToReady();
-        
-        // 发布取消通知
-        publishStateUpdate("AUTO_RECHARGE_CANCELLED_IN_SEARCH");
-        break;
-    }
-    case WheelchairState::WALKING:
-    {
-        RCLCPP_WARN(this->get_logger(), "在行走状态下取消自主回充");
-        // 行走状态下取消，保持行走状态
-        break;
-    }
-    default:
-        break;
-    }
-    
-    // 发布取消通知
-    publishStateUpdate("AUTO_RECHARGE_CANCELLED");
-    
-    RCLCPP_INFO(this->get_logger(), "自主回充已取消，前状态: %s", previous_state.c_str());
-}
-
-// ==================== 电池相关事件处理 ====================
-
-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();
-}