recharge

src/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -12,7 +12,8 @@
 #include <memory>
 #include <tuple>
 #include <std_msgs/msg/bool.hpp>
-
+#include <std_msgs/msg/string.hpp>
+#include<sstream>
 class LidarScanController
 {
 public:
@@ -61,6 +62,7 @@ private:
     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_;
 
     // 状态变量
     bool active_;

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -63,8 +63,10 @@ void LidarScanController::initialize()
 
     // 创建基站检测状态发布者（使用 transient_local 确保新订阅者能收到最后一条消息）
     pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
-        "station/detected",
+        "/station/detected",
         rclcpp::QoS(10).transient_local()); // 修改为 transient_local
+    pub_charging_voltage_ = node_->create_publisher<std_msgs::msg::String>(
+        "/charging/voltage_detected", 10);
 
     // 初始化控制消息
     ctrl_twist_.linear.x = 0.0;
@@ -466,7 +468,7 @@ void LidarScanController::controlRechargeMotion(const RechargeResult &result)
         std::string adjust_type;
 
         const double ANGLE_8DEG = 5.0 * M_PI / 180.0; // 5度阈值
-       // const double DY_THRESHOLD = 0.05;             // 5cm阈值
+                                                      // const double DY_THRESHOLD = 0.05;             // 5cm阈值
 
         // 优先调整角度
         if (fabs(yaw_diff) > ANGLE_8DEG)
@@ -1281,15 +1283,24 @@ void LidarScanController::setChargingVoltage(bool value)
 {
     detect_charging_voltage_ = value;
     detect_charging_voltage_time_ = node_->now();
+    auto msg = std_msgs::msg::String();
+    std::stringstream ss;
+    ss << (value ? "CHARGING_VOLTAGE_DETECTED" : "CHARGING_VOLTAGE_LOST")
+       << ":timestamp=" << node_->now().seconds()
+       << ",voltage_status=" << (value ? "true" : "false");
 
     if (value)
     {
+        ss << ",message=充电电压已检测到，开始充电流程";
         RCLCPP_INFO(node_->get_logger(), "检测到充电电压");
     }
     else
     {
+        ss << ",message=充电电压已断开";
         RCLCPP_INFO(node_->get_logger(), "充电电压已断开");
     }
+     msg.data = ss.str();
+    pub_charging_voltage_->publish(msg);
 }
 
 bool LidarScanController::getChargingVoltage() const

src/wheelchair_state_machine/src/workflow.cpp

@@ -20,11 +20,11 @@ WorkflowManager::WorkflowManager(rclcpp::Node *node)
 
     // 创建状态发布者
     state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
-        "wheelchair/state", 10);
+        "/wheelchair/state", 10);
 
     // 创建回充超时报告发布者
     recharge_timeout_publisher_ = node_->create_publisher<std_msgs::msg::String>(
-        "wheelchair/recharge_timeout", 10);
+        "/wheelchair/recharge_timeout", 10);
 
     RCLCPP_INFO(node_->get_logger(), "工作流管理器已初始化");
 }
@@ -32,7 +32,7 @@ void WorkflowManager::initializeSubscriptions()
 {
     // 创建基站检测订阅者
     station_detected_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
-        "station/detected", 10,
+        "/station/detected", 10,
         [this](const std_msgs::msg::Bool::SharedPtr msg)
         {
             this->stationDetectedCallback(msg);

src/wheelchair_state_machine/src/第一种调到垂线范围（角度误差在5度）

@@ -11,3 +11,17 @@
 /cmd_vel_nav
 /wheel/twist
 
+
+
+
+
+
+
+发布输出：
+主题名	                              消息类型	                          发布者                                                        	描述
+/cmd_vel_raw	             geometry_msgs/msg/Twist	           LidarScanController,RotationManager	                          轮椅运动控制命令
+station/detected	         std_msgs/msg/Bool	                   LidarScanController	                                          充电基站检测状态
+wheelchair/state	             std_msgs/msg/String	           WorkflowManager	                                                轮椅当前状态
+wheelchair/report	              std_msgs/msg/String	            ReportManager	                                                 系统状态报告
+wheelchair/recharge_timeout	      std_msgs/msg/String	             WorkflowManager	                                            回充超时/状态报告
+charging/voltage_detected        std_msgs/msg/String               LidarScanController                                              检测到充电电压