recharge/src3/wheelchair_state_machine/src/workflow.cpp

// workflow.cpp
#include "wheelchair_state_machine/workflow.hpp"
#include <rclcpp/logging.hpp>
#include <chrono>

using namespace std::chrono_literals;
using std::placeholders::_1;

WorkflowManager::WorkflowManager(rclcpp::Node* node)
    : node_(node)
    , current_state_(WheelchairState::READY)
{
    // 初始化状态转移表
    initializeTransitionTable();
    
    // 创建状态发布者
    state_publisher_ = node_->create_publisher<std_msgs::msg::String>(
        "wheelchair/state", 10);
    
    RCLCPP_INFO(node_->get_logger(), "工作流管理器已初始化");
}

void WorkflowManager::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(node_->get_logger(), "状态转移表已初始化完成");
}

bool WorkflowManager::handleEvent(const std::string &event)
{
    if (!checkEventPermission(event))
    {
        RCLCPP_WARN(node_->get_logger(),
                    "事件 '%s' 在当前状态 '%s' 下不允许执行",
                    event.c_str(),
                    getCurrentState().c_str());
        return false;
    }
    
    executeEvent(event);
    return true;
}

bool WorkflowManager::checkEventPermission(const std::string &event)
{
    if (transition_table_.find(event) == transition_table_.end())
    {
        RCLCPP_WARN(node_->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())
    {
        return state_permissions[current_state_];
    }
    
    return false;
}

void WorkflowManager::executeEvent(const std::string &event)
{
    RCLCPP_INFO(node_->get_logger(), "执行事件: %s", event.c_str());
    
    // 根据事件类型执行相应处理
    if (event == "ipad_phone_interface_start")
    {
        processIpadStartEvent();
    }
    else if (event == "ipad_phone_interface_cancel")
    {
        processIpadCancelEvent();
    }
    else if (event == "bluetooth_disconnected")
    {
        processBluetoothDisconnected();
    }
    else if (event == "bluetooth_connected")
    {
        processBluetoothConnected();
    }
    else if (event == "base_station_power_off")
    {
        processBaseStationPowerOff();
    }
    else if (event == "low_battery_warning")
    {
        // 由外部回调处理
    }
    else if (event == "lock_vehicle")
    {
        processLockVehicle();
    }
    else if (event == "unlock_vehicle")
    {
        processUnlockVehicle();
    }
    else if (event == "joystick_pull_back")
    {
        processJoystickPullBack();
    }
    else if (event == "joystick_stop")
    {
        processJoystickStop();
    }
    else if (event == "push_start")
    {
        processPushStart();
    }
    else if (event == "push_stop")
    {
        processPushStop();
    }
    else if (event == "battery_start_charging")
    {
        // 由外部回调处理
    }
    else if (event == "battery_stop_charging")
    {
        // 由外部回调处理
    }
    else if (event == "battery_full")
    {
        // 由外部回调处理
    }
    else if (event == "error_code_handling")
    {
        processErrorCode();
    }
    else if (event == "base_station_lost")
    {
        // 由外部回调处理
    }
    else if (event == "search_30s_timeout")
    {
        // 由外部回调处理
    }
    
    // 根据事件触发状态转移
    processStateTransition(event);
    
    // 发布状态更新
    publishState(event, "事件已处理");
}

void WorkflowManager::processStateTransition(const std::string &event)
{
    // 根据事件检查是否需要状态转移
    switch (current_state_)
    {
    case WheelchairState::READY:
        if (event == "low_battery_warning" || event == "ipad_phone_interface_start")
        {
            transitionToSearching();
        }
        else if (event == "unlock_vehicle")
        {
            transitionToWalking();
        }
        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")
        {
            transitionToCharging();
        }
        else if (event == "joystick_pull_back")
        {
            transitionToWalking();
        }
        break;
        
    case WheelchairState::CHARGING:
        if (event == "battery_full" ||
            event == "battery_stop_charging" ||
            event == "base_station_power_off")
        {
            transitionToReady();
        }
        else if (event == "push_start")
        {
            transitionToWalking();
        }
        break;
    }
}

void WorkflowManager::transitionToWalking()
{
    if (current_state_ == WheelchairState::READY ||
        current_state_ == WheelchairState::SEARCHING ||
        current_state_ == WheelchairState::CHARGING)
    {
        setState(WheelchairState::WALKING);
        RCLCPP_INFO(node_->get_logger(), "轮椅开始行走");
        publishState("WHEELCHAIR_WALKING");
    }
}

void WorkflowManager::transitionToSearching()
{
    if (current_state_ == WheelchairState::READY ||
        current_state_ == WheelchairState::WALKING)
    {
        setState(WheelchairState::SEARCHING);
        RCLCPP_INFO(node_->get_logger(), "开始搜索充电站");
        publishState("SEARCH_CHARGING_STATION_START");
        startSearchTimeoutTimer();
    }
}

void WorkflowManager::transitionToCharging()
{
    if (current_state_ == WheelchairState::SEARCHING || 
        current_state_ == WheelchairState::WALKING)
    {
        setState(WheelchairState::CHARGING);
        RCLCPP_INFO(node_->get_logger(), "开始充电");
        stopSearchTimeoutTimer();
        publishState("CHARGING_START");
    }
}

void WorkflowManager::transitionToReady()
{
    std::string previous_state = getCurrentState();
    setState(WheelchairState::READY);
    stopSearchTimeoutTimer();
    RCLCPP_INFO(node_->get_logger(), "返回到就绪状态 (前状态: %s)", 
                previous_state.c_str());
    publishState("WHEELCHAIR_READY");
}

// ==================== 事件处理函数实现 ====================

void WorkflowManager::processIpadStartEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理iPad启动事件");
    
    // 发布状态更新
    publishState("IPAD_RECHARGE_STARTED", "iPad启动自主回充");
    
    // 如果当前是就绪状态，自动转移到搜索状态
    if (current_state_ == WheelchairState::READY)
    {
        transitionToSearching();
    }
    else
    {
        RCLCPP_WARN(node_->get_logger(), 
                   "当前状态 %s 无法启动回充", getCurrentState().c_str());
    }
}

void WorkflowManager::processIpadCancelEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理iPad取消事件");
    
    // 发布状态更新
    publishState("IPAD_RECHARGE_CANCELLED", "iPad取消自主回充");
    
    // 根据当前状态决定是否取消回充
    if (current_state_ == WheelchairState::SEARCHING || 
        current_state_ == WheelchairState::CHARGING)
    {
        transitionToReady();
        RCLCPP_INFO(node_->get_logger(), "iPad自主回充已取消，返回到就绪状态");
    }
}

void WorkflowManager::processBluetoothDisconnected()
{
    RCLCPP_INFO(node_->get_logger(), "处理蓝牙断开事件");
    
    // 发布状态更新
    publishState("BLUETOOTH_DISCONNECTED", "蓝牙连接已断开");
    
    // 根据状态处理蓝牙断开
    if (current_state_ == WheelchairState::WALKING ||
        current_state_ == WheelchairState::SEARCHING ||
        current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_WARN(node_->get_logger(), "蓝牙断开，返回到就绪状态");
        transitionToReady();
    }
}

void WorkflowManager::processBluetoothConnected()
{
    RCLCPP_INFO(node_->get_logger(), "处理蓝牙连接成功事件");
    
    // 发布状态更新
    publishState("BLUETOOTH_CONNECTED", "蓝牙连接成功");
    
    // 蓝牙连接成功通常不需要改变当前状态
    // 但可以根据业务需求添加逻辑
}

void WorkflowManager::processBaseStationPowerOff()
{
    RCLCPP_INFO(node_->get_logger(), "处理基站断电事件");
    
    // 发布状态更新
    publishState("BASE_STATION_POWER_OFF", "充电基站断电");
    
    // 根据状态处理基站断电
    if (current_state_ == WheelchairState::SEARCHING)
    {
        RCLCPP_WARN(node_->get_logger(), "基站断电，停止搜索");
        transitionToReady();
    }
    else if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_ERROR(node_->get_logger(), "充电中基站断电!");
        transitionToReady();
    }
}

void WorkflowManager::handleLowBatteryEvent(BatteryState state, float percentage)
{
    RCLCPP_INFO(node_->get_logger(), "处理低电量事件: %.1f%%", percentage);
    
    // 发布相应的事件名称
    if (state == BatteryState::CRITICAL)
    {
        triggerEvent("low_battery_warning");
    }
    else if (state == BatteryState::LOW)
    {
        triggerEvent("low_battery_warning");
    }
    
    // 根据电池状态采取行动
    if (state == BatteryState::CRITICAL)
    {
        RCLCPP_ERROR(node_->get_logger(), "严重低电量! 自动启动回充搜索");
        publishState("CRITICAL_BATTERY_EMERGENCY", "自动启动回充");
        
        // 紧急情况下，无论当前状态如何都尝试启动回充
        if (current_state_ == WheelchairState::READY)
        {
            transitionToSearching();
        }
        else if (current_state_ == WheelchairState::WALKING)
        {
            // 如果是行走状态，先停止行走再搜索
            RCLCPP_WARN(node_->get_logger(), "严重低电量，停止行走并启动回充搜索");
            transitionToReady();
            // 短暂延迟后启动搜索
            // 注意：在实际应用中可能需要异步处理
            transitionToSearching();
        }
    }
    else if (state == BatteryState::LOW)
    {
        RCLCPP_WARN(node_->get_logger(), "低电量警告，建议启动回充");
        publishState("LOW_BATTERY_WARNING", "建议启动回充");
        
        // 如果当前是就绪状态，自动启动回充
        if (current_state_ == WheelchairState::READY)
        {
            RCLCPP_INFO(node_->get_logger(), "自动启动回充搜索");
            transitionToSearching();
        }
    }
}

void WorkflowManager::processLockVehicle()
{
    RCLCPP_INFO(node_->get_logger(), "处理锁车操作");
    
    // 发布状态更新
    publishState("VEHICLE_LOCKED", "轮椅已锁定");
    
    // 根据状态处理锁车
    if (current_state_ == WheelchairState::WALKING)
    {
        RCLCPP_WARN(node_->get_logger(), "行走中锁车，停止运动");
        transitionToReady();
    }
    else if (current_state_ == WheelchairState::SEARCHING)
    {
        RCLCPP_WARN(node_->get_logger(), "搜索中锁车，取消搜索");
        transitionToReady();
    }
    else if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_WARN(node_->get_logger(), "充电中锁车，保持充电状态");
        // 充电中可以锁车，保持当前状态
    }
}

void WorkflowManager::processUnlockVehicle()
{
    RCLCPP_INFO(node_->get_logger(), "处理解锁操作");
    
    // 发布状态更新
    publishState("VEHICLE_UNLOCKED", "轮椅已解锁");
    
    // 根据状态处理解锁
    if (current_state_ == WheelchairState::READY)
    {
        RCLCPP_INFO(node_->get_logger(), "解锁后准备行走");
        transitionToWalking();
    }
}

void WorkflowManager::processJoystickPullBack()
{
    RCLCPP_INFO(node_->get_logger(), "处理摇杆后拉操作");
    
    // 发布状态更新
    publishState("JOYSTICK_PULL_BACK", "摇杆后拉");
    
    // 根据状态处理摇杆后拉
    if (current_state_ == WheelchairState::SEARCHING)
    {
        RCLCPP_INFO(node_->get_logger(), "搜索中摇杆后拉，切换为行走模式");
        transitionToWalking();
    }
}

void WorkflowManager::processJoystickStop()
{
    RCLCPP_INFO(node_->get_logger(), "处理摇杆停止操作");
    
    // 发布状态更新
    publishState("JOYSTICK_STOP", "摇杆停止");
    
    // 根据状态处理摇杆停止
    if (current_state_ == WheelchairState::WALKING)
    {
        RCLCPP_INFO(node_->get_logger(), "行走中摇杆停止，返回就绪状态");
        transitionToReady();
    }
    else if (current_state_ == WheelchairState::SEARCHING)
    {
        RCLCPP_INFO(node_->get_logger(), "搜索中摇杆停止，返回就绪状态");
        transitionToReady();
    }
}

void WorkflowManager::processPushStart()
{
    RCLCPP_INFO(node_->get_logger(), "处理推行启动");
    
    // 发布状态更新
    publishState("PUSH_START", "推行模式启动");
    
    // 根据状态处理推行启动
    if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_INFO(node_->get_logger(), "充电中启动推行，切换到行走模式");
        transitionToWalking();
    }
}

void WorkflowManager::processPushStop()
{
    RCLCPP_INFO(node_->get_logger(), "处理推行停止");
    
    // 发布状态更新
    publishState("PUSH_STOP", "推行模式停止");
    
    // 根据状态处理推行停止
    if (current_state_ == WheelchairState::WALKING)
    {
        RCLCPP_INFO(node_->get_logger(), "推行中停止，返回就绪状态");
        transitionToReady();
    }
    else if (current_state_ == WheelchairState::SEARCHING)
    {
        RCLCPP_INFO(node_->get_logger(), "搜索中停止推行，返回就绪状态");
        transitionToReady();
    }
}

void WorkflowManager::handleChargingStartEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理充电开始事件");
    
    // 触发相应事件
    triggerEvent("battery_start_charging");
    
    // 发布状态更新
    publishState("BATTERY_START_CHARGING", "开始充电");
    
    // 如果当前在搜索状态，转移到充电状态
    if (current_state_ == WheelchairState::SEARCHING)
    {
        transitionToCharging();
    }
    else if (current_state_ == WheelchairState::WALKING)
    {
        RCLCPP_WARN(node_->get_logger(), "行走中开始充电，切换到充电状态");
        transitionToCharging();
    }
}

void WorkflowManager::handleChargingStopEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理充电停止事件");
    
    // 触发相应事件
    triggerEvent("battery_stop_charging");
    
    // 发布状态更新
    publishState("BATTERY_STOP_CHARGING", "停止充电");
    
    // 如果当前在充电状态，返回就绪状态
    if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_WARN(node_->get_logger(), "充电停止，返回就绪状态");
        transitionToReady();
    }
}

void WorkflowManager::handleChargingFullEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理电池充满事件");
    
    // 触发相应事件
    triggerEvent("battery_full");
    
    // 发布状态更新
    publishState("BATTERY_FULL", "电池已充满");
    
    // 停止充电，返回就绪状态
    if (current_state_ == WheelchairState::CHARGING)
    {
        transitionToReady();
    }
}

void WorkflowManager::processErrorCode()
{
    RCLCPP_INFO(node_->get_logger(), "处理错误码");
    
    // 发布状态更新
    publishState("ERROR_CODE_HANDLING", "处理系统错误码");
    
    // 根据错误码的严重程度处理
    // 严重错误可能需要紧急停止并返回就绪状态
    bool critical_error = true; // 示例：假设是严重错误
    
    if (critical_error)
    {
        RCLCPP_ERROR(node_->get_logger(), "检测到严重错误，紧急停止所有操作");
        transitionToReady();
    }
}

void WorkflowManager::handleBaseStationLostEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理基站丢失事件");
    
    // 触发相应事件
    triggerEvent("base_station_lost");
    
    // 发布状态更新
    publishState("CHARGING_STATION_LOST", "充电基站丢失");
    
    // 根据状态处理基站丢失
    if (current_state_ == WheelchairState::WALKING)
    {
        // 行走中检测到基站丢失，进入搜索状态
        transitionToSearching();
    }
    else if (current_state_ == WheelchairState::SEARCHING)
    {
        // 搜索中检测到基站丢失，返回就绪状态
        transitionToReady();
    }
    else if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_ERROR(node_->get_logger(), "充电中基站丢失，停止充电");
        transitionToReady();
    }
}

void WorkflowManager::handleSearchTimeoutEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理搜索超时事件");
    
    // 触发相应事件
    triggerEvent("search_30s_timeout");
    
    // 发布超时通知
    publishState("SEARCH_CHARGING_STATION_TIMEOUT", "搜索充电站超时");
    
    // 根据状态转移表，搜索超时应返回就绪状态
    if (current_state_ == WheelchairState::SEARCHING)
    {
        transitionToReady();
    }
}

// ==================== 定时器管理 ====================

void WorkflowManager::startSearchTimeoutTimer()
{
    // 如果已经有定时器，先取消
    if (search_timeout_timer_)
    {
        search_timeout_timer_->cancel();
    }
    
    // 创建30秒定时器
    search_timeout_timer_ = node_->create_wall_timer(
        std::chrono::seconds(30),
        [this]()
        {
            this->searchTimeoutCallback();
        });
    
    RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已启动");
}

void WorkflowManager::stopSearchTimeoutTimer()
{
    if (search_timeout_timer_)
    {
        search_timeout_timer_->cancel();
        search_timeout_timer_.reset();
        RCLCPP_INFO(node_->get_logger(), "搜索超时定时器已停止");
    }
}

void WorkflowManager::searchTimeoutCallback()
{
    if (current_state_ == WheelchairState::SEARCHING)
    {
        RCLCPP_WARN(node_->get_logger(), "搜索充电站超时30秒");
        handleSearchTimeoutEvent();
    }
}

// ==================== 辅助函数 ====================

std::string WorkflowManager::getCurrentState() const
{
    return stateToString(current_state_);
}

void WorkflowManager::setState(WheelchairState new_state)
{
    std::string old_state = getCurrentState();
    current_state_ = new_state;
    
    RCLCPP_INFO(node_->get_logger(),
                "状态转移: %s -> %s",
                old_state.c_str(),
                getCurrentState().c_str());
    
    // 发布状态到ROS话题
    auto msg = std_msgs::msg::String();
    msg.data = getCurrentState();
    state_publisher_->publish(msg);
    
    // 调用外部回调
    if (state_update_callback_)
    {
        state_update_callback_(getCurrentState(), "状态已更新");
    }
}

std::string WorkflowManager::stateToString(WheelchairState state) const
{
    switch (state)
    {
    case WheelchairState::READY:
        return "就绪中";
    case WheelchairState::WALKING:
        return "行走中";
    case WheelchairState::SEARCHING:
        return "搜索中";
    case WheelchairState::CHARGING:
        return "充电中";
    default:
        return "未知状态";
    }
}

void WorkflowManager::publishState(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);
    
    // 调用外部回调
    if (state_update_callback_)
    {
        state_update_callback_(state_str, message);
    }
}

void WorkflowManager::setStateUpdateCallback(StateUpdateCallback callback)
{
    state_update_callback_ = callback;
    RCLCPP_INFO(node_->get_logger(), "已设置状态更新回调");
}

// ==================== 内部事件触发 ====================

void WorkflowManager::triggerEvent(const std::string &event_name)
{
    // 检查事件权限
    if (!checkEventPermission(event_name))
    {
        RCLCPP_WARN(node_->get_logger(),
                    "内部触发事件 '%s' 在当前状态 '%s' 下不允许执行",
                    event_name.c_str(),
                    getCurrentState().c_str());
        return;
    }
    
    // 执行事件
    executeEvent(event_name);
}