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);

    // 创建回充超时报告发布者
    recharge_timeout_publisher_ = node_->create_publisher<std_msgs::msg::String>(
        "wheelchair/recharge_timeout", 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")
    {
        // 搜索超时事件
        handleSearchTimeoutEvent();
    }

    // 根据事件触发状态转移
    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 == "base_station_lost")
        {
            transitionToReady();
        }
        else if (event == "battery_start_charging")
        {
            transitionToCharging();
        }
        else if (event == "joystick_pull_back")
        {
            transitionToWalking();
        }
        // 搜索超时事件
        else if (event == "search_30s_timeout")
        {
            RCLCPP_WARN(node_->get_logger(), "搜索30秒超时，回充失败");
            // 触发回充失败处理
            handleRechargeFailure("搜索30秒超时");
        }
        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();

        // 发布回充开始通知
        publishRechargeStatus("RECHARGE_STARTED", "回充功能已启动，正在搜索充电站");
    }
}

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

        // 发布回充成功通知
        publishRechargeStatus("RECHARGE_SUCCESS", "成功对接充电站，开始充电");
    }
}

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自主回充已取消，返回到就绪状态");

        // 发布回充取消通知
        publishRechargeStatus("RECHARGE_CANCELLED", "用户取消回充");
    }
}

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();

        // 如果正在回充，发布回充失败通知
        if (current_state_ == WheelchairState::SEARCHING)
        {
            publishRechargeStatus("RECHARGE_FAILED", "蓝牙断开导致回充失败");
        }
    }
}

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();

        // 发布回充失败通知
        publishRechargeStatus("RECHARGE_FAILED", "基站断电导致回充失败");
    }
    else if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_ERROR(node_->get_logger(), "充电中基站断电!");
        transitionToReady();

        // 发布充电中断通知
        publishRechargeStatus("CHARGING_INTERRUPTED", "基站断电导致充电中断");
    }
}

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();

        // 发布回充取消通知
        publishRechargeStatus("RECHARGE_CANCELLED", "锁车操作取消回充");
    }
    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();

        // 发布回充取消通知
        publishRechargeStatus("RECHARGE_CANCELLED", "用户手动中断回充");
    }
}

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();

        // 发布回充取消通知
        publishRechargeStatus("RECHARGE_CANCELLED", "用户停止操作取消回充");
    }
}

void WorkflowManager::processPushStart()
{
    RCLCPP_INFO(node_->get_logger(), "处理推行启动");

    // 发布状态更新
    publishState("PUSH_START", "推行模式启动");

    // 根据状态处理推行启动
    if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_INFO(node_->get_logger(), "充电中启动推行，切换到行走模式");
        transitionToWalking();

        // 发布充电中断通知
        publishRechargeStatus("CHARGING_INTERRUPTED", "推行操作中断充电");
    }
}

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();

        // 发布充电停止通知
        publishRechargeStatus("CHARGING_STOPPED", "充电已停止");
    }
}

void WorkflowManager::handleChargingFullEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理电池充满事件");

    // 触发相应事件
    triggerEvent("battery_full");

    // 发布状态更新
    publishState("BATTERY_FULL", "电池已充满");

    // 停止充电，返回就绪状态
    if (current_state_ == WheelchairState::CHARGING)
    {
        transitionToReady();

        // 发布充电完成通知
        publishRechargeStatus("CHARGING_COMPLETED", "电池已充满，充电完成");
    }
}

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();

        // 发布回充失败通知
        publishRechargeStatus("RECHARGE_FAILED", "系统错误导致回充失败");
    }
}

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)
    {
        // 搜索中检测到基站丢失，返回就绪状态
        RCLCPP_WARN(node_->get_logger(), "搜索中基站丢失，返回就绪状态");
        transitionToReady();

        // 发布回充失败通知
        publishRechargeStatus("RECHARGE_FAILED", "基站丢失导致回充失败");
    }
    else if (current_state_ == WheelchairState::CHARGING)
    {
        RCLCPP_ERROR(node_->get_logger(), "充电中基站丢失，停止充电");
        transitionToReady();

        // 发布充电中断通知
        publishRechargeStatus("CHARGING_INTERRUPTED", "基站丢失导致充电中断");
    }
}

void WorkflowManager::handleSearchTimeoutEvent()
{
    RCLCPP_INFO(node_->get_logger(), "处理搜索超时事件");

    // 搜索超时30秒视为回充失败
    RCLCPP_WARN(node_->get_logger(), "搜索30秒超时，回充失败");

    // 发布回充失败通知
    publishRechargeStatus("RECHARGE_FAILED", "搜索充电站30秒超时");

    // 生成回充失败错误码
    publishRechargeErrorCode(0x51010101); // 回充检测失败（超时）

    // 触发基站丢失事件（保持向后兼容）
    handleBaseStationLostEvent();

    RCLCPP_INFO(node_->get_logger(), "搜索30秒超时，已作为回充失败事件处理");
}

void WorkflowManager::handleRechargeFailure(const std::string &reason)
{
    RCLCPP_WARN(node_->get_logger(), "回充失败: %s", reason.c_str());

    // 发布回充失败通知
    publishRechargeStatus("RECHARGE_FAILED", "回充失败: " + reason);

    // 生成回充失败错误码
    publishRechargeErrorCode(0x51010101); // 回充检测失败

    // 返回就绪状态
    transitionToReady();
}

// ==================== 回充状态管理 ====================

void WorkflowManager::publishRechargeStatus(const std::string &status, const std::string &message)
{
    // 发布到专用话题
    auto msg = std_msgs::msg::String();
    msg.data = status + ": " + message;
    recharge_timeout_publisher_->publish(msg);

    // 同时记录日志
    RCLCPP_INFO(node_->get_logger(), "[回充状态] %s: %s", status.c_str(), message.c_str());

    // 调用外部回调（如果有）
    if (recharge_status_callback_)
    {
        recharge_status_callback_(status, message);
    }
}

void WorkflowManager::publishRechargeErrorCode(uint32_t error_code)
{
    // 创建错误码消息
    auto error_msg = std_msgs::msg::UInt32();
    error_msg.data = error_code;

    // 发布到错误码话题
    // 注意：这里需要外部提供错误码发布者，或者通过回调传递

    RCLCPP_WARN(node_->get_logger(), "生成回充错误码: 0x%08X", error_code);

    // 调用外部错误处理回调（如果有）
    if (recharge_error_callback_)
    {
        ErrorInfo error_info;
        error_info.error_code = error_code;
        error_info.description = "回充功能失败";
        error_info.timestamp = node_->now().nanoseconds() / 1000000;
        recharge_error_callback_(error_info);
    }
}

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

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(), "搜索超时定时器已启动 (30秒)");
}

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();
    }
    else
    {
        RCLCPP_DEBUG(node_->get_logger(), "搜索超时定时器触发，但当前不在搜索状态");
        stopSearchTimeoutTimer();
    }
}

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

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::setRechargeStatusCallback(StateUpdateCallback callback)
{
    recharge_status_callback_ = callback;
    RCLCPP_INFO(node_->get_logger(), "已设置回充状态回调");
}

// 设置回充错误回调
void WorkflowManager::setRechargeErrorCallback(std::function<void(const ErrorInfo &)> callback)
{
    recharge_error_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);
}