wheelchair_state_machine

src/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -40,6 +40,8 @@ public:
 
     void publishStationDetected(bool detected);
 
+    void resetStationDetection();
+
 private:
     rclcpp::Node *node_;
 

src/wheelchair_state_machine/src/event_input.cpp

@@ -67,6 +67,11 @@ void EventInputHandler::setupModuleCallbacks()
         [this]()
         {
             RCLCPP_INFO(node_->get_logger(), "iPad启动回充");
+            if (lidar_controller_)
+            {
+                lidar_controller_->resetStationDetection();
+                RCLCPP_INFO(node_->get_logger(), "已重置基站检测状态为false");
+            }
             triggerEvent("ipad_phone_interface_start");
         });
 

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -59,6 +59,11 @@ void LidarScanController::initialize()
     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);
 
+    // 创建基站检测状态发布者（使用 transient_local 确保新订阅者能收到最后一条消息）
+    pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
+        "station/detected",
+        rclcpp::QoS(10).transient_local()); // 修改为 transient_local
+
     // 初始化控制消息
     ctrl_twist_.linear.x = 0.0;
     ctrl_twist_.angular.z = 0.0;
@@ -68,19 +73,31 @@ void LidarScanController::initialize()
     navigation_timer_ = node_->create_wall_timer(
         100ms, std::bind(&LidarScanController::navigationTimerCallback, this));
 
-    auto now = node_->now();
-    ctrl_twist_time_ = now;
-    detect_station_time_ = now - rclcpp::Duration(100.0, 0);
-    detect_charging_voltage_time_ = now;
-    last_station_publish_time_ = now;
+    detect_station_time_ = node_->now() - rclcpp::Duration(100.0, 0);
+    detect_charging_voltage_time_ = node_->now();
+
+    // 初始化基站检测状态
     station_detected_ = false;
-    pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
-        "station/detected", 10);
+    last_station_publish_time_ = node_->now();
+
+    RCLCPP_INFO(node_->get_logger(), "激光扫描控制器已初始化");
+}
+
+void LidarScanController::resetStationDetection()
+{
+    // 重置内部状态
+    station_detected_ = false;
+    detect_charging_voltage_ = false;
+
+    // 发布false信号
     auto false_msg = std_msgs::msg::Bool();
     false_msg.data = false;
     pub_station_detected_->publish(false_msg);
 
-    RCLCPP_INFO(node_->get_logger(), "激光扫描控制器已初始化");
+    // 重置时间戳
+    last_station_publish_time_ = node_->now();
+
+    RCLCPP_INFO(node_->get_logger(), "基站检测状态已重置: false");
 }
 
 void LidarScanController::startNavigation()