|
|
@@ -1,935 +0,0 @@
|
|
|
-// wheelchair_state_machine.cpp
|
|
|
-#include "wheelchair_state_machine/wheelchair_state_machine.hpp"
|
|
|
-#include <rclcpp/logging.hpp>
|
|
|
-#include <rclcpp/qos.hpp>
|
|
|
-#include <cmath>
|
|
|
-
|
|
|
-using namespace std::chrono_literals;
|
|
|
-
|
|
|
-WheelchairStateMachine::WheelchairStateMachine()
|
|
|
- : Node("wheelchair_state_machine")
|
|
|
- , current_state_(WheelchairState::READY)
|
|
|
- , interface_active_(false)
|
|
|
- , battery_manager_(this, 20.0f, 10.0f)
|
|
|
- , recharge_controller_(this)
|
|
|
- , last_mode_("")
|
|
|
-{
|
|
|
- // 初始化状态转移表
|
|
|
- initializeTransitionTable();
|
|
|
-
|
|
|
- // 创建状态发布者
|
|
|
- state_publisher_ = this->create_publisher<std_msgs::msg::String>(
|
|
|
- "wheelchair/state", 10);
|
|
|
-
|
|
|
- cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
|
|
|
- "cmd_vel", 10);
|
|
|
-
|
|
|
- // 初始化电池管理器
|
|
|
- battery_manager_.setBatteryStateCallback(
|
|
|
- std::bind(&WheelchairStateMachine::onBatteryStateChanged, this, std::placeholders::_1, std::placeholders::_2));
|
|
|
- battery_manager_.setLowBatteryCallback(
|
|
|
- std::bind(&WheelchairStateMachine::onLowBattery, this, std::placeholders::_1, std::placeholders::_2));
|
|
|
- battery_manager_.initialize();
|
|
|
-
|
|
|
- // 初始化回充控制器
|
|
|
- recharge_controller_.initialize();
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "智能轮椅状态机（集成回充功能）已启动");
|
|
|
- RCLCPP_INFO(this->get_logger(), "当前状态: %s", getCurrentState().c_str());
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::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(this->get_logger(), "状态转移表已初始化完成");
|
|
|
-}
|
|
|
-
|
|
|
-bool WheelchairStateMachine::handleEvent(const std::string &event)
|
|
|
-{
|
|
|
- if (transition_table_.find(event) == transition_table_.end())
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->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() &&
|
|
|
- state_permissions[current_state_])
|
|
|
- {
|
|
|
- executeEvent(event);
|
|
|
- return true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(),
|
|
|
- "事件 '%s' 在当前状态 '%s' 下不允许执行",
|
|
|
- event.c_str(),
|
|
|
- getCurrentState().c_str());
|
|
|
- return false;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::executeEvent(const std::string &event)
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "执行事件: %s", event.c_str());
|
|
|
-
|
|
|
- if (event == "ipad_phone_interface_start")
|
|
|
- {
|
|
|
- handleIpadPhoneStart();
|
|
|
- }
|
|
|
- else if (event == "ipad_phone_interface_cancel")
|
|
|
- {
|
|
|
- handleIpadPhoneCancel();
|
|
|
- }
|
|
|
- else if (event == "bluetooth_disconnected")
|
|
|
- {
|
|
|
- handleBluetoothDisconnected();
|
|
|
- }
|
|
|
- else if (event == "bluetooth_connected")
|
|
|
- {
|
|
|
- handleBluetoothConnected();
|
|
|
- }
|
|
|
- else if (event == "base_station_power_off")
|
|
|
- {
|
|
|
- handleBaseStationPowerOff();
|
|
|
- }
|
|
|
- else if (event == "low_battery_warning")
|
|
|
- {
|
|
|
- handleLowBatteryWarning();
|
|
|
- }
|
|
|
- else if (event == "lock_vehicle")
|
|
|
- {
|
|
|
- handleLockVehicle();
|
|
|
- }
|
|
|
- else if (event == "unlock_vehicle")
|
|
|
- {
|
|
|
- handleUnlockVehicle();
|
|
|
- }
|
|
|
- else if (event == "joystick_pull_back")
|
|
|
- {
|
|
|
- handleJoystickPullBack();
|
|
|
- }
|
|
|
- else if (event == "joystick_stop")
|
|
|
- {
|
|
|
- handleJoystickStop();
|
|
|
- }
|
|
|
- else if (event == "push_start")
|
|
|
- {
|
|
|
- handlePushStart();
|
|
|
- }
|
|
|
- else if (event == "push_stop")
|
|
|
- {
|
|
|
- handlePushStop();
|
|
|
- }
|
|
|
- else if (event == "battery_start_charging")
|
|
|
- {
|
|
|
- handleBatteryStartCharging();
|
|
|
- }
|
|
|
- else if (event == "battery_stop_charging")
|
|
|
- {
|
|
|
- handleBatteryStopCharging();
|
|
|
- }
|
|
|
- else if (event == "battery_full")
|
|
|
- {
|
|
|
- handleBatteryFull();
|
|
|
- }
|
|
|
- else if (event == "error_code_handling")
|
|
|
- {
|
|
|
- handleErrorCode();
|
|
|
- }
|
|
|
- else if (event == "base_station_lost")
|
|
|
- {
|
|
|
- handleBaseStationLost();
|
|
|
- }
|
|
|
- else if (event == "search_30s_timeout")
|
|
|
- {
|
|
|
- handleSearchTimeout();
|
|
|
- }
|
|
|
-
|
|
|
- // 根据事件可能触发状态转移
|
|
|
- checkStateTransition(event);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::checkStateTransition(const std::string &event)
|
|
|
-{
|
|
|
- // 根据事件检查是否需要状态转移
|
|
|
- switch (current_state_)
|
|
|
- {
|
|
|
- case WheelchairState::READY:
|
|
|
- if (event == "low_battery_warning" || event == "ipad_phone_interface_start")
|
|
|
- {
|
|
|
- transitionToSearching();
|
|
|
- }
|
|
|
- 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" || getChargingVoltage())
|
|
|
- {
|
|
|
- transitionToCharging();
|
|
|
- }
|
|
|
- break;
|
|
|
-
|
|
|
- case WheelchairState::CHARGING:
|
|
|
- if (event == "battery_full" ||
|
|
|
- event == "battery_stop_charging" ||
|
|
|
- event == "base_station_power_off")
|
|
|
- {
|
|
|
- transitionToReady();
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-std::string WheelchairStateMachine::getCurrentState() const
|
|
|
-{
|
|
|
- return stateToString(current_state_);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::setState(WheelchairState new_state)
|
|
|
-{
|
|
|
- std::string old_state = getCurrentState();
|
|
|
- current_state_ = new_state;
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(),
|
|
|
- "状态转移: %s -> %s",
|
|
|
- old_state.c_str(),
|
|
|
- getCurrentState().c_str());
|
|
|
-
|
|
|
- // 发布状态话题
|
|
|
- auto msg = std_msgs::msg::String();
|
|
|
- msg.data = getCurrentState();
|
|
|
- state_publisher_->publish(msg);
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 状态转移函数 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::transitionToWalking()
|
|
|
-{
|
|
|
- if (current_state_ == WheelchairState::READY ||
|
|
|
- current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- setState(WheelchairState::WALKING);
|
|
|
- RCLCPP_INFO(this->get_logger(), "轮椅开始行走");
|
|
|
-
|
|
|
- // 发布状态通知
|
|
|
- publishStateUpdate("WHEELCHAIR_WALKING");
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::transitionToSearching()
|
|
|
-{
|
|
|
- if (current_state_ == WheelchairState::READY)
|
|
|
- {
|
|
|
- setState(WheelchairState::SEARCHING);
|
|
|
- RCLCPP_INFO(this->get_logger(), "开始搜索充电站");
|
|
|
-
|
|
|
- // 激活回充系统
|
|
|
- recharge_controller_.startRecharge();
|
|
|
- last_mode_ = "recharge";
|
|
|
-
|
|
|
- // 发布搜索开始通知
|
|
|
- publishStateUpdate("SEARCH_CHARGING_STATION_START");
|
|
|
-
|
|
|
- // 启动30秒超时定时器
|
|
|
- startSearchTimeoutTimer();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::transitionToCharging()
|
|
|
-{
|
|
|
- if (current_state_ == WheelchairState::SEARCHING || current_state_ == WheelchairState::WALKING)
|
|
|
- {
|
|
|
- setState(WheelchairState::CHARGING);
|
|
|
- RCLCPP_INFO(this->get_logger(), "开始充电");
|
|
|
-
|
|
|
- // 停止运动
|
|
|
- recharge_controller_.stopMotion();
|
|
|
-
|
|
|
- // 停止搜索超时定时器
|
|
|
- stopSearchTimeoutTimer();
|
|
|
-
|
|
|
- // 更新充电状态
|
|
|
- recharge_controller_.setChargingState(ChargingState::CHARGING);
|
|
|
-
|
|
|
- // 设置充电电压检测
|
|
|
- recharge_controller_.setChargingVoltage(true);
|
|
|
-
|
|
|
- // 发布充电开始通知
|
|
|
- publishStateUpdate("CHARGING_START");
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::transitionToReady()
|
|
|
-{
|
|
|
- // 可以从所有状态返回到就绪状态
|
|
|
- std::string previous_state = getCurrentState();
|
|
|
-
|
|
|
- // 停止回充系统
|
|
|
- recharge_controller_.cancelRecharge();
|
|
|
- last_mode_ = "";
|
|
|
-
|
|
|
- // 停止所有运动
|
|
|
- recharge_controller_.stopMotion();
|
|
|
-
|
|
|
- // 停止定时器
|
|
|
- stopSearchTimeoutTimer();
|
|
|
-
|
|
|
- setState(WheelchairState::READY);
|
|
|
-
|
|
|
- // 根据之前的状态执行清理操作
|
|
|
- if (previous_state == "搜索中")
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "停止搜索，返回到就绪状态");
|
|
|
- }
|
|
|
- else if (previous_state == "充电中")
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "停止充电，返回到就绪状态");
|
|
|
- recharge_controller_.setChargingVoltage(false);
|
|
|
- }
|
|
|
- else if (previous_state == "行走中")
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "停止行走，返回到就绪状态");
|
|
|
- }
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "返回到就绪状态");
|
|
|
-
|
|
|
- // 发布就绪状态通知
|
|
|
- publishStateUpdate("WHEELCHAIR_READY");
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 定时器相关函数 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::startSearchTimeoutTimer()
|
|
|
-{
|
|
|
- // 如果已经有定时器，先取消
|
|
|
- if (search_timeout_timer_)
|
|
|
- {
|
|
|
- search_timeout_timer_->cancel();
|
|
|
- }
|
|
|
-
|
|
|
- // 创建30秒定时器
|
|
|
- search_timeout_timer_ = this->create_wall_timer(
|
|
|
- std::chrono::seconds(30),
|
|
|
- [this]()
|
|
|
- {
|
|
|
- this->searchTimeoutCallback();
|
|
|
- });
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "搜索超时定时器已启动");
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::stopSearchTimeoutTimer()
|
|
|
-{
|
|
|
- if (search_timeout_timer_)
|
|
|
- {
|
|
|
- search_timeout_timer_->cancel();
|
|
|
- search_timeout_timer_.reset();
|
|
|
- RCLCPP_INFO(this->get_logger(), "搜索超时定时器已停止");
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::searchTimeoutCallback()
|
|
|
-{
|
|
|
- if (current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "搜索充电站超时30秒");
|
|
|
- handleSearchTimeout();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== iPad事件处理 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleIpadPhoneStart()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理iPad自主回充按钮点击");
|
|
|
-
|
|
|
- // 1. 检查当前状态
|
|
|
- std::string current_state_str = getCurrentState();
|
|
|
- RCLCPP_INFO(this->get_logger(), "当前状态: %s", current_state_str.c_str());
|
|
|
-
|
|
|
- // 2. 用户通过iPad点击了自主回充按钮
|
|
|
- RCLCPP_INFO(this->get_logger(), "用户通过iPad启动自主回充功能");
|
|
|
-
|
|
|
- // 3. 发布自主回充启动通知
|
|
|
- publishStateUpdate("AUTO_RECHARGE_INITIATED_BY_IPAD");
|
|
|
-
|
|
|
- // 4. 开始搜索充电站
|
|
|
- if (current_state_ == WheelchairState::READY)
|
|
|
- {
|
|
|
- // 直接进入搜索状态
|
|
|
- transitionToSearching();
|
|
|
-
|
|
|
- // 发布搜索开始的通知
|
|
|
- publishStateUpdate("AUTO_RECHARGE_SEARCH_STARTED");
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "iPad自主回充已启动，开始搜索充电站");
|
|
|
-
|
|
|
- // 显示系统状态
|
|
|
- displaySystemStatus();
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "当前不是就绪状态(%s)，无法启动自主回充",
|
|
|
- current_state_str.c_str());
|
|
|
-
|
|
|
- // 发布错误通知
|
|
|
- publishStateUpdate("AUTO_RECHARGE_FAILED_NOT_READY");
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleIpadPhoneCancel()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理iPad自主回充取消");
|
|
|
-
|
|
|
- // 记录取消前的状态
|
|
|
- std::string previous_state = getCurrentState();
|
|
|
-
|
|
|
- // 根据当前状态进行不同的取消处理
|
|
|
- switch (current_state_)
|
|
|
- {
|
|
|
- case WheelchairState::SEARCHING:
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "取消自主回充搜索");
|
|
|
-
|
|
|
- // 停止搜索，返回到就绪状态
|
|
|
- transitionToReady();
|
|
|
-
|
|
|
- // 发布取消通知
|
|
|
- publishStateUpdate("AUTO_RECHARGE_CANCELLED_IN_SEARCH");
|
|
|
- break;
|
|
|
- }
|
|
|
- case WheelchairState::WALKING:
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "在行走状态下取消自主回充");
|
|
|
- // 行走状态下取消，保持行走状态
|
|
|
- break;
|
|
|
- }
|
|
|
- default:
|
|
|
- break;
|
|
|
- }
|
|
|
-
|
|
|
- // 发布取消通知
|
|
|
- publishStateUpdate("AUTO_RECHARGE_CANCELLED");
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "自主回充已取消，前状态: %s", previous_state.c_str());
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 电池相关事件处理 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::onBatteryStateChanged(BatteryState state, float percentage)
|
|
|
-{
|
|
|
- // 处理电池状态变化
|
|
|
- switch (state)
|
|
|
- {
|
|
|
- case BatteryState::CHARGING:
|
|
|
- handleBatteryStartCharging();
|
|
|
- break;
|
|
|
- case BatteryState::FULL:
|
|
|
- handleBatteryFull();
|
|
|
- break;
|
|
|
- default:
|
|
|
- break;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::onLowBattery(BatteryState state, float percentage)
|
|
|
-{
|
|
|
- // 触发低电量警告
|
|
|
- handleLowBatteryWarning();
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleLowBatteryWarning()
|
|
|
-{
|
|
|
- BatteryState battery_state = battery_manager_.getBatteryState();
|
|
|
- float battery_percentage = battery_manager_.getBatteryPercentage();
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理低电量警告 - 当前电量: %.1f%%", battery_percentage);
|
|
|
-
|
|
|
- // 根据当前电池状态采取行动
|
|
|
- if (battery_state == BatteryState::CRITICAL)
|
|
|
- {
|
|
|
- RCLCPP_ERROR(this->get_logger(), "严重低电量! 自动启动回充搜索");
|
|
|
- publishStateUpdate("CRITICAL_BATTERY_EMERGENCY", "自动启动回充");
|
|
|
-
|
|
|
- // 紧急情况下，无论当前状态如何都尝试启动回充
|
|
|
- if (current_state_ == WheelchairState::READY)
|
|
|
- {
|
|
|
- transitionToSearching();
|
|
|
- }
|
|
|
- else if (current_state_ == WheelchairState::WALKING)
|
|
|
- {
|
|
|
- // 如果是行走状态，先停止行走再搜索
|
|
|
- RCLCPP_WARN(this->get_logger(), "严重低电量，停止行走并启动回充搜索");
|
|
|
- transitionToReady();
|
|
|
- rclcpp::sleep_for(std::chrono::seconds(1));
|
|
|
- transitionToSearching();
|
|
|
- }
|
|
|
- }
|
|
|
- else if (battery_state == BatteryState::LOW)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "低电量警告，建议启动回充");
|
|
|
- publishStateUpdate("LOW_BATTERY_WARNING", "建议启动回充");
|
|
|
-
|
|
|
- // 如果当前是就绪状态，自动启动回充
|
|
|
- if (current_state_ == WheelchairState::READY)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "自动启动回充搜索");
|
|
|
- transitionToSearching();
|
|
|
- }
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBatteryStartCharging()
|
|
|
-{
|
|
|
- float battery_percentage = battery_manager_.getBatteryPercentage();
|
|
|
- RCLCPP_INFO(this->get_logger(), "开始充电 - 当前电量: %.1f%%", battery_percentage);
|
|
|
-
|
|
|
- // 设置充电电压检测
|
|
|
- recharge_controller_.setChargingVoltage(true);
|
|
|
-
|
|
|
- // 如果当前在搜索状态，转移到充电状态
|
|
|
- if (current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- transitionToCharging();
|
|
|
- }
|
|
|
-
|
|
|
- // 发布充电开始通知
|
|
|
- publishStateUpdate("BATTERY_START_CHARGING");
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBatteryStopCharging()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "停止充电");
|
|
|
- recharge_controller_.setChargingVoltage(false);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBatteryFull()
|
|
|
-{
|
|
|
- float battery_percentage = battery_manager_.getBatteryPercentage();
|
|
|
- RCLCPP_INFO(this->get_logger(), "电池已充满 - 电量: %.1f%%", battery_percentage);
|
|
|
-
|
|
|
- // 停止充电，返回就绪状态
|
|
|
- if (current_state_ == WheelchairState::CHARGING)
|
|
|
- {
|
|
|
- transitionToReady();
|
|
|
- }
|
|
|
-
|
|
|
- publishStateUpdate("BATTERY_FULL");
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 其他事件处理函数 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBluetoothDisconnected()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理蓝牙断开");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBluetoothConnected()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理蓝牙连接成功");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBaseStationPowerOff()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理基站断电");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleLockVehicle()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理锁车操作");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleUnlockVehicle()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理解锁操作");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleJoystickPullBack()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理摇杆后拉操作");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleJoystickStop()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理摇杆停止操作");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handlePushStart()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理推行启动");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handlePushStop()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理推行停止");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleErrorCode()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "处理错误码");
|
|
|
- // 实现具体逻辑
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBaseStationLost()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "基站检测丢失");
|
|
|
-
|
|
|
- // 发布基站丢失通知
|
|
|
- publishStateUpdate("CHARGING_STATION_LOST");
|
|
|
-
|
|
|
- // 根据状态处理基站丢失
|
|
|
- if (current_state_ == WheelchairState::WALKING)
|
|
|
- {
|
|
|
- // 行走中检测到基站丢失，进入搜索状态
|
|
|
- transitionToSearching();
|
|
|
- }
|
|
|
- else if (current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- // 搜索中检测到基站丢失，返回就绪状态
|
|
|
- transitionToReady();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleSearchTimeout()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "搜索30秒超时");
|
|
|
-
|
|
|
- // 发布超时通知
|
|
|
- publishStateUpdate("SEARCH_CHARGING_STATION_TIMEOUT");
|
|
|
-
|
|
|
- // 根据状态转移表，搜索超时应返回就绪状态
|
|
|
- if (current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- transitionToReady();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 辅助函数实现 ====================
|
|
|
-
|
|
|
-std::string WheelchairStateMachine::stateToString(WheelchairState state) const
|
|
|
-{
|
|
|
- switch (state)
|
|
|
- {
|
|
|
- case WheelchairState::READY:
|
|
|
- return "就绪中";
|
|
|
- case WheelchairState::WALKING:
|
|
|
- return "行走中";
|
|
|
- case WheelchairState::SEARCHING:
|
|
|
- return "搜索中";
|
|
|
- case WheelchairState::CHARGING:
|
|
|
- return "充电中";
|
|
|
- default:
|
|
|
- return "未知状态";
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-std::string WheelchairStateMachine::chargingStateToString(ChargingState state) const
|
|
|
-{
|
|
|
- switch (state)
|
|
|
- {
|
|
|
- case ChargingState::IDLE:
|
|
|
- return "空闲";
|
|
|
- case ChargingState::NAVIGATING:
|
|
|
- return "导航中";
|
|
|
- case ChargingState::CHARGING:
|
|
|
- return "充电中";
|
|
|
- case ChargingState::UNAVAILABLE:
|
|
|
- return "不可用";
|
|
|
- case ChargingState::COMPLETED:
|
|
|
- return "完成";
|
|
|
- default:
|
|
|
- return "未知";
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-std::string WheelchairStateMachine::batteryStateToString(BatteryState state) const
|
|
|
-{
|
|
|
- switch (state)
|
|
|
- {
|
|
|
- case BatteryState::NORMAL:
|
|
|
- return "正常";
|
|
|
- case BatteryState::LOW:
|
|
|
- return "低电量";
|
|
|
- case BatteryState::CRITICAL:
|
|
|
- return "严重低电量";
|
|
|
- case BatteryState::CHARGING:
|
|
|
- return "充电中";
|
|
|
- case BatteryState::FULL:
|
|
|
- return "充满";
|
|
|
- default:
|
|
|
- return "未知";
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::displaySystemStatus()
|
|
|
-{
|
|
|
- std::string status_report = "系统状态报告:\n";
|
|
|
-
|
|
|
- // 轮椅状态
|
|
|
- status_report += " - 轮椅状态: " + getCurrentState() + "\n";
|
|
|
-
|
|
|
- // 电池状态
|
|
|
- status_report += " - 电池状态: " + battery_manager_.stateToString() +
|
|
|
- " (" + std::to_string(battery_manager_.getBatteryPercentage()) + "%)\n";
|
|
|
-
|
|
|
- // 充电状态
|
|
|
- status_report += " - 充电状态: " + chargingStateToString(recharge_controller_.getChargingState()) + "\n";
|
|
|
-
|
|
|
- // 回充状态
|
|
|
- if (current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- status_report += " - 回充状态: 正在搜索充电站\n";
|
|
|
- }
|
|
|
- else if (current_state_ == WheelchairState::CHARGING)
|
|
|
- {
|
|
|
- status_report += " - 回充状态: 正在充电\n";
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- status_report += " - 回充状态: 待命\n";
|
|
|
- }
|
|
|
-
|
|
|
- // 控制模式
|
|
|
- status_report += " - 控制模式: ";
|
|
|
- status_report += (interface_active_ ? "iPad远程控制" : "本地控制");
|
|
|
- status_report += "\n";
|
|
|
-
|
|
|
- // 充电电压检测
|
|
|
- status_report += " - 充电电压: " + std::string(getChargingVoltage() ? "检测到" : "未检测") + "\n";
|
|
|
-
|
|
|
- // 安全状态
|
|
|
- status_report += " - 安全状态: 正常\n";
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "\n%s", status_report.c_str());
|
|
|
-
|
|
|
- // 发布到状态话题
|
|
|
- auto status_msg = std_msgs::msg::String();
|
|
|
- status_msg.data = status_report;
|
|
|
- state_publisher_->publish(status_msg);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::publishStateUpdate(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);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::setMode(const std::string &mode)
|
|
|
-{
|
|
|
- last_mode_ = mode;
|
|
|
- if (last_mode_ != "recharge")
|
|
|
- {
|
|
|
- recharge_controller_.setChargingVoltage(false);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-std::string WheelchairStateMachine::getMode() const
|
|
|
-{
|
|
|
- return last_mode_;
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::setChargingVoltage(bool value)
|
|
|
-{
|
|
|
- recharge_controller_.setChargingVoltage(value);
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 回充控制接口 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::startRecharge()
|
|
|
-{
|
|
|
- if (current_state_ == WheelchairState::READY)
|
|
|
- {
|
|
|
- handleIpadPhoneStart();
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "当前状态无法启动回充: %s", getCurrentState().c_str());
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::cancelRecharge()
|
|
|
-{
|
|
|
- handleIpadPhoneCancel();
|
|
|
-}
|
sousenw