recharge

.vscode/c_cpp_properties.json

@@ -3,13 +3,19 @@
         {
             "name": "Linux",
             "includePath": [
-                "${workspaceFolder}/**"
+                "${workspaceFolder}/src",
+                "${workspaceFolder}/include",
+                "${workspaceFolder}/src/wheelchair_state_machine/include",
+                "${workspaceFolder}/src/wheelchair_state_machine/include/wheelchair_state_machine",
+                "/opt/ros/humble/include/**",
+                "/usr/include/eigen3"
             ],
             "defines": [],
-            "compilerPath": "/usr/bin/gcc",
+            "compilerPath": "/usr/bin/g++",
             "cStandard": "c17",
-            "cppStandard": "gnu++17",
-            "intelliSenseMode": "linux-gcc-arm64"
+            "cppStandard": "c++17",
+            "intelliSenseMode": "linux-gcc-x64",
+            "configurationProvider": "ms-vscode.cpptools"
         }
     ],
     "version": 4

.vscode/launch.json

@@ -0,0 +1,109 @@
+{
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "调试轮椅状态机",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${workspaceFolder}/build/wheelchair_state_machine/wheelchair_state_machine_node",
+            "args": [],
+            "stopAtEntry": false,
+            "cwd": "${workspaceFolder}",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "gdb",
+            "miDebuggerPath": "/usr/bin/gdb",
+            "setupCommands": [
+                {
+                    "description": "启用GDB整齐打印",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                },
+                {
+                    "description": "设置GDB在进入没有调试信息的函数时自动跳过",
+                    "text": "set step-mode off",
+                    "ignoreFailures": true
+                },
+                {
+                    "description": "禁用源文件查找错误提示",
+                    "text": "set source open off",
+                    "ignoreFailures": true
+                },
+                {
+                    "description": "设置反汇编风格",
+                    "text": "-gdb-set disassembly-flavor intel",
+                    "ignoreFailures": true
+                },
+                {
+                    "description": "设置当找不到源文件时继续执行",
+                    "text": "set print source-filename off",
+                    "ignoreFailures": true
+                }
+            ],
+            "preLaunchTask": "编译轮椅状态机",
+            "postDebugTask": "",
+            "logging": {
+                "engineLogging": false,
+                "programOutput": true,
+                "exceptions": true,
+                "moduleLoad": false,
+                "trace": false
+            },
+            "sourceFileMap": {
+                "/workspace": "${workspaceFolder}",
+                "/home/ros2_ws/src": "${workspaceFolder}"
+            }
+        },
+        {
+            "name": "带参数调试",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${workspaceFolder}/build/wheelchair_state_machine/wheelchair_state_machine_node",
+            "args": [
+                "--ros-args",
+                "-p",
+                "low_battery_threshold:=25.0",
+                "-p",
+                "critical_battery_threshold:=15.0",
+                "-p",
+                "distance_threshold:=0.6",
+                "-p",
+                "fit_window_size:=15"
+            ],
+            "stopAtEntry": false,
+            "cwd": "${workspaceFolder}",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "gdb",
+            "miDebuggerPath": "/usr/bin/gdb",
+            "setupCommands": [
+                {
+                    "description": "启用GDB整齐打印",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                },
+                {
+                    "description": "设置跳过无调试信息的函数",
+                    "text": "set step-mode off",
+                    "ignoreFailures": true
+                }
+            ],
+            "preLaunchTask": "编译轮椅状态机"
+        },
+        {
+            "name": "附加到运行进程",
+            "type": "cppdbg",
+            "request": "attach",
+            "program": "${workspaceFolder}/build/wheelchair_state_machine/wheelchair_state_machine_node",
+            "processId": "${command:pickProcess}",
+            "MIMode": "gdb",
+            "setupCommands": [
+                {
+                    "description": "启用GDB整齐打印",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                }
+            ]
+        }
+    ]
+}

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "/home/wljy/zkf/recharge/src/wheelchair_state_machine"
+}

.vscode/tasks.json

@@ -0,0 +1,103 @@
+{
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "label": "编译轮椅状态机",
+            "type": "shell",
+            "command": "bash",
+            "args": [
+                "-c",
+                "cd ${workspaceFolder} && source /opt/ros/humble/setup.bash && colcon build --packages-select wheelchair_state_machine --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS='-O0 -g -Wall'"
+            ],
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "problemMatcher": [
+                "$gcc"
+            ],
+            "presentation": {
+                "reveal": "always",
+                "panel": "new",
+                "clear": true
+            },
+            "options": {
+                "cwd": "${workspaceFolder}"
+            }
+        },
+        {
+            "label": "清理并重新编译",
+            "type": "shell",
+            "command": "bash",
+            "args": [
+                "-c",
+                "cd ${workspaceFolder} && rm -rf build/ install/ log/ && source /opt/ros/humble/setup.bash && colcon build --packages-select wheelchair_state_machine --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS='-O0 -g -Wall'"
+            ],
+            "group": "build",
+            "problemMatcher": ["$gcc"],
+            "presentation": {
+                "reveal": "always",
+                "panel": "new",
+                "clear": true
+            }
+        },
+        {
+            "label": "运行节点",
+            "type": "shell",
+            "command": "bash",
+            "args": [
+                "-c",
+                "cd ${workspaceFolder} && source install/setup.bash && ros2 run wheelchair_state_machine wheelchair_state_machine_node"
+            ],
+            "dependsOn": ["编译轮椅状态机"],
+            "problemMatcher": [],
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            }
+        },
+        {
+            "label": "运行节点（带参数）",
+            "type": "shell",
+            "command": "bash",
+            "args": [
+                "-c",
+                "cd ${workspaceFolder} && source install/setup.bash && ros2 run wheelchair_state_machine wheelchair_state_machine_node --ros-args -p low_battery_threshold:=25.0 -p critical_battery_threshold:=15.0"
+            ],
+            "dependsOn": ["编译轮椅状态机"],
+            "problemMatcher": [],
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            }
+        },
+        {
+            "label": "查看话题列表",
+            "type": "shell",
+            "command": "bash",
+            "args": [
+                "-c",
+                "cd ${workspaceFolder} && source install/setup.bash && ros2 topic list"
+            ],
+            "problemMatcher": [],
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            }
+        },
+        {
+            "label": "查看电池状态话题",
+            "type": "shell",
+            "command": "bash",
+            "args": [
+                "-c",
+                "cd ${workspaceFolder} && source install/setup.bash && ros2 topic echo /battery_state"
+            ],
+            "problemMatcher": [],
+            "presentation": {
+                "reveal": "always",
+                "panel": "new"
+            }
+        }
+    ]
+}

src/wheelchair_state_machine/CMakeLists.txt

@@ -15,7 +15,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
-
+find_package(std_srvs REQUIRED)
 # 1. 主节点可执行文件
 add_executable(wheelchair_state_machine_node
   src/main.cpp
@@ -43,6 +43,7 @@ ament_target_dependencies(wheelchair_state_machine_node
   geometry_msgs
   visualization_msgs
   tf2_geometry_msgs
+  std_srvs
 )
 
 # 安装规则

src/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -16,14 +16,17 @@
 #include<std_msgs/msg/string.hpp>
 #include"recharge_status.hpp"
 #include<sstream>
+#include "report.hpp"
+class ReportManager;
 class LidarScanController
 {
 public:
     using LaserScan = sensor_msgs::msg::LaserScan;
     using LaserScanSharedPtr = sensor_msgs::msg::LaserScan::SharedPtr;
 
-    LidarScanController(rclcpp::Node *node);
+    explicit LidarScanController(rclcpp::Node *node);
     ~LidarScanController();
+    void setReportManager(ReportManager* report_manager);
 
     // 初始化
     void initialize();
@@ -50,6 +53,7 @@ public:
 
 private:
     rclcpp::Node *node_;
+    ReportManager* report_manager_ = nullptr;
 
     // 新增：存储当前角平分线信息
     double current_angle_bisector_k_;
@@ -63,10 +67,10 @@ private:
     rclcpp::TimerBase::SharedPtr navigation_timer_;
     rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
     rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;
-    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_station_detected_; 
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_charging_voltage_;
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub1_;//对外
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub2_;//对外
+    // rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_station_detected_; 
+    // rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_charging_voltage_;
+    // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub1_;//对外
+    // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub2_;//对外
     // 状态变量
     bool active_;
     ChargingState charging_state_;

src/wheelchair_state_machine/include/wheelchair_state_machine/report.hpp

@@ -1,9 +1,11 @@
 // report.hpp
 #ifndef REPORT_MANAGER_HPP
 #define REPORT_MANAGER_HPP
-
-#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <std_msgs/msg/bool.hpp>
 #include <string>
+#include <rclcpp/rclcpp.hpp>
 #include <functional>
 #include <memory>
 #include <map>
@@ -11,16 +13,19 @@
 #include <mutex>
 #include "wheelchair_types.hpp"
 #include "error_code.hpp"
-#include <std_msgs/msg/string.hpp>
 #include <std_msgs/msg/u_int32.hpp>
 
+enum class WheelchairState;
+enum class BatteryState;
+enum class ChargingState;
+
 class ReportManager
 {
 public:
     using StatusReportCallback = std::function<void(const std::string &)>;
     using ErrorCallback = std::function<void(const ErrorInfo &)>;
 
-    ReportManager(rclcpp::Node *node);
+    explicit ReportManager(rclcpp::Node *node);
 
     // 报告生成接口
     std::string generateSystemStatusReport(
@@ -33,8 +38,15 @@ public:
         const std::string &control_mode);
 
     // 状态报告发布
-    void publishStatusReport(const std::string &report);
+    void publishRechargeStatus1(int status);
+    void publishRechargeFeedback(int feedback);
+    void publishChargingVoltage(const std::string& msg_str);
+    void publishStationDetectedMsg(bool detected);
+    void publishRotationTimeoutMsg();
+    void publishWheelchairStateMsg(const std::string& state_str);
+    void publishRechargeTimeoutMsg(const std::string& timeout_str);
 
+    void publishStatusReport(const std::string &report);
     // 特定状态报告
     void reportSystemStatus(
         WheelchairState wheelchair_state,
@@ -72,6 +84,13 @@ public:
 private:
     rclcpp::Node *node_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr report_publisher_;
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr recharge_status1_pub_;
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr recharge_feedback_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr charging_voltage_pub_;
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr station_detected_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr rotation_timeout_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr wheelchair_state_pub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr recharge_timeout_pub_;
 
     // 错误订阅者
     rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr error_sub_;

src/wheelchair_state_machine/include/wheelchair_state_machine/rotation_manager.hpp

@@ -9,11 +9,13 @@
 #include<std_msgs/msg/int32.hpp>
 #include<std_msgs/msg/bool.h>
 #include "recharge_status.hpp"
-
+#include "report.hpp"
+class ReportManager;
 class RotationManager
 {
 public:
-    RotationManager(rclcpp::Node *node);
+    explicit RotationManager(rclcpp::Node *node);
+    void setReportManager(ReportManager* report_manager);
 
     // 核心接口
     void startRotation();
@@ -33,11 +35,12 @@ public:
 
 private:
     rclcpp::Node *node_;
+    ReportManager* report_manager_ = nullptr;
     // ROS发布者
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr rotation_timeout_pub_;
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub1_;//对外
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub2_;//对外
+    // rclcpp::Publisher<std_msgs::msg::String>::SharedPtr rotation_timeout_pub_;
+    // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub1_;//对外
+    // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rotation_timeout_pub2_;//对外
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;
     // 定时器
     rclcpp::TimerBase::SharedPtr rotation_timer_;

src/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -16,13 +16,14 @@
 #include <std_msgs/msg/bool.hpp>
 #include<std_msgs/msg/int32.hpp>
 #include "recharge_status.hpp"
-
+class ReportManager;
 class WorkflowManager
 {
 public:
     using StateUpdateCallback = std::function<void(const std::string &, const std::string &)>;
 
-    WorkflowManager(rclcpp::Node *node);
+    explicit WorkflowManager(rclcpp::Node *node);
+    void setReportManager(ReportManager* report_manager);
 
     // ==================== 状态机核心接口 ====================
 
@@ -83,6 +84,7 @@ public:
 
 private:
     rclcpp::Node *node_;
+    ReportManager* report_manager_ = nullptr;
 
     // 新增：基站检测处理相关
     bool station_detected_while_rotating_ = false;      // 旋转中检测到基站
@@ -97,10 +99,10 @@ private:
     std::unique_ptr<RotationManager> rotation_manager_;
 
     // ROS发布者
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr state_publisher_;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr recharge_timeout_publisher_;
+   // rclcpp::Publisher<std_msgs::msg::String>::SharedPtr state_publisher_;
+    //rclcpp::Publisher<std_msgs::msg::String>::SharedPtr recharge_timeout_publisher_;
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr station_detected_sub_; // 新增
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub1_;//对外
+    //rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub1_;//对外
     rclcpp::Subscription<std_msgs::msg::String>::SharedPtr rotation_timeout_sub_;
 
     // 定时器

src/wheelchair_state_machine/package.xml

@@ -17,6 +17,8 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>interface</depend>
+  <depend>std_srvs</depend>
+
   
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/wheelchair_state_machine/src/event_input.cpp

@@ -14,12 +14,15 @@ EventInputHandler::EventInputHandler(rclcpp::Node *node)
 
 void EventInputHandler::initializeModules()
 {
+    // 初始化报告管理器
+    report_manager_ = std::make_unique<ReportManager>(node_);
     // 初始化电池管理器
     battery_manager_ = std::make_unique<BatteryManager>(node_, 20.0f, 10.0f);
     battery_manager_->initialize();
 
     // 初始化激光控制器
     lidar_controller_ = std::make_unique<LidarScanController>(node_);
+     lidar_controller_->setReportManager(report_manager_.get());
     lidar_controller_->initialize();
 
     // 初始化iPad管理器
@@ -27,8 +30,8 @@ void EventInputHandler::initializeModules()
 
     // 初始化工作流管理器
     workflow_manager_ = std::make_unique<WorkflowManager>(node_);
-    // 初始化报告管理器
-    report_manager_ = std::make_unique<ReportManager>(node_);
+    workflow_manager_->setReportManager(report_manager_.get());
+
 
     setupModuleCallbacks();
     setupSubscriptions();

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -16,7 +16,8 @@ using std::placeholders::_1;
 LidarScanController::LidarScanController(rclcpp::Node *node)
     : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0), station_detected_(false), current_angle_bisector_k_(0.0),
       current_angle_bisector_b_(0.0), // 新增
-      has_angle_bisector_(false)      // 新增
+      has_angle_bisector_(false),
+      report_manager_(nullptr)
 {
 }
 
@@ -24,6 +25,11 @@ LidarScanController::~LidarScanController()
 {
 }
 
+void LidarScanController::setReportManager(ReportManager *report_manager)
+{
+    report_manager_ = report_manager;
+}
+
 void LidarScanController::initialize()
 {
     // 声明参数
@@ -62,13 +68,13 @@ void LidarScanController::initialize()
     pub_line_ = node_->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out_line", 10);
 
     // 创建基站检测状态发布者（使用 transient_local 确保新订阅者能收到最后一条消息）
-    pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
-        "/station/detected",
-        rclcpp::QoS(10).transient_local()); // 修改为 transient_local
-    pub_charging_voltage_ = node_->create_publisher<std_msgs::msg::String>(
-        "/charging/voltage_detected", 10);
-    pub1_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/state",10);
-    pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
+    // pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
+    //     "/station/detected",
+    //     rclcpp::QoS(10).transient_local()); // 修改为 transient_local
+    // pub_charging_voltage_ = node_->create_publisher<std_msgs::msg::String>(
+    //     "/charging/voltage_detected", 10);
+    // pub1_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/state",10);
+    // pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
     // 初始化控制消息
     ctrl_twist_.linear.x = 0.0;
     ctrl_twist_.angular.z = 0.0;
@@ -95,9 +101,11 @@ void LidarScanController::resetStationDetection()
     detect_charging_voltage_ = false;
 
     // 发布false信号
-    auto false_msg = std_msgs::msg::Bool();
-    false_msg.data = false;
-    pub_station_detected_->publish(false_msg);
+    // auto false_msg = std_msgs::msg::Bool();
+    // false_msg.data = false;
+    // pub_station_detected_->publish(false_msg);
+    if (report_manager_)
+        report_manager_->publishStationDetectedMsg(false);
 
     // 重置时间戳
     last_station_publish_time_ = node_->now();
@@ -125,30 +133,17 @@ void LidarScanController::publishStationDetected(bool detected)
 {
     auto now = node_->now();
     auto time_diff = (now - last_station_publish_time_).seconds();
-    // if (station_detected_ == false && detected == false)
-    // {
-    //     auto msg = std_msgs::msg::Bool();
-    //     msg.data = detected;
-    //     pub_station_detected_->publish(msg);
-    // }
     // 防止频繁发布，至少间隔0.5秒
     if (station_detected_ != detected)
     {
-        auto msg = std_msgs::msg::Bool();
-        msg.data = detected;
-        pub_station_detected_->publish(msg);
+        // auto msg = std_msgs::msg::Bool();
+        // msg.data = detected;
+        // pub_station_detected_->publish(msg);
+        if (report_manager_)
+            report_manager_->publishStationDetectedMsg(detected);
         station_detected_ = detected;
     }
     last_station_publish_time_ = now;
-
-    // if (detected)
-    // {
-    //     RCLCPP_INFO(node_->get_logger(), "发布基站检测成功信号: true");
-    // }
-    // else
-    // {
-    //     RCLCPP_DEBUG(node_->get_logger(), "发布基站检测信号: false");
-    // }
 }
 
 void LidarScanController::stopMotion()
@@ -406,7 +401,8 @@ void LidarScanController::procData(RechargeResult &result)
 
 void LidarScanController::controlRechargeMotion(const RechargeResult &result)
 {
-    if (!active_) return;
+    if (!active_)
+        return;
     publishStationDetected(true);
     bool in_rotation_delay = node_->get_parameter("in_rotation_delay").as_bool();
     if (in_rotation_delay)
@@ -507,7 +503,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
                 rclcpp::Time adjust_start = node_->now();
                 int publish_count = 0;
 
-                while (rclcpp::ok() && active_ &&(node_->now() - adjust_start).seconds() < adjust_duration)
+                while (rclcpp::ok() && active_ && (node_->now() - adjust_start).seconds() < adjust_duration)
                 {
                     pub_ctrl_->publish(adjust_cmd);
                     publish_count++;
@@ -541,7 +537,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
             rclcpp::Time forward_start = node_->now();
             int forward_count = 0;
 
-            while (rclcpp::ok() && active_ &&(node_->now() - forward_start).seconds() < forward_duration)
+            while (rclcpp::ok() && active_ && (node_->now() - forward_start).seconds() < forward_duration)
             {
                 pub_ctrl_->publish(forward_cmd);
                 forward_count++;
@@ -646,9 +642,13 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         ctrl_twist_.linear.x = 0.0;
         ctrl_twist_.angular.z = 0.0;
         ctrl_twist_time_ = node_->now();
-        auto msg =std_msgs::msg::Int32();
-        msg.data=RECHARGE_FEEDBACK_SUCCESS_DOCKED;
-        pub1_->publish(msg);
+        // auto msg = std_msgs::msg::Int32();
+        // msg.data = RECHARGE_FEEDBACK_SUCCESS_DOCKED;
+        // pub1_->publish(msg);
+
+        if (report_manager_)
+            report_manager_->publishRechargeFeedback(RECHARGE_FEEDBACK_SUCCESS_DOCKED);
+
         stopNavigation();
 
         // 模拟检测到充电电压
@@ -1290,7 +1290,7 @@ void LidarScanController::setChargingVoltage(bool value)
 {
     detect_charging_voltage_ = value;
     detect_charging_voltage_time_ = node_->now();
-    auto msg = std_msgs::msg::String();
+    // auto msg = std_msgs::msg::String();
     std::stringstream ss;
     ss << (value ? "CHARGING_VOLTAGE_DETECTED" : "CHARGING_VOLTAGE_LOST")
        << ":timestamp=" << node_->now().seconds()
@@ -1306,8 +1306,9 @@ void LidarScanController::setChargingVoltage(bool value)
         ss << ",message=充电电压已断开";
         RCLCPP_INFO(node_->get_logger(), "充电电压已断开");
     }
-     msg.data = ss.str();
-    pub_charging_voltage_->publish(msg);
+    // msg.data = ss.str();
+    // pub_charging_voltage_->publish(msg);
+
 }
 
 bool LidarScanController::getChargingVoltage() const
@@ -1321,5 +1322,5 @@ bool LidarScanController::getChargingVoltage() const
     //     return true;
     // }
     // return false;
-    return detect_charging_voltage_; 
+    return detect_charging_voltage_;
 }

src/wheelchair_state_machine/src/report.cpp

@@ -3,6 +3,8 @@
 #include <rclcpp/logging.hpp>
 #include <sstream>
 #include <iomanip>
+#include <std_msgs/msg/int32.hpp>
+#include <std_msgs/msg/bool.hpp>
 using namespace std::chrono_literals;
 
 std::map<uint32_t, std::string> ReportManager::error_descriptions_;
@@ -16,11 +18,75 @@ ReportManager::ReportManager(rclcpp::Node *node)
     report_publisher_ = node_->create_publisher<std_msgs::msg::String>(
         "wheelchair/report", 10);
 
+    recharge_status1_pub_ = node_->create_publisher<std_msgs::msg::Int32>(
+        "/vlago/recharge/state", 10);
+    recharge_feedback_pub_ = node_->create_publisher<std_msgs::msg::Int32>(
+        "/vlago/recharge/feedback", 10);
+    charging_voltage_pub_ = node_->create_publisher<std_msgs::msg::String>(
+        "/charging/voltage_detected", 10);
+    station_detected_pub_ = node_->create_publisher<std_msgs::msg::Bool>(
+        "/station/detected", rclcpp::QoS(10).transient_local());
+    rotation_timeout_pub_ = node_->create_publisher<std_msgs::msg::String>(
+        "/wheelchair/rotation_timeout", 10);
+    wheelchair_state_pub_ = node_->create_publisher<std_msgs::msg::String>(
+        "/wheelchair/state", 10);
+    recharge_timeout_pub_ = node_->create_publisher<std_msgs::msg::String>(
+        "/wheelchair/recharge_timeout", 10);
+
     // 初始化错误映射
     initializeErrorMaps();
     RCLCPP_INFO(node_->get_logger(), "报告管理器已初始化");
 }
 
+void ReportManager::publishRechargeStatus1(int status)
+{
+    auto msg = std_msgs::msg::Int32();
+    msg.data = status;
+    recharge_status1_pub_->publish(msg);
+}
+
+void ReportManager::publishRechargeFeedback(int feedback)
+{
+    auto msg = std_msgs::msg::Int32();
+    msg.data = feedback;
+    recharge_feedback_pub_->publish(msg);
+}
+
+void ReportManager::publishChargingVoltage(const std::string &msg_str)
+{
+    auto msg = std_msgs::msg::String();
+    msg.data = msg_str;
+    charging_voltage_pub_->publish(msg);
+}
+
+void ReportManager::publishStationDetectedMsg(bool detected)
+{
+    auto msg = std_msgs::msg::Bool();
+    msg.data = detected;
+    station_detected_pub_->publish(msg);
+}
+
+void ReportManager::publishRotationTimeoutMsg()
+{
+    auto msg = std_msgs::msg::String();
+    msg.data ="rotation_timeout";
+    rotation_timeout_pub_->publish(msg);
+}
+
+void ReportManager::publishWheelchairStateMsg(const std::string &state_str)
+{
+    auto msg = std_msgs::msg::String();
+    msg.data = state_str;
+    wheelchair_state_pub_->publish(msg);
+}
+
+void ReportManager::publishRechargeTimeoutMsg(const std::string &timeout_str)
+{
+    auto msg = std_msgs::msg::String();
+    msg.data = timeout_str;
+    recharge_timeout_pub_->publish(msg);
+}
+
 std::string ReportManager::generateSystemStatusReport(
     WheelchairState wheelchair_state,
     BatteryState battery_state,

src/wheelchair_state_machine/src/rotation_manager.cpp

@@ -9,19 +9,25 @@ RotationManager::RotationManager(rclcpp::Node *node)
       ,
       angular_velocity_(0.15) // 角速度0.3 rad/s
       ,
-      linear_velocity_(0.0) // 线速度0 m/s
+      linear_velocity_(0.0), // 线速度0 m/s
+      report_manager_(nullptr)
 {
     // 创建控制命令发布者
     cmd_vel_publisher_ = node_->create_publisher<geometry_msgs::msg::Twist>(
         "/cmd_vel_raw", 10);
-    rotation_timeout_pub1_ = node_->create_publisher<std_msgs::msg::Int32>(
-        "/vlago/recharge/state", 10);
-    rotation_timeout_pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
-    rotation_timeout_pub_ = node_->create_publisher<std_msgs::msg::String>(
-        "/wheelchair/rotation_timeout", 10);
+    // rotation_timeout_pub1_ = node_->create_publisher<std_msgs::msg::Int32>(
+    //     "/vlago/recharge/state", 10);
+    // rotation_timeout_pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
+    // rotation_timeout_pub_ = node_->create_publisher<std_msgs::msg::String>(
+    //     "/wheelchair/rotation_timeout", 10);
     logInfo("旋转管理器初始化完成");
 }
 
+void RotationManager::setReportManager(ReportManager *report_manager)
+{
+    report_manager_ = report_manager;
+}
+
 void RotationManager::startRotation()
 {
     if (is_rotating_)
@@ -47,16 +53,23 @@ void RotationManager::startRotation()
         [this]()
         {
             logInfo("旋转超时，自动停止旋转");
-             auto timeout_msg = std_msgs::msg::String();
-            timeout_msg.data = "rotation_timeout";
-            rotation_timeout_pub_->publish(timeout_msg);
+            //  auto timeout_msg = std_msgs::msg::String();
+            // timeout_msg.data = "rotation_timeout";
+            // rotation_timeout_pub_->publish(timeout_msg);
+            if (report_manager_)
+                report_manager_->publishRotationTimeoutMsg();
             stopRotation();
-            auto timeout_msg1 = std_msgs::msg::Int32();
-            timeout_msg1.data = RECHARGE_EVENT_STATION_NOT_FOUND;
-            rotation_timeout_pub1_->publish(timeout_msg1); 
-            auto timeout_msg2 = std_msgs::msg::Int32();
-            timeout_msg2.data = RECHARGE_FEEDBACK_FAILED_TIMEOUT;
-            rotation_timeout_pub2_->publish(timeout_msg2);
+            // auto timeout_msg1 = std_msgs::msg::Int32();
+            // timeout_msg1.data = RECHARGE_EVENT_STATION_NOT_FOUND;
+            // rotation_timeout_pub1_->publish(timeout_msg1);
+            // auto timeout_msg2 = std_msgs::msg::Int32();
+            // timeout_msg2.data = RECHARGE_FEEDBACK_FAILED_TIMEOUT;
+            // rotation_timeout_pub2_->publish(timeout_msg2);
+            if (report_manager_)
+            {
+                report_manager_->publishRechargeStatus1(RECHARGE_EVENT_STATION_NOT_FOUND);
+                report_manager_->publishRechargeFeedback(RECHARGE_FEEDBACK_FAILED_TIMEOUT);
+            }
         });
 
     logInfo("开始旋转 - 角速度: " + std::to_string(angular_velocity_) +

src/wheelchair_state_machine/src/workflow.cpp

@@ -18,17 +18,26 @@ WorkflowManager::WorkflowManager(rclcpp::Node *node)
 
     initializeSubscriptions(); // 新增
 
-    // 创建状态发布者
-    state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
-        "/wheelchair/state", 10);
+    // // 创建状态发布者
+    // state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
+    //     "/wheelchair/state", 10);
 
-    // 创建回充超时报告发布者
-    recharge_timeout_publisher_ = node_->create_publisher<std_msgs::msg::String>(
-        "/wheelchair/recharge_timeout", 10);
-    pub1_ = node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/state", 10);
+    // // 创建回充超时报告发布者
+    // recharge_timeout_publisher_ = node_->create_publisher<std_msgs::msg::String>(
+    //     "/wheelchair/recharge_timeout", 10);
+    // pub1_ = node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/state", 10);
 
     RCLCPP_INFO(node_->get_logger(), "工作流管理器已初始化");
 }
+
+void WorkflowManager::setReportManager(ReportManager *report_manager)
+{
+    report_manager_ = report_manager;
+    if (rotation_manager_)
+    {
+        rotation_manager_->setReportManager(report_manager);
+    }
+}
 void WorkflowManager::initializeSubscriptions()
 {
     // 创建基站检测订阅者
@@ -45,13 +54,13 @@ void WorkflowManager::initializeSubscriptions()
             if (msg->data == "rotation_timeout")
             {
                 RCLCPP_INFO(node_->get_logger(), "收到旋转超时信号，返回就绪状态");
-                
+
                 // 如果当前是旋转状态，处理超时
                 if (current_state_ == WheelchairState::ROTATING)
                 {
                     // 发布回充失败通知
                     publishRechargeStatus("RECHARGE_FAILED", "旋转搜索超时，未检测到基站");
-                    
+
                     // 返回就绪状态
                     transitionToReady();
                 }
@@ -755,9 +764,12 @@ void WorkflowManager::transitionToWalking()
             stopRotationTimer();
             RCLCPP_INFO(node_->get_logger(), "轮椅开始行走");
             publishState("WHEELCHAIR_WALKING");
-            auto msg = std_msgs::msg::Int32();
-            msg.data = RECHARGE_EVENT_OBSTACLE_DETECTED;
-            pub1_->publish(msg);
+            // auto msg = std_msgs::msg::Int32();
+            // msg.data = RECHARGE_EVENT_OBSTACLE_DETECTED;
+            // pub1_->publish(msg);
+
+            if (report_manager_)
+                report_manager_->publishRechargeStatus1(RECHARGE_EVENT_OBSTACLE_DETECTED);
         }
     }
     catch (const std::exception &e)
@@ -1405,9 +1417,15 @@ void WorkflowManager::emergencyTransitionToReady(const std::string &reason)
         current_state_ = WheelchairState::READY;
 
         // 发布紧急状态
-        auto msg = std_msgs::msg::String();
-        msg.data = "EMERGENCY_READY:系统异常后强制就绪 - " + reason;
-        state_publisher_->publish(msg);
+        // auto msg = std_msgs::msg::String();
+        // msg.data = "EMERGENCY_READY:系统异常后强制就绪 - " + reason;
+        // state_publisher_->publish(msg);
+
+        if (report_manager_)
+        {
+            report_manager_->publishWheelchairStateMsg(
+                "EMERGENCY_READY:系统异常后强制就绪 - " + reason);
+        }
 
         // 如果之前是回充相关状态，发布回充失败
         if (previous_state == "搜索中" || previous_state == "充电中" || previous_state == "旋转中")
@@ -1434,9 +1452,14 @@ void WorkflowManager::publishRechargeStatus(const std::string &status, const std
     try
     {
         // 发布到专用话题
-        auto msg = std_msgs::msg::String();
-        msg.data = status + ": " + message;
-        recharge_timeout_publisher_->publish(msg);
+        // auto msg = std_msgs::msg::String();
+        // msg.data = status + ": " + message;
+        // recharge_timeout_publisher_->publish(msg);
+        if (report_manager_)
+        {
+            report_manager_->publishRechargeTimeoutMsg(status +
+                                                       ": " + message);
+        }
 
         // 同时记录日志
         RCLCPP_INFO(node_->get_logger(), "[回充状态] %s: %s", status.c_str(), message.c_str());
@@ -1586,9 +1609,12 @@ void WorkflowManager::setState(WheelchairState new_state)
                        << old_state << " -> " << getCurrentState();
 
         // 发布状态到ROS话题
-        auto msg = std_msgs::msg::String();
-        msg.data = enhanced_state.str();
-        state_publisher_->publish(msg);
+        // auto msg = std_msgs::msg::String();
+        // msg.data = enhanced_state.str();
+        // state_publisher_->publish(msg);
+
+        if (report_manager_)
+            report_manager_->publishWheelchairStateMsg(enhanced_state.str());
 
         // 调用外部回调
         if (state_update_callback_)
@@ -1625,16 +1651,11 @@ void WorkflowManager::publishState(const std::string &state_str, const std::stri
 {
     try
     {
-        auto msg = std_msgs::msg::String();
-        if (message.empty())
-        {
-            msg.data = state_str;
-        }
-        else
-        {
-            msg.data = state_str + ":" + message;
-        }
-        state_publisher_->publish(msg);
+
+        std::string msg_data = message.empty() ? state_str : (state_str + ":" + message);
+
+        if (report_manager_)
+            report_manager_->publishWheelchairStateMsg(msg_data);
 
         // 调用外部回调
         if (state_update_callback_)