wheelchair_state_machine

src/wheelchair_state_machine/include/wheelchair_state_machine/event_input.hpp

@@ -70,7 +70,7 @@ private:
     std::unique_ptr<ReportManager> report_manager_;
 
     // ROS订阅者
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr cmd_enable_sub_;
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr cmd_enable_sub_;
 
     // 事件回调映射
     std::map<std::string, EventCallback> event_callbacks_;

src/wheelchair_state_machine/include/wheelchair_state_machine/ipad_manager.hpp

@@ -3,7 +3,7 @@
 #define IPAD_MANAGER_HPP
 
 #include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <functional>
 
 class IpadManager
@@ -16,7 +16,7 @@ public:
     IpadManager(rclcpp::Node *node);
 
     // 核心接口 - 只处理开始和结束
-    void onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg);
+    void onCmdEnable(const std_msgs::msg::String::SharedPtr msg);
 
     // 回调设置
     void setRechargeStartCallback(RechargeStartCallback callback);
@@ -34,7 +34,7 @@ private:
 
     // 状态
     bool recharge_active_;
-    bool cmd_enable_;
+    std::string cmd_enable_;
 
     // 内部方法
     void startRecharge();

src/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

@@ -11,6 +11,7 @@
 #include <map>
 #include <memory>
 #include <tuple>
+#include <std_msgs/msg/bool.hpp>
 
 class LidarScanController
 {
@@ -35,7 +36,11 @@ public:
 
     // 状态查询
     bool isActive() const { return active_; }
-    ChargingState getChargingState() const { return charging_state_; }
+    ChargingState getChargingState() const { return charging_state_; };
+
+    void publishStationDetected(bool detected);
+
+    void resetStationDetection();
 
 private:
     rclcpp::Node *node_;
@@ -47,6 +52,7 @@ 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_; // 新增
 
     // 状态变量
     bool active_;
@@ -55,6 +61,9 @@ private:
     rclcpp::Time ctrl_twist_time_;
     rclcpp::Time detect_station_time_;
 
+    bool station_detected_;
+    rclcpp::Time last_station_publish_time_;
+
     // 参数
     ScanFilterParam param_;
     double distance_threshold_;

src/wheelchair_state_machine/include/wheelchair_state_machine/workflow.hpp

@@ -13,6 +13,7 @@
 #include <std_msgs/msg/u_int32.hpp>
 #include <rclcpp/timer.hpp>
 #include "rotation_manager.hpp" // 新增头文件
+#include <std_msgs/msg/bool.hpp>
 
 class WorkflowManager
 {
@@ -91,6 +92,7 @@ private:
     // ROS发布者
     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::TimerBase::SharedPtr search_timeout_timer_;
@@ -107,6 +109,7 @@ private:
     // 初始化方法
     void initializeTransitionTable();
     void initializeRotationManager();
+    void initializeSubscriptions(); // 新增
 
     // 事件权限检查
     bool checkEventPermission(const std::string &event);
@@ -149,5 +152,7 @@ private:
 
     void searchTimeoutCallback();
     void rotationCheckCallback();
+
+    void stationDetectedCallback(const std_msgs::msg::Bool::SharedPtr msg);
 };
 #endif

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");
         });
 
@@ -105,6 +110,10 @@ void EventInputHandler::setupModuleCallbacks()
             {
                 stopNavigationControl();
             }
+            else if (current_state == WheelchairState::WALKING)
+            {
+                RCLCPP_INFO(node_->get_logger(), "进入行走对接状态");
+            }
         });
 
     // 4. 设置工作流管理器回充状态回调
@@ -259,9 +268,9 @@ void EventInputHandler::setupModuleCallbacks()
 void EventInputHandler::setupSubscriptions()
 {
     // 创建命令使能订阅者
-    cmd_enable_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
-        "/recharge/cmd_enable", 10,
-        [this](const std_msgs::msg::Bool::SharedPtr msg)
+    cmd_enable_sub_ = node_->create_subscription<std_msgs::msg::String>(
+        "/scene_services/mode", 10,
+        [this](const std_msgs::msg::String::SharedPtr msg)
         {
             this->ipad_manager_->onCmdEnable(msg);
         });

src/wheelchair_state_machine/src/ipad_manager.cpp

@@ -5,30 +5,24 @@
 using namespace std::chrono_literals;
 
 IpadManager::IpadManager(rclcpp::Node *node)
-    : node_(node), recharge_active_(false), cmd_enable_(false)
+    : node_(node), recharge_active_(false), cmd_enable_("auxiliary")
 {
     logInfo("iPad管理器初始化完成");
 }
 
-void IpadManager::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
+void IpadManager::onCmdEnable(const std_msgs::msg::String::SharedPtr msg)
 {
-    bool new_enable = msg->data;
-    logInfo("收到命令使能: " + std::to_string(new_enable));
-    if (new_enable == false)
-    {
-        return;
-    }
+    std::string new_enable = msg->data;
+    logInfo("收到命令使能: " + new_enable);
     // 状态变化检查
-    if (cmd_enable_ == false && new_enable == true)
+    if (cmd_enable_ == "auxiliary" && new_enable == "recharge")
     {
-        // 命令使能从false变为true - 启动回充
-        cmd_enable_ = true;
+        cmd_enable_ = "recharge";
         startRecharge();
     }
-    else if (cmd_enable_ == true && new_enable == true)
+    else if (cmd_enable_ == "recharge" && new_enable == "auxiliary")
     {
-        // 命令使能从true变为false - 取消回充
-        cmd_enable_ = false;
+        cmd_enable_ = "auxiliary";
         cancelRecharge();
     }
     else

src/wheelchair_state_machine/src/lidascan_ctrl.cpp

@@ -14,7 +14,7 @@ using namespace std::chrono_literals;
 using std::placeholders::_1;
 
 LidarScanController::LidarScanController(rclcpp::Node *node)
-    : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0)
+    : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0), station_detected_(false)
 {
 }
 
@@ -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;
@@ -71,13 +76,35 @@ void LidarScanController::initialize()
     detect_station_time_ = node_->now() - rclcpp::Duration(100.0, 0);
     detect_charging_voltage_time_ = node_->now();
 
+    // 初始化基站检测状态
+    station_detected_ = false;
+    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);
+
+    // 重置时间戳
+    last_station_publish_time_ = node_->now();
+
+    RCLCPP_INFO(node_->get_logger(), "基站检测状态已重置: false");
+}
+
 void LidarScanController::startNavigation()
 {
     active_ = true;
     charging_state_ = ChargingState::NAVIGATING;
+    station_detected_ = false;
     RCLCPP_INFO(node_->get_logger(), "导航功能已启动");
 }
 
@@ -89,6 +116,36 @@ void LidarScanController::stopNavigation()
     RCLCPP_INFO(node_->get_logger(), "导航功能已停止");
 }
 
+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);
+        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()
 {
     ctrl_twist_.linear.x = 0.0;
@@ -212,6 +269,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
     auto xy_list_full = laserScanToXY(filtered_msg);
     if (xy_list_full.size() < 2)
     {
+        // publishStationDetected(false);
         RCLCPP_ERROR(node_->get_logger(), "xy坐标系没有转换成功");
         return false;
     }
@@ -222,6 +280,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
 
     if (filtered_segments.empty())
     {
+        // publishStationDetected(false);
         RCLCPP_DEBUG(node_->get_logger(), "未检测到有效的分段");
         return false;
     }
@@ -268,7 +327,7 @@ bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
         procData(result);
         return true;
     }
-
+    // publishStationDetected(false);
     return false;
 }
 
@@ -325,6 +384,7 @@ void LidarScanController::procData(RechargeResult &result)
 
 void LidarScanController::controlRechargeMotion(const RechargeResult &result)
 {
+    publishStationDetected(true);
     // 如果检测到充电电压，停止移动
     if (getChargingVoltage())
     {

src/wheelchair_state_machine/src/workflow.cpp

@@ -16,6 +16,8 @@ WorkflowManager::WorkflowManager(rclcpp::Node *node)
     // 初始化旋转管理器
     initializeRotationManager();
 
+    initializeSubscriptions(); // 新增
+
     // 创建状态发布者
     state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
         "wheelchair/state", 10);
@@ -26,6 +28,64 @@ WorkflowManager::WorkflowManager(rclcpp::Node *node)
 
     RCLCPP_INFO(node_->get_logger(), "工作流管理器已初始化");
 }
+void WorkflowManager::initializeSubscriptions()
+{
+    // 创建基站检测订阅者
+    station_detected_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
+        "station/detected", 10,
+        [this](const std_msgs::msg::Bool::SharedPtr msg)
+        {
+            this->stationDetectedCallback(msg);
+        });
+
+    RCLCPP_INFO(node_->get_logger(), "基站检测订阅者已初始化");
+}
+
+void WorkflowManager::stationDetectedCallback(const std_msgs::msg::Bool::SharedPtr msg)
+{
+    try
+    {
+        bool station_detected = msg->data;
+        std::string current_state_str = getCurrentState();
+
+        RCLCPP_DEBUG(node_->get_logger(),
+                     "收到基站检测信号: %s, 当前状态: %s",
+                     station_detected ? "true" : "false",
+                     current_state_str.c_str());
+
+        // 只有在搜索或旋转状态下，且检测到基站时，才考虑状态转移
+        if (station_detected &&
+            (current_state_ == WheelchairState::SEARCHING ||
+             current_state_ == WheelchairState::ROTATING))
+        {
+            RCLCPP_INFO(node_->get_logger(),
+                        "搜索中检测到基站，切换到行走状态进行对接");
+
+            // 从搜索/旋转状态切换到行走状态
+            transitionToWalking();
+
+            // 发布状态更新
+            publishState("STATION_DETECTED", "检测到基站，开始移动对接");
+
+            // 发布回充状态
+            publishRechargeStatus("STATION_DETECTED",
+                                  "检测到充电基站，开始导航对接");
+        }
+        else if (!station_detected)
+        {
+            // 基站丢失处理（如果之前正在行走对接，但现在基站丢失）
+            if (current_state_ == WheelchairState::WALKING)
+            {
+                // 可以根据需要处理基站丢失的情况
+                RCLCPP_DEBUG(node_->get_logger(), "行走对接过程中基站信号丢失");
+            }
+        }
+    }
+    catch (const std::exception &e)
+    {
+        handleException("处理基站检测回调", e);
+    }
+}
 
 void WorkflowManager::initializeRotationManager()
 {
@@ -50,7 +110,7 @@ void WorkflowManager::initializeTransitionTable()
     std::map<WheelchairState, bool> ipad_cancel_perms = {
         {WheelchairState::READY, true},
         {WheelchairState::WALKING, true},
-        {WheelchairState::ROTATING, false}, // 旋转中❌
+        {WheelchairState::ROTATING, true}, // 旋转中❌
         {WheelchairState::SEARCHING, true},
         {WheelchairState::CHARGING, true}};
     transition_table_["ipad_phone_interface_cancel"] = ipad_cancel_perms;
@@ -199,6 +259,14 @@ void WorkflowManager::initializeTransitionTable()
         {WheelchairState::CHARGING, false}};
     transition_table_["search_30s_timeout"] = search_timeout_perms;
 
+    std::map<WheelchairState, bool> station_detected_perms = {
+        {WheelchairState::READY, false},
+        {WheelchairState::WALKING, false},
+        {WheelchairState::ROTATING, true},  // 旋转中✅
+        {WheelchairState::SEARCHING, true}, // 搜索中✅
+        {WheelchairState::CHARGING, false}};
+    transition_table_["station_detected"] = station_detected_perms;
+
     RCLCPP_INFO(node_->get_logger(), "状态转移表已初始化完成");
 }
 
@@ -316,6 +384,11 @@ void WorkflowManager::executeEvent(const std::string &event)
     {
         handleSearchTimeoutEvent();
     }
+    else if (event == "station_detected")
+    {
+        RCLCPP_INFO(node_->get_logger(), "处理基站检测成功事件");
+        // 实际上这个事件是通过回调处理的，这里只做日志记录
+    }
     else
     {
         RCLCPP_WARN(node_->get_logger(), "未实现的事件处理: %s", event.c_str());
@@ -362,7 +435,11 @@ void WorkflowManager::processStateTransition(const std::string &event)
             break;
         case WheelchairState::ROTATING:
             // 旋转中状态下，所有事件默认不处理，可以添加特定逻辑
-            RCLCPP_DEBUG(node_->get_logger(), "旋转中状态下忽略事件: %s", event.c_str());
+            if (event == "station_detected")
+            {
+                RCLCPP_INFO(node_->get_logger(), "旋转中检测到基站，转为行走对接");
+                transitionToWalking();
+            }
             break;
         case WheelchairState::SEARCHING:
             if (event == "base_station_lost")
@@ -377,6 +454,11 @@ void WorkflowManager::processStateTransition(const std::string &event)
             {
                 transitionToWalking();
             }
+            else if (event == "station_detected")
+            {
+                RCLCPP_INFO(node_->get_logger(), "搜索中检测到基站，转为行走对接");
+                transitionToWalking();
+            }
             // 搜索超时事件
             else if (event == "search_30s_timeout")
             {
@@ -528,9 +610,8 @@ void WorkflowManager::transitionToWalking()
         {
             rotation_manager_->stopRotation();
         }
-        if (current_state_ == WheelchairState::READY ||
+        if (
             current_state_ == WheelchairState::SEARCHING ||
-            current_state_ == WheelchairState::CHARGING ||
             current_state_ == WheelchairState::ROTATING)
         {
             setState(WheelchairState::WALKING);
@@ -671,7 +752,7 @@ void WorkflowManager::processIpadCancelEvent()
 
         // 根据当前状态决定是否取消回充
         if (current_state_ == WheelchairState::SEARCHING ||
-            current_state_ == WheelchairState::CHARGING)
+            current_state_ == WheelchairState::WALKING || current_state_ == WheelchairState::ROTATING)
         {
             transitionToReady();
             RCLCPP_INFO(node_->get_logger(), "iPad自主回充已取消，返回到就绪状态");
@@ -1346,10 +1427,14 @@ void WorkflowManager::setState(WheelchairState new_state)
                     "状态转移: %s -> %s",
                     old_state.c_str(),
                     getCurrentState().c_str());
+        // 增强状态发布：包含时间戳和前后状态
+        std::stringstream enhanced_state;
+        enhanced_state << "[" << node_->now().seconds() << "] "
+                       << old_state << " -> " << getCurrentState();
 
         // 发布状态到ROS话题
         auto msg = std_msgs::msg::String();
-        msg.data = getCurrentState();
+        msg.data = enhanced_state.str();
         state_publisher_->publish(msg);
 
         // 调用外部回调