|
|
@@ -1,2366 +0,0 @@
|
|
|
-// wheelchair_state_machine.cpp
|
|
|
-#include "wheelchair_state_machine/wheelchair_state_machine.hpp"
|
|
|
-#include <rclcpp/logging.hpp>
|
|
|
-#include <rclcpp/qos.hpp>
|
|
|
-#include <tf2/LinearMath/Quaternion.h>
|
|
|
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
|
|
-#include <cmath>
|
|
|
-#include <limits>
|
|
|
-#include <deque>
|
|
|
-#include <numeric>
|
|
|
-#include <thread>
|
|
|
-#include <Eigen/Dense>
|
|
|
-
|
|
|
-using namespace std::chrono_literals;
|
|
|
-using std::placeholders::_1;
|
|
|
-
|
|
|
-// ==================== 工具类实现 ====================
|
|
|
-
|
|
|
-double RechargeTool::angleBetween(const IndexPoint &p1, const IndexPoint &p2)
|
|
|
-{
|
|
|
- return atan2(p2.y - p1.y, p2.x - p1.x);
|
|
|
-}
|
|
|
-
|
|
|
-double RechargeTool::angleDiff(double a1, double a2)
|
|
|
-{
|
|
|
- double diff = fabs(a1 - a2);
|
|
|
- return std::min(diff, 2 * M_PI - diff) * 180.0 / M_PI;
|
|
|
-}
|
|
|
-
|
|
|
-double RechargeTool::averageAngle(const std::vector<double> &angles)
|
|
|
-{
|
|
|
- double sin_sum = 0.0, cos_sum = 0.0;
|
|
|
- for (double a : angles)
|
|
|
- {
|
|
|
- sin_sum += sin(a);
|
|
|
- cos_sum += cos(a);
|
|
|
- }
|
|
|
- return atan2(sin_sum, cos_sum);
|
|
|
-}
|
|
|
-
|
|
|
-std::vector<std::vector<IndexPoint>> RechargeTool::splitPointsToSegments(
|
|
|
- const std::vector<IndexPoint> &points,
|
|
|
- double angle_threshold_deg,
|
|
|
- int history_window_size)
|
|
|
-{
|
|
|
- std::vector<std::vector<IndexPoint>> segments;
|
|
|
- if (points.size() < 2)
|
|
|
- {
|
|
|
- segments.push_back(points);
|
|
|
- return segments;
|
|
|
- }
|
|
|
-
|
|
|
- std::vector<IndexPoint> current_segment;
|
|
|
- std::deque<double> angle_history;
|
|
|
-
|
|
|
- current_segment.push_back(points[0]);
|
|
|
-
|
|
|
- for (size_t i = 1; i < points.size(); i++)
|
|
|
- {
|
|
|
- const IndexPoint &p_prev = points[i - 1];
|
|
|
- const IndexPoint &p_curr = points[i];
|
|
|
- double curr_angle = angleBetween(p_prev, p_curr);
|
|
|
-
|
|
|
- // 更新方向历史
|
|
|
- angle_history.push_back(curr_angle);
|
|
|
- if (angle_history.size() > history_window_size)
|
|
|
- {
|
|
|
- angle_history.pop_front();
|
|
|
- }
|
|
|
-
|
|
|
- // 若历史方向足够，开始判断角度差
|
|
|
- if (angle_history.size() >= history_window_size)
|
|
|
- {
|
|
|
- // 计算历史方向平均值（排除当前角度）
|
|
|
- std::vector<double> hist(angle_history.begin(), angle_history.end() - 1);
|
|
|
- double avg_angle = averageAngle(hist);
|
|
|
- double diff = angleDiff(avg_angle, curr_angle);
|
|
|
-
|
|
|
- if (diff > angle_threshold_deg)
|
|
|
- {
|
|
|
- // 发生显著拐弯，分段
|
|
|
- current_segment.push_back(p_prev);
|
|
|
- segments.push_back(current_segment);
|
|
|
-
|
|
|
- // 开始新分段，以当前点为起点
|
|
|
- current_segment.clear();
|
|
|
- current_segment.push_back(p_curr);
|
|
|
-
|
|
|
- // 重置方向历史，仅保留当前角度
|
|
|
- angle_history.clear();
|
|
|
- angle_history.push_back(curr_angle);
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- current_segment.push_back(points.back());
|
|
|
- segments.push_back(current_segment);
|
|
|
-
|
|
|
- return segments;
|
|
|
-}
|
|
|
-
|
|
|
-std::pair<double, double> RechargeTool::calculateAngleBisector(double k1, double b1, double k2, double b2)
|
|
|
-{
|
|
|
- // 计算两条直线的夹角
|
|
|
- double angle1 = atan(k1);
|
|
|
- double angle2 = atan(k2);
|
|
|
-
|
|
|
- // 计算两条直线之间的夹角
|
|
|
- double delta_angle = fabs(angle1 - angle2);
|
|
|
-
|
|
|
- // 计算中线角度
|
|
|
- double bisector_angle1;
|
|
|
- if (angle1 < angle2)
|
|
|
- {
|
|
|
- bisector_angle1 = angle1 + delta_angle / 2;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- bisector_angle1 = angle2 + delta_angle / 2;
|
|
|
- }
|
|
|
-
|
|
|
- // 处理平行线（无交点）的情况
|
|
|
- if (fabs(k1 - k2) < 1e-10)
|
|
|
- {
|
|
|
- // 平行线没有交点，返回原直线
|
|
|
- return {k1, b1};
|
|
|
- }
|
|
|
-
|
|
|
- // 计算交点坐标
|
|
|
- double x_intersect = (b2 - b1) / (k1 - k2);
|
|
|
- double y_intersect = k1 * x_intersect + b1;
|
|
|
-
|
|
|
- // 计算平分线的斜率
|
|
|
- double k3 = tan(bisector_angle1);
|
|
|
- double b3 = y_intersect - k3 * x_intersect;
|
|
|
-
|
|
|
- return {k3, b3};
|
|
|
-}
|
|
|
-
|
|
|
-std::pair<double, double> RechargeTool::fitLineLeastSquares(const Eigen::MatrixXd &points)
|
|
|
-{
|
|
|
- int n = points.rows();
|
|
|
- Eigen::VectorXd x = points.col(0);
|
|
|
- Eigen::VectorXd y = points.col(1);
|
|
|
-
|
|
|
- // 最小二乘法拟合 y = kx + b
|
|
|
- double x_mean = x.mean();
|
|
|
- double y_mean = y.mean();
|
|
|
-
|
|
|
- double numerator = 0.0;
|
|
|
- double denominator = 0.0;
|
|
|
-
|
|
|
- for (int i = 0; i < n; i++)
|
|
|
- {
|
|
|
- numerator += (x(i) - x_mean) * (y(i) - y_mean);
|
|
|
- denominator += (x(i) - x_mean) * (x(i) - x_mean);
|
|
|
- }
|
|
|
-
|
|
|
- double k = numerator / denominator;
|
|
|
- double b = y_mean - k * x_mean;
|
|
|
-
|
|
|
- return {k, b};
|
|
|
-}
|
|
|
-
|
|
|
-std::pair<double, double> RechargeTool::computeDockingVelocity(double dx, double dy, double yaw_error)
|
|
|
-{
|
|
|
- double max_linear_speed = 0.12;
|
|
|
- double max_angular_speed = 0.16;
|
|
|
-
|
|
|
- double Kp_linear = 0.5;
|
|
|
- double Kp_angular = 0.5;
|
|
|
- double Kp_lateral = 0.6;
|
|
|
-
|
|
|
- // 线速度控制
|
|
|
- double linear_velocity = Kp_linear * std::max(0.1, dx - 0.6);
|
|
|
- linear_velocity = std::clamp(linear_velocity, -max_linear_speed, max_linear_speed);
|
|
|
- linear_velocity = std::max(0.08, linear_velocity);
|
|
|
-
|
|
|
- // 角速度控制
|
|
|
- double angular_velocity = Kp_angular * yaw_error + Kp_lateral * dy;
|
|
|
- angular_velocity = std::clamp(angular_velocity, -max_angular_speed, max_angular_speed);
|
|
|
-
|
|
|
- return {-linear_velocity, -angular_velocity};
|
|
|
-}
|
|
|
-
|
|
|
-std::pair<double, double> RechargeTool::simpleFitLine(const std::vector<IndexPoint> &points)
|
|
|
-{
|
|
|
- if (points.size() < 2)
|
|
|
- {
|
|
|
- return {0.0, 0.0};
|
|
|
- }
|
|
|
-
|
|
|
- double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0;
|
|
|
- int n = points.size();
|
|
|
-
|
|
|
- for (const auto &p : points)
|
|
|
- {
|
|
|
- sum_x += p.x;
|
|
|
- sum_y += p.y;
|
|
|
- sum_xy += p.x * p.y;
|
|
|
- sum_xx += p.x * p.x;
|
|
|
- }
|
|
|
-
|
|
|
- double k = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x);
|
|
|
- double b = (sum_y - k * sum_x) / n;
|
|
|
-
|
|
|
- return {k, b};
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 状态机实现 ====================
|
|
|
-
|
|
|
-WheelchairStateMachine::WheelchairStateMachine()
|
|
|
- : Node("wheelchair_state_machine"),
|
|
|
- current_state_(WheelchairState::READY),
|
|
|
- interface_active_(false),
|
|
|
- recharge_active_(false),
|
|
|
- charging_state_(ChargingState::IDLE),
|
|
|
- battery_state_(BatteryState::NORMAL),
|
|
|
- battery_percentage_(100.0),
|
|
|
- low_battery_threshold_(20.0), // 默认低电量阈值20%
|
|
|
- critical_battery_threshold_(10.0), // 默认严重低电量阈值10%
|
|
|
- frame_index_(0),
|
|
|
- cmd_enable_(false),
|
|
|
- detect_charging_voltage_(false),
|
|
|
- last_mode_("")
|
|
|
-{
|
|
|
- // 声明电池阈值参数
|
|
|
- this->declare_parameter<float>("low_battery_threshold", 20.0);
|
|
|
- this->declare_parameter<float>("critical_battery_threshold", 10.0);
|
|
|
-
|
|
|
- // 获取电池阈值参数
|
|
|
- low_battery_threshold_ = this->get_parameter("low_battery_threshold").as_double();
|
|
|
- critical_battery_threshold_ = this->get_parameter("critical_battery_threshold").as_double();
|
|
|
-
|
|
|
- // 初始化状态转移表
|
|
|
- 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);
|
|
|
-
|
|
|
- // 初始化电池监控
|
|
|
- initializeBatteryMonitor();
|
|
|
-
|
|
|
- // 初始化回充系统
|
|
|
- initializeRecharge();
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "智能轮椅状态机（集成回充功能）已启动");
|
|
|
- RCLCPP_INFO(this->get_logger(), "当前状态: %s", getCurrentState().c_str());
|
|
|
- RCLCPP_INFO(this->get_logger(), "电池阈值 - 低电量: %.1f%%, 严重低电量: %.1f%%",
|
|
|
- low_battery_threshold_, critical_battery_threshold_);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::initializeBatteryMonitor()
|
|
|
-{
|
|
|
- // 创建电池状态订阅者
|
|
|
- battery_sub_ = this->create_subscription<sensor_msgs::msg::BatteryState>(
|
|
|
- "battery_state", 10,
|
|
|
- std::bind(&WheelchairStateMachine::batteryStateCallback, this, _1));
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "电池状态监控已初始化");
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::initializeRecharge()
|
|
|
-{
|
|
|
- // 声明参数
|
|
|
- this->declare_parameter<double>("distance_threshold", 0.5);
|
|
|
- this->declare_parameter<int>("fit_window_size", 10);
|
|
|
- this->declare_parameter<double>("fit_residual_threshold", 0.09);
|
|
|
-
|
|
|
- // 获取参数
|
|
|
- distance_threshold_ = this->get_parameter("distance_threshold").as_double();
|
|
|
- fit_window_size_ = this->get_parameter("fit_window_size").as_int();
|
|
|
- fit_residual_threshold_ = this->get_parameter("fit_residual_threshold").as_double();
|
|
|
-
|
|
|
- // QoS配置
|
|
|
- auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10))
|
|
|
- .best_effort()
|
|
|
- .durability_volatile();
|
|
|
-
|
|
|
- // 创建激光雷达订阅者
|
|
|
- scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
|
|
|
- "/scan/back", qos_profile,
|
|
|
- std::bind(&WheelchairStateMachine::scanCallback, this, _1));
|
|
|
-
|
|
|
- // 创建控制发布者
|
|
|
- pub_ctrl_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_raw", 10);
|
|
|
-
|
|
|
- // 创建命令使能订阅者
|
|
|
- sub_cmd_enable_ = this->create_subscription<std_msgs::msg::Bool>(
|
|
|
- "/recharge/cmd_enable", 10,
|
|
|
- std::bind(&WheelchairStateMachine::onCmdEnable, this, _1));
|
|
|
-
|
|
|
- // 创建可视化发布者
|
|
|
- pub_rviz_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
|
|
|
- "/line_marker_array", 10);
|
|
|
-
|
|
|
- // 创建过滤后的激光数据发布者
|
|
|
- pub_filtered_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out", 10);
|
|
|
- pub_line_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out_line", 10);
|
|
|
-
|
|
|
- // 初始化控制消息
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
-
|
|
|
- // 创建回充定时器
|
|
|
- recharge_timer_ = this->create_wall_timer(
|
|
|
- 100ms, std::bind(&WheelchairStateMachine::rechargeTimerCallback, this));
|
|
|
-
|
|
|
- detect_station_time_ = this->now() - rclcpp::Duration(100.0, 0);
|
|
|
- detect_charging_voltage_time_ = this->now();
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 电池状态处理 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::batteryStateCallback(const sensor_msgs::msg::BatteryState::SharedPtr msg)
|
|
|
-{
|
|
|
- // 更新电池百分比
|
|
|
- if (msg->percentage >= 0.0 && msg->percentage <= 1.0)
|
|
|
- {
|
|
|
- battery_percentage_ = msg->percentage * 100.0;
|
|
|
- }
|
|
|
- else if (msg->voltage > 0.0)
|
|
|
- {
|
|
|
- // 如果percentage无效，根据电压估算（示例）
|
|
|
- float voltage = msg->voltage;
|
|
|
- // 简单线性估算：假设12V为满电，10V为没电
|
|
|
- battery_percentage_ = std::clamp((voltage - 10.0f) / 2.0f * 100.0f, 0.0f, 100.0f);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- battery_percentage_ = 100.0f; // 默认值
|
|
|
- }
|
|
|
-
|
|
|
- // 检查充电状态
|
|
|
- if (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING)
|
|
|
- {
|
|
|
- // 如果正在充电，更新状态
|
|
|
- if (current_state_ != WheelchairState::CHARGING)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "检测到电池正在充电");
|
|
|
- handleBatteryStartCharging();
|
|
|
- }
|
|
|
- battery_state_ = BatteryState::CHARGING;
|
|
|
- }
|
|
|
- else if (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL)
|
|
|
- {
|
|
|
- // 电池已充满
|
|
|
- if (current_state_ == WheelchairState::CHARGING)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "电池已充满");
|
|
|
- handleBatteryFull();
|
|
|
- }
|
|
|
- battery_state_ = BatteryState::FULL;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // 更新电池状态
|
|
|
- updateBatteryState(battery_percentage_);
|
|
|
- }
|
|
|
-
|
|
|
- // 检查电池阈值并触发相应动作
|
|
|
- checkBatteryThresholds();
|
|
|
-
|
|
|
- // 定期记录电池状态（每10%记录一次）
|
|
|
- static int last_logged_percentage = -1;
|
|
|
- int current_10percent = static_cast<int>(battery_percentage_ / 10.0);
|
|
|
- if (current_10percent != last_logged_percentage)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "电池状态: %.1f%% [%s]",
|
|
|
- battery_percentage_, batteryStateToString(battery_state_).c_str());
|
|
|
- last_logged_percentage = current_10percent;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::updateBatteryState(float percentage)
|
|
|
-{
|
|
|
- BatteryState old_state = battery_state_;
|
|
|
-
|
|
|
- if (percentage <= critical_battery_threshold_)
|
|
|
- {
|
|
|
- battery_state_ = BatteryState::CRITICAL;
|
|
|
- }
|
|
|
- else if (percentage <= low_battery_threshold_)
|
|
|
- {
|
|
|
- battery_state_ = BatteryState::LOW;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- battery_state_ = BatteryState::NORMAL;
|
|
|
- }
|
|
|
-
|
|
|
- // 状态变化时记录
|
|
|
- if (old_state != battery_state_)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "电池状态变化: %s -> %s (%.1f%%)",
|
|
|
- batteryStateToString(old_state).c_str(),
|
|
|
- batteryStateToString(battery_state_).c_str(),
|
|
|
- percentage);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::checkBatteryThresholds()
|
|
|
-{
|
|
|
- static bool low_battery_triggered = false;
|
|
|
- static bool critical_battery_triggered = false;
|
|
|
-
|
|
|
- // 检查低电量警告
|
|
|
- if (battery_state_ == BatteryState::LOW && !low_battery_triggered)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "低电量警告: %.1f%% (阈值: %.1f%%)",
|
|
|
- battery_percentage_, low_battery_threshold_);
|
|
|
- handleLowBatteryWarning();
|
|
|
- low_battery_triggered = true;
|
|
|
- critical_battery_triggered = false; // 重置严重低电量触发
|
|
|
- }
|
|
|
-
|
|
|
- // 检查严重低电量警告
|
|
|
- if (battery_state_ == BatteryState::CRITICAL && !critical_battery_triggered)
|
|
|
- {
|
|
|
- RCLCPP_ERROR(this->get_logger(), "严重低电量警告: %.1f%% (阈值: %.1f%%)",
|
|
|
- battery_percentage_, critical_battery_threshold_);
|
|
|
- triggerLowBatteryAction();
|
|
|
- critical_battery_triggered = true;
|
|
|
- }
|
|
|
-
|
|
|
- // 如果电池恢复到正常水平，重置触发标志
|
|
|
- if (battery_state_ == BatteryState::NORMAL)
|
|
|
- {
|
|
|
- low_battery_triggered = false;
|
|
|
- critical_battery_triggered = false;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::triggerLowBatteryAction()
|
|
|
-{
|
|
|
- // 根据当前状态采取不同行动
|
|
|
- switch (current_state_)
|
|
|
- {
|
|
|
- case WheelchairState::READY:
|
|
|
- // 在就绪状态下，自动启动回充搜索
|
|
|
- RCLCPP_WARN(this->get_logger(), "严重低电量，自动启动回充搜索");
|
|
|
- if (handleEvent("low_battery_warning"))
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "已自动启动回充搜索");
|
|
|
- }
|
|
|
- break;
|
|
|
-
|
|
|
- case WheelchairState::WALKING:
|
|
|
- // 在行走状态下，发出警告但不自动切换
|
|
|
- RCLCPP_WARN(this->get_logger(), "严重低电量警告，请尽快启动回充");
|
|
|
- publishStateUpdate("CRITICAL_BATTERY_WARNING", "请尽快启动回充");
|
|
|
- break;
|
|
|
-
|
|
|
- case WheelchairState::SEARCHING:
|
|
|
- // 已经在搜索中，提高搜索优先级
|
|
|
- RCLCPP_WARN(this->get_logger(), "严重低电量，提高回充搜索优先级");
|
|
|
- publishStateUpdate("CRITICAL_BATTERY_PRIORITY", "提高回充搜索优先级");
|
|
|
- break;
|
|
|
-
|
|
|
- case WheelchairState::CHARGING:
|
|
|
- // 已经在充电中，继续充电
|
|
|
- RCLCPP_INFO(this->get_logger(), "严重低电量，正在充电中");
|
|
|
- break;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 状态机核心方法 ====================
|
|
|
-
|
|
|
-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_active_ = true;
|
|
|
- charging_state_ = ChargingState::NAVIGATING;
|
|
|
- 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(), "开始充电");
|
|
|
-
|
|
|
- // 停止运动
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
-
|
|
|
- // 停止搜索超时定时器
|
|
|
- stopSearchTimeoutTimer();
|
|
|
-
|
|
|
- // 更新充电状态
|
|
|
- charging_state_ = ChargingState::CHARGING;
|
|
|
-
|
|
|
- // 设置充电电压检测
|
|
|
- setChargingVoltage(true);
|
|
|
-
|
|
|
- // 发布充电开始通知
|
|
|
- publishStateUpdate("CHARGING_START");
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::transitionToReady()
|
|
|
-{
|
|
|
- // 可以从所有状态返回到就绪状态
|
|
|
- std::string previous_state = getCurrentState();
|
|
|
-
|
|
|
- // 停止回充系统
|
|
|
- recharge_active_ = false;
|
|
|
- charging_state_ = ChargingState::IDLE;
|
|
|
- last_mode_ = "";
|
|
|
-
|
|
|
- // 停止所有运动
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
-
|
|
|
- // 停止定时器
|
|
|
- stopSearchTimeoutTimer();
|
|
|
-
|
|
|
- setState(WheelchairState::READY);
|
|
|
-
|
|
|
- // 根据之前的状态执行清理操作
|
|
|
- if (previous_state == "搜索中")
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "停止搜索，返回到就绪状态");
|
|
|
- }
|
|
|
- else if (previous_state == "充电中")
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "停止充电，返回到就绪状态");
|
|
|
- detect_charging_voltage_ = 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::handleLowBatteryWarning()
|
|
|
-{
|
|
|
- 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()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "开始充电 - 当前电量: %.1f%%", battery_percentage_);
|
|
|
-
|
|
|
- // 设置充电电压检测
|
|
|
- setChargingVoltage(true);
|
|
|
-
|
|
|
- // 更新电池状态
|
|
|
- battery_state_ = BatteryState::CHARGING;
|
|
|
-
|
|
|
- // 如果当前在搜索状态，转移到充电状态
|
|
|
- if (current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- transitionToCharging();
|
|
|
- }
|
|
|
-
|
|
|
- // 发布充电开始通知
|
|
|
- publishStateUpdate("BATTERY_START_CHARGING");
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBatteryStopCharging()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "停止充电");
|
|
|
- setChargingVoltage(false);
|
|
|
-
|
|
|
- // 更新电池状态
|
|
|
- if (battery_percentage_ >= 95.0)
|
|
|
- {
|
|
|
- battery_state_ = BatteryState::FULL;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- updateBatteryState(battery_percentage_);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::handleBatteryFull()
|
|
|
-{
|
|
|
- RCLCPP_INFO(this->get_logger(), "电池已充满 - 电量: %.1f%%", battery_percentage_);
|
|
|
-
|
|
|
- // 更新电池状态
|
|
|
- battery_state_ = BatteryState::FULL;
|
|
|
-
|
|
|
- // 停止充电，返回就绪状态
|
|
|
- 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();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 回充功能实现 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
|
|
|
-{
|
|
|
- bool new_enable = msg->data;
|
|
|
-
|
|
|
- // 如果从false变为true，启动回充
|
|
|
- if (!cmd_enable_ && new_enable) {
|
|
|
- RCLCPP_INFO(this->get_logger(), "收到回充启动命令");
|
|
|
- handleEvent("ipad_phone_interface_start"); // 触发回充
|
|
|
- }
|
|
|
- // 如果从true变为false，取消回充
|
|
|
- else if (cmd_enable_ && !new_enable) {
|
|
|
- RCLCPP_INFO(this->get_logger(), "收到回充取消命令");
|
|
|
- handleEvent("ipad_phone_interface_cancel"); // 取消回充
|
|
|
- }
|
|
|
-
|
|
|
- cmd_enable_ = new_enable;
|
|
|
- RCLCPP_INFO(this->get_logger(), "回充命令使能: %s", cmd_enable_ ? "启用" : "禁用");
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::rechargeTimerCallback()
|
|
|
-{
|
|
|
- // 如果不是回充模式，直接返回
|
|
|
- if (last_mode_ != "recharge" || !recharge_active_)
|
|
|
- {
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- auto now = this->now();
|
|
|
- auto time_diff = (now - ctrl_twist_time_).seconds();
|
|
|
-
|
|
|
- // 如果最近有控制命令且使能，发布控制命令
|
|
|
- if (time_diff < 2.0 && cmd_enable_ &&
|
|
|
- (current_state_ == WheelchairState::SEARCHING || current_state_ == WheelchairState::WALKING))
|
|
|
- {
|
|
|
- pub_ctrl_->publish(ctrl_twist_);
|
|
|
- }
|
|
|
-
|
|
|
- // 检查充电电压
|
|
|
- if (getChargingVoltage() && current_state_ == WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "检测到充电电压，开始充电");
|
|
|
- transitionToCharging();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- // 如果回充未激活或不是搜索状态，不处理激光数据
|
|
|
- if (!recharge_active_ || current_state_ != WheelchairState::SEARCHING)
|
|
|
- {
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- processLaserScan(msg);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::processLaserScan(const sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- // 应用过滤
|
|
|
- filterScanModify0(msg);
|
|
|
- filterScanModify1(msg);
|
|
|
- filterScanModify2(msg);
|
|
|
- filterScanModify3(msg);
|
|
|
-
|
|
|
- auto [begin_index, end_index] = filterScanModify4(msg);
|
|
|
-
|
|
|
- if (end_index - begin_index < 2)
|
|
|
- {
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "没有可连续性的点");
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 处理分段数据
|
|
|
- if (processSegmentData(msg, begin_index, end_index))
|
|
|
- {
|
|
|
- // 更新检测时间
|
|
|
- detect_station_time_ = this->now();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-bool WheelchairStateMachine::processSegmentData(const sensor_msgs::msg::LaserScan::SharedPtr msg,
|
|
|
- int begin_index, int end_index)
|
|
|
-{
|
|
|
- // 转换到XY坐标系
|
|
|
- auto xy_list = laserScanToXY1(begin_index, end_index, msg);
|
|
|
- if (xy_list.size() < 2)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "XY转换结果点数不足: %zu", xy_list.size());
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- // 获取分段
|
|
|
- auto sections = getSections(begin_index, end_index, msg, xy_list);
|
|
|
- if (sections.empty())
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "没有获取到分段");
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- // 选择最近的分段
|
|
|
- int nearest_index = -1;
|
|
|
- double nearest_x = 100.0;
|
|
|
-
|
|
|
- for (size_t i = 0; i < sections.size(); i++)
|
|
|
- {
|
|
|
- int section_start = sections[i].first;
|
|
|
- int section_end = sections[i].second;
|
|
|
-
|
|
|
- if (section_end - section_start < 10)
|
|
|
- continue;
|
|
|
-
|
|
|
- double avg_total = 0.0;
|
|
|
- int avg_count = 0;
|
|
|
-
|
|
|
- for (int index = section_start; index <= section_end; index++)
|
|
|
- {
|
|
|
- if (xy_list.find(index) == xy_list.end())
|
|
|
- continue;
|
|
|
-
|
|
|
- double x = msg->ranges[index];
|
|
|
- avg_count++;
|
|
|
- avg_total += x;
|
|
|
- }
|
|
|
-
|
|
|
- if (avg_count == 0)
|
|
|
- continue;
|
|
|
-
|
|
|
- double avg_value = avg_total / avg_count;
|
|
|
-
|
|
|
- if (nearest_x > avg_value)
|
|
|
- {
|
|
|
- nearest_x = avg_value;
|
|
|
- nearest_index = i;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if (nearest_index < 0)
|
|
|
- {
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- begin_index = sections[nearest_index].first;
|
|
|
- end_index = sections[nearest_index].second;
|
|
|
-
|
|
|
- // 重置激光数据
|
|
|
- auto filtered_msg = resetLaserData(msg, begin_index, end_index);
|
|
|
-
|
|
|
- // 发布过滤后的数据
|
|
|
- publishFilteredScan(filtered_msg, filtered_msg->ranges);
|
|
|
-
|
|
|
- // 进一步处理
|
|
|
- auto xy_list_full = laserScanToXY(filtered_msg);
|
|
|
- if (xy_list_full.size() < 2)
|
|
|
- {
|
|
|
- RCLCPP_ERROR(this->get_logger(), "xy坐标系没有转换成功");
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- // 获取分段
|
|
|
- auto turn_segments = getSegments(xy_list_full);
|
|
|
- auto filtered_segments = filterSegments(turn_segments);
|
|
|
-
|
|
|
- if (filtered_segments.empty())
|
|
|
- {
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "未检测到有效的充电站分段");
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- // 创建结果对象
|
|
|
- RechargeResult result;
|
|
|
- result.frameid = frame_index_++;
|
|
|
- result.laserScanData = filtered_msg;
|
|
|
-
|
|
|
- // 处理左右点
|
|
|
- int segment_index = 0;
|
|
|
- for (const auto &seg : filtered_segments)
|
|
|
- {
|
|
|
- const auto &info = seg.second;
|
|
|
- int start_idx = std::get<0>(info);
|
|
|
- int end_idx = std::get<1>(info);
|
|
|
-
|
|
|
- for (int index = start_idx; index <= end_idx; index++)
|
|
|
- {
|
|
|
- if (xy_list_full.find(index) == xy_list_full.end())
|
|
|
- continue;
|
|
|
-
|
|
|
- double x = xy_list_full[index].first;
|
|
|
- double y = xy_list_full[index].second;
|
|
|
- IndexPoint item(x, y, index);
|
|
|
-
|
|
|
- if (segment_index == 0)
|
|
|
- {
|
|
|
- result.left_points.push_back(item);
|
|
|
- }
|
|
|
- else if (segment_index == 1)
|
|
|
- {
|
|
|
- result.right_points.push_back(item);
|
|
|
- }
|
|
|
- }
|
|
|
- segment_index++;
|
|
|
-
|
|
|
- if (segment_index >= 2)
|
|
|
- break; // 只需要两个分段
|
|
|
- }
|
|
|
-
|
|
|
- // 如果成功获取左右点，进行控制
|
|
|
- if (!result.left_points.empty() && !result.right_points.empty())
|
|
|
- {
|
|
|
- procData(result);
|
|
|
- return true;
|
|
|
- }
|
|
|
-
|
|
|
- return false;
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::procData(RechargeResult &result)
|
|
|
-{
|
|
|
- if (result.left_points.size() < 2 || result.right_points.size() < 2)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "点数量不足，跳过拟合");
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 拟合左边直线
|
|
|
- auto [k1, b1] = RechargeTool::simpleFitLine(result.left_points);
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "左直线拟合: y = %.4fx + %.4f", k1, b1);
|
|
|
-
|
|
|
- // 拟合右边直线
|
|
|
- auto [k2, b2] = RechargeTool::simpleFitLine(result.right_points);
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "右直线拟合: y = %.4fx + %.4f", k2, b2);
|
|
|
-
|
|
|
- // 检查是否平行
|
|
|
- if (fabs(k1 - k2) < 1e-10)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "两直线平行，无交点");
|
|
|
- result.middle_point = IndexPoint(0.0, 0.0, 0);
|
|
|
- result.middle_line = StraightLine(0.0, 0.0);
|
|
|
- result.left_line = StraightLine(k1, b1);
|
|
|
- result.right_line = StraightLine(k2, b2);
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 计算交点
|
|
|
- double x = (b2 - b1) / (k1 - k2);
|
|
|
- double y = k1 * x + b1;
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "交点: (%.4f, %.4f)", x, y);
|
|
|
-
|
|
|
- // 更新中间点
|
|
|
- result.middle_point = IndexPoint(x, y, 0);
|
|
|
-
|
|
|
- // 计算角平分线
|
|
|
- auto [k3, b3] = RechargeTool::calculateAngleBisector(k1, b1, k2, b2);
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "角平分线: y = %.4fx + %.4f", k3, b3);
|
|
|
-
|
|
|
- // 更新结果
|
|
|
- result.middle_line = StraightLine(k3, b3);
|
|
|
- result.left_line = StraightLine(k1, b1);
|
|
|
- result.right_line = StraightLine(k2, b2);
|
|
|
-
|
|
|
- // 控制运动
|
|
|
- controlRechargeMotion(result);
|
|
|
-
|
|
|
- // 发布可视化
|
|
|
- publishRechargeVisualization(result);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::controlRechargeMotion(const RechargeResult &result)
|
|
|
-{
|
|
|
- // 如果检测到充电电压，停止移动
|
|
|
- if (getChargingVoltage())
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "检测到充电电压，停止移动");
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- double dx = result.middle_point.x;
|
|
|
- double dy = result.middle_point.y;
|
|
|
- double k = result.middle_line.k;
|
|
|
-
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);
|
|
|
-
|
|
|
- // 调整 dy
|
|
|
- dy = dy - 0.10;
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "调整后 dy: %.4f", dy);
|
|
|
-
|
|
|
- // 计算偏航角误差
|
|
|
- double yaw_error = atan(k);
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
|
|
|
- yaw_error, yaw_error * 180.0 / M_PI);
|
|
|
-
|
|
|
- // 计算控制速度
|
|
|
- auto [linear_velocity, angular_velocity] = RechargeTool::computeDockingVelocity(dx - 0.3, dy, yaw_error);
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "基础控制速度 - 线速度: %.4f, 角速度: %.4f",
|
|
|
- linear_velocity, angular_velocity);
|
|
|
-
|
|
|
- // 限制角速度
|
|
|
- if (angular_velocity > 0.0)
|
|
|
- {
|
|
|
- angular_velocity = std::min(fabs(angular_velocity), 0.09);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- angular_velocity = -std::min(fabs(angular_velocity), 0.09);
|
|
|
- }
|
|
|
-
|
|
|
- // 根据距离调整速度系数
|
|
|
- double radio_x = 1.0;
|
|
|
- double radio_z = 1.0;
|
|
|
-
|
|
|
- if (dx > 1.0)
|
|
|
- {
|
|
|
- // 保持默认
|
|
|
- }
|
|
|
- else if (dx > 0.8)
|
|
|
- {
|
|
|
- radio_x = 0.7;
|
|
|
- }
|
|
|
- else if (dx > 0.7)
|
|
|
- {
|
|
|
- radio_x = 0.5;
|
|
|
- }
|
|
|
- else if (dx > 0.68)
|
|
|
- {
|
|
|
- radio_x = 0.3;
|
|
|
- radio_z = 1.0;
|
|
|
- }
|
|
|
- else if (dx > 0.65)
|
|
|
- {
|
|
|
- radio_x = 0.2;
|
|
|
- radio_z = 1.0;
|
|
|
- }
|
|
|
- else if (dx > 0.62)
|
|
|
- {
|
|
|
- radio_x = 0.1;
|
|
|
- radio_z = 1.0;
|
|
|
- }
|
|
|
- else if (dx > 0.2)
|
|
|
- {
|
|
|
- radio_x = 0.15;
|
|
|
- radio_z = 1.0;
|
|
|
- }
|
|
|
-
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "距离调整系数 - radio_x: %.2f, radio_z: %.2f", radio_x, radio_z);
|
|
|
-
|
|
|
- // 设置控制命令
|
|
|
- if (dx > 0.54)
|
|
|
- {
|
|
|
- double final_linear_vel = linear_velocity * radio_x;
|
|
|
- if (final_linear_vel > -0.04)
|
|
|
- {
|
|
|
- final_linear_vel = -0.04;
|
|
|
- }
|
|
|
-
|
|
|
- ctrl_twist_.linear.x = final_linear_vel;
|
|
|
- ctrl_twist_.angular.z = angular_velocity * radio_z;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
-
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "控制命令 - 线速度: %.4f, 角速度: %.4f",
|
|
|
- ctrl_twist_.linear.x, ctrl_twist_.angular.z);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // 到达充电位置
|
|
|
- RCLCPP_INFO(this->get_logger(), "到达充电位置 (dx=%.4f <= 0.54), 停止移动", dx);
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
-
|
|
|
- // 模拟检测到充电电压
|
|
|
- setChargingVoltage(true);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 过滤函数实现 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::filterScanModify0(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- int fixed = getFixed();
|
|
|
- double diff_yaw = msg->angle_increment * fixed;
|
|
|
- msg->angle_min -= diff_yaw;
|
|
|
- msg->angle_max -= diff_yaw;
|
|
|
-
|
|
|
- std::vector<float> ranges = msg->ranges;
|
|
|
- for (size_t i = 0; i < msg->ranges.size(); i++)
|
|
|
- {
|
|
|
- int index = getFixedIndex(msg, i);
|
|
|
- ranges[index] = msg->ranges[i];
|
|
|
- }
|
|
|
- msg->ranges = ranges;
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::filterScanModify1(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- // 过滤角度，只接受后面90度
|
|
|
- int min_index = param_.min_index;
|
|
|
- int max_index = param_.max_index;
|
|
|
-
|
|
|
- for (size_t i = 0; i < msg->ranges.size(); i++)
|
|
|
- {
|
|
|
- if (i < min_index || i > max_index)
|
|
|
- {
|
|
|
- msg->ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- }
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::filterScanModify2(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- // 过滤距离
|
|
|
- for (size_t i = 0; i < msg->ranges.size(); i++)
|
|
|
- {
|
|
|
- if (!isValid(msg->ranges[i]))
|
|
|
- continue;
|
|
|
-
|
|
|
- if (msg->ranges[i] < param_.min_distance ||
|
|
|
- msg->ranges[i] > param_.max_distance)
|
|
|
- {
|
|
|
- msg->ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- }
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::filterScanModify3(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- // 过滤强度
|
|
|
- for (size_t i = 0; i < msg->ranges.size(); i++)
|
|
|
- {
|
|
|
- if (!isValid(msg->ranges[i]))
|
|
|
- continue;
|
|
|
-
|
|
|
- float value = msg->intensities[i];
|
|
|
- if (value < param_.min_intensities || value > param_.max_intensities)
|
|
|
- {
|
|
|
- msg->ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- }
|
|
|
-
|
|
|
- // 再次检查距离
|
|
|
- if (msg->ranges[i] < param_.min_distance ||
|
|
|
- msg->ranges[i] > param_.max_distance)
|
|
|
- {
|
|
|
- msg->ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- }
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-std::pair<int, int> WheelchairStateMachine::filterScanModify4(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- // 过滤连续性
|
|
|
- int begin_index = 0;
|
|
|
- int end_index = 0;
|
|
|
- std::vector<float> ranges = msg->ranges;
|
|
|
-
|
|
|
- // 顺序找起点
|
|
|
- for (int i = param_.min_index + 1; i < param_.max_index; i++)
|
|
|
- {
|
|
|
- if (!isValid(ranges[i]))
|
|
|
- continue;
|
|
|
- if (!isValid(ranges[i - 1]))
|
|
|
- {
|
|
|
- begin_index = i;
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if (static_cast<int>(msg->ranges.size()) - begin_index < 10)
|
|
|
- {
|
|
|
- RCLCPP_ERROR(this->get_logger(), "数据太少无法继续处理");
|
|
|
- return {0, 0};
|
|
|
- }
|
|
|
-
|
|
|
- // 倒序找终点
|
|
|
- for (int i = param_.max_index - 2; i > 0; i--)
|
|
|
- {
|
|
|
- if (!isValid(ranges[i]))
|
|
|
- continue;
|
|
|
- if (!isValid(ranges[i + 1]))
|
|
|
- {
|
|
|
- end_index = i;
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if (end_index < 10)
|
|
|
- {
|
|
|
- return {0, 0};
|
|
|
- }
|
|
|
-
|
|
|
- // 顺序检查连续性
|
|
|
- for (int i = begin_index + 1; i < end_index - 1; i++)
|
|
|
- {
|
|
|
- float prev = ranges[i - 1];
|
|
|
- float curr = ranges[i];
|
|
|
- float next = ranges[i + 1];
|
|
|
-
|
|
|
- if (!isValid(prev) && !isValid(curr) && !isValid(next))
|
|
|
- {
|
|
|
- if (i - begin_index < param_.min_count)
|
|
|
- {
|
|
|
- begin_index = i + 1;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- float low = 100.0;
|
|
|
- float high = 0.0;
|
|
|
- for (int j = i - 1; j <= i + 1; j++)
|
|
|
- {
|
|
|
- if (isValid(ranges[j]))
|
|
|
- {
|
|
|
- if (ranges[j] > 0.03 && ranges[j] < low)
|
|
|
- {
|
|
|
- low = ranges[j];
|
|
|
- }
|
|
|
- if (ranges[j] > high)
|
|
|
- {
|
|
|
- high = ranges[j];
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- if (high - low > 0.08)
|
|
|
- {
|
|
|
- if (i - begin_index < param_.min_count)
|
|
|
- {
|
|
|
- begin_index = i + 1;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- // 倒序检查连续性
|
|
|
- for (int i = end_index - 1; i > begin_index + 1; i--)
|
|
|
- {
|
|
|
- float prev = ranges[i - 1];
|
|
|
- float curr = ranges[i];
|
|
|
- float next = ranges[i + 1];
|
|
|
-
|
|
|
- if (!isValid(prev) && !isValid(curr) && !isValid(next))
|
|
|
- {
|
|
|
- if (end_index - i < param_.min_count)
|
|
|
- {
|
|
|
- end_index = i - 1;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- if (isValid(next) && isValid(prev))
|
|
|
- {
|
|
|
- if (next - curr > 0.07)
|
|
|
- {
|
|
|
- if (end_index - i < param_.min_count)
|
|
|
- {
|
|
|
- end_index = i - 1;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- // 设置无效区域
|
|
|
- for (int i = param_.min_index; i < begin_index - 1; i++)
|
|
|
- {
|
|
|
- msg->ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- }
|
|
|
- for (int i = end_index + 1; i <= param_.max_index; i++)
|
|
|
- {
|
|
|
- msg->ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- }
|
|
|
-
|
|
|
- return {begin_index, end_index};
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 坐标转换函数 ====================
|
|
|
-
|
|
|
-std::map<int, std::pair<double, double>> WheelchairStateMachine::laserScanToXY(
|
|
|
- const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
|
|
|
-{
|
|
|
- std::map<int, std::pair<double, double>> xy_points_map;
|
|
|
- double angle = scan_msg->angle_min;
|
|
|
-
|
|
|
- for (size_t i = 0; i < scan_msg->ranges.size(); i++)
|
|
|
- {
|
|
|
- float range_val = scan_msg->ranges[i];
|
|
|
-
|
|
|
- // 跳过无效测量值
|
|
|
- if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
|
|
|
- {
|
|
|
- // 极坐标转笛卡尔坐标
|
|
|
- double x1 = range_val * cos(angle);
|
|
|
- double y1 = range_val * sin(angle);
|
|
|
- double x = -y1;
|
|
|
- double y = x1;
|
|
|
- xy_points_map[i] = {x, y};
|
|
|
- }
|
|
|
- angle += scan_msg->angle_increment;
|
|
|
- }
|
|
|
- return xy_points_map;
|
|
|
-}
|
|
|
-
|
|
|
-std::map<int, std::pair<double, double>> WheelchairStateMachine::laserScanToXY1(
|
|
|
- int begin_index, int end_index,
|
|
|
- const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
|
|
|
-{
|
|
|
- std::map<int, std::pair<double, double>> xy_points_map;
|
|
|
- double angle = scan_msg->angle_min;
|
|
|
-
|
|
|
- for (int i = begin_index - 1; i <= end_index; i++)
|
|
|
- {
|
|
|
- float range_val = scan_msg->ranges[i];
|
|
|
-
|
|
|
- if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
|
|
|
- {
|
|
|
- double x1 = range_val * cos(angle);
|
|
|
- double y1 = range_val * sin(angle);
|
|
|
- double x = -y1;
|
|
|
- double y = x1;
|
|
|
- xy_points_map[i] = {x, y};
|
|
|
- }
|
|
|
- angle += scan_msg->angle_increment;
|
|
|
- }
|
|
|
- return xy_points_map;
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 分段处理函数 ====================
|
|
|
-
|
|
|
-std::vector<std::pair<int, int>> WheelchairStateMachine::getSections(
|
|
|
- int begin_index, int end_index,
|
|
|
- const sensor_msgs::msg::LaserScan::SharedPtr msg,
|
|
|
- const std::map<int, std::pair<double, double>> &xy_list)
|
|
|
-{
|
|
|
- std::vector<std::pair<int, int>> sections;
|
|
|
-
|
|
|
- int section_start = begin_index + 1;
|
|
|
- int section_end = 0;
|
|
|
-
|
|
|
- int last_index = -1;
|
|
|
- double last_x = 0.0, last_y = 0.0;
|
|
|
-
|
|
|
- for (int i = begin_index + 1; i < end_index - 1; i++)
|
|
|
- {
|
|
|
- if (xy_list.find(i) == xy_list.end())
|
|
|
- {
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- auto curr = xy_list.at(i);
|
|
|
- if (last_index < 0)
|
|
|
- {
|
|
|
- last_index = i;
|
|
|
- last_x = curr.first;
|
|
|
- last_y = curr.second;
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- double dx = curr.first - last_x;
|
|
|
- double dy = curr.second - last_y;
|
|
|
- double dist = sqrt(dx * dx + dy * dy);
|
|
|
-
|
|
|
- if (dist > 0.05 || (i - last_index) > 2)
|
|
|
- {
|
|
|
- section_end = i;
|
|
|
- if ((section_end - section_start) >= param_.min_count)
|
|
|
- {
|
|
|
- sections.emplace_back(section_start, section_end);
|
|
|
- }
|
|
|
- section_start = i;
|
|
|
- }
|
|
|
-
|
|
|
- last_index = i;
|
|
|
- last_x = curr.first;
|
|
|
- last_y = curr.second;
|
|
|
- }
|
|
|
-
|
|
|
- if (last_index > section_end + 10)
|
|
|
- {
|
|
|
- section_end = last_index;
|
|
|
- if ((section_end - section_start) > param_.min_count)
|
|
|
- {
|
|
|
- sections.emplace_back(section_start, section_end);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- return sections;
|
|
|
-}
|
|
|
-
|
|
|
-std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
|
|
|
-WheelchairStateMachine::getSegments(const std::map<int, std::pair<double, double>> &xy_points_map)
|
|
|
-{
|
|
|
- std::map<int, std::tuple<int, int, double, double, double, double, double, int>> turn_segments;
|
|
|
-
|
|
|
- if (xy_points_map.empty())
|
|
|
- {
|
|
|
- return turn_segments;
|
|
|
- }
|
|
|
-
|
|
|
- // 将点转换为路径
|
|
|
- std::vector<IndexPoint> path;
|
|
|
- for (const auto &point : xy_points_map)
|
|
|
- {
|
|
|
- path.emplace_back(point.second.first, point.second.second, point.first);
|
|
|
- }
|
|
|
-
|
|
|
- // 使用相同的参数进行分段
|
|
|
- auto segments = RechargeTool::splitPointsToSegments(path, 24.0, 4);
|
|
|
-
|
|
|
- // 处理每个分段
|
|
|
- for (size_t i = 0; i < segments.size(); i++)
|
|
|
- {
|
|
|
- if (segments[i].size() < 2)
|
|
|
- continue;
|
|
|
-
|
|
|
- // 获取分段的前两个点
|
|
|
- const auto &s_prev = segments[i][0];
|
|
|
- const auto &s_next = segments[i][1];
|
|
|
-
|
|
|
- // 计算距离和角度
|
|
|
- double dx = s_prev.x - s_next.x;
|
|
|
- double dy = s_prev.y - s_next.y;
|
|
|
- double distance = sqrt(dx * dx + dy * dy);
|
|
|
- double line_angle = atan2(dy, dx);
|
|
|
-
|
|
|
- int index_start = s_prev.index;
|
|
|
- int index_end = s_next.index;
|
|
|
-
|
|
|
- // 应用过滤条件
|
|
|
- if (distance > 0.14 && (index_end - index_start) > 3)
|
|
|
- {
|
|
|
- turn_segments[index_start] = std::make_tuple(
|
|
|
- index_start, // 0: 开始索引
|
|
|
- index_end, // 1: 结束索引
|
|
|
- 0.0, // 2: 0.0
|
|
|
- s_prev.x, // 3: x坐标
|
|
|
- s_prev.y, // 4: y坐标
|
|
|
- distance, // 5: 距离
|
|
|
- line_angle, // 6: 线角度
|
|
|
- 1 // 7: 1
|
|
|
- );
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- return turn_segments;
|
|
|
-}
|
|
|
-
|
|
|
-std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
|
|
|
-WheelchairStateMachine::filterSegments(
|
|
|
- const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments)
|
|
|
-{
|
|
|
- std::map<int, std::tuple<int, int, double, double, double, double, double, int>> new_segments;
|
|
|
-
|
|
|
- if (turn_segments.empty() || turn_segments.size() < 2)
|
|
|
- {
|
|
|
- return new_segments;
|
|
|
- }
|
|
|
-
|
|
|
- bool last_line_angle_valid = false;
|
|
|
- double last_line_angle = 0.0;
|
|
|
- std::tuple<int, int, double, double, double, double, double, int> last_seg;
|
|
|
- int last_key = -1;
|
|
|
-
|
|
|
- // 遍历所有分段
|
|
|
- for (const auto &seg : turn_segments)
|
|
|
- {
|
|
|
- const auto &info = seg.second;
|
|
|
- double line_angle = std::get<6>(info);
|
|
|
-
|
|
|
- if (last_line_angle_valid)
|
|
|
- {
|
|
|
- // 计算角度差
|
|
|
- double diff_angle = line_angle - last_line_angle;
|
|
|
-
|
|
|
- // 目标角度计算
|
|
|
- double target_angle = 0.5 * M_PI * (120.0 / 90.0);
|
|
|
- target_angle = M_PI - target_angle;
|
|
|
- double tolerance = 0.2;
|
|
|
-
|
|
|
- // 角度条件判断
|
|
|
- if (diff_angle > (target_angle - tolerance) && diff_angle < (target_angle + tolerance))
|
|
|
- {
|
|
|
- // 索引检查
|
|
|
- int right_start_idx = std::get<0>(info);
|
|
|
- int left_end_idx = std::get<1>(last_seg);
|
|
|
-
|
|
|
- if (right_start_idx - left_end_idx < 3)
|
|
|
- {
|
|
|
- // 添加到结果
|
|
|
- new_segments[last_key] = last_seg;
|
|
|
- new_segments[seg.first] = info;
|
|
|
- break; // 找到一对就跳出循环
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- // 更新上一个分段信息
|
|
|
- last_line_angle = line_angle;
|
|
|
- last_line_angle_valid = true;
|
|
|
- last_seg = info;
|
|
|
- last_key = seg.first;
|
|
|
- }
|
|
|
-
|
|
|
- return new_segments;
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 辅助函数实现 ====================
|
|
|
-
|
|
|
-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 += " - 电池状态: " + batteryStateToString(battery_state_) +
|
|
|
- " (" + std::to_string(battery_percentage_) + "%)\n";
|
|
|
-
|
|
|
- // 充电状态
|
|
|
- status_report += " - 充电状态: " + chargingStateToString(charging_state_) + "\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::publishRechargeVisualization(const RechargeResult &result)
|
|
|
-{
|
|
|
- auto marker_array = std::make_shared<visualization_msgs::msg::MarkerArray>();
|
|
|
-
|
|
|
- // 添加中线标记
|
|
|
- visualization_msgs::msg::Marker line_marker;
|
|
|
- line_marker.header.frame_id = "laser_frame_back1";
|
|
|
- line_marker.header.stamp = this->now();
|
|
|
- line_marker.ns = "rotating_line";
|
|
|
- line_marker.id = 0;
|
|
|
- line_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
|
|
|
- line_marker.action = visualization_msgs::msg::Marker::ADD;
|
|
|
- line_marker.pose.orientation.w = 1.0;
|
|
|
- line_marker.scale.x = 0.02;
|
|
|
- line_marker.color.r = 1.0;
|
|
|
- line_marker.color.a = 1.0;
|
|
|
-
|
|
|
- // 计算线段端点
|
|
|
- double k = result.middle_line.k;
|
|
|
- double b = result.middle_line.b;
|
|
|
- double x1 = result.middle_point.x;
|
|
|
- double x2 = x1 - 1.0;
|
|
|
-
|
|
|
- if (x1 > 10.0 || x1 < -10.0)
|
|
|
- {
|
|
|
- x1 = 0.5;
|
|
|
- x2 = -0.5;
|
|
|
- }
|
|
|
-
|
|
|
- geometry_msgs::msg::Point p1, p2;
|
|
|
- p1.x = x1;
|
|
|
- p1.y = k * x1 + b;
|
|
|
- p1.z = 0.0;
|
|
|
- p2.x = x2;
|
|
|
- p2.y = k * x2 + b;
|
|
|
- p2.z = 0.0;
|
|
|
-
|
|
|
- line_marker.points.push_back(p1);
|
|
|
- line_marker.points.push_back(p2);
|
|
|
- marker_array->markers.push_back(line_marker);
|
|
|
-
|
|
|
- // 添加中间点标记
|
|
|
- visualization_msgs::msg::Marker middle_marker;
|
|
|
- middle_marker.header.frame_id = "laser_frame_back1";
|
|
|
- middle_marker.header.stamp = this->now();
|
|
|
- middle_marker.ns = "middle_point";
|
|
|
- middle_marker.id = 1;
|
|
|
- middle_marker.type = visualization_msgs::msg::Marker::SPHERE;
|
|
|
- middle_marker.action = visualization_msgs::msg::Marker::ADD;
|
|
|
- middle_marker.pose.orientation.w = 1.0;
|
|
|
- middle_marker.scale.x = 0.03;
|
|
|
- middle_marker.scale.y = 0.03;
|
|
|
- middle_marker.color.b = 1.0;
|
|
|
- middle_marker.color.a = 1.0;
|
|
|
-
|
|
|
- middle_marker.pose.position.x = result.middle_point.x;
|
|
|
- middle_marker.pose.position.y = result.middle_point.y;
|
|
|
- middle_marker.pose.position.z = 0.0;
|
|
|
-
|
|
|
- marker_array->markers.push_back(middle_marker);
|
|
|
-
|
|
|
- // 发布标记数组
|
|
|
- pub_rviz_->publish(*marker_array);
|
|
|
-}
|
|
|
-
|
|
|
-Eigen::MatrixXd WheelchairStateMachine::getPointList(const std::vector<IndexPoint> &points)
|
|
|
-{
|
|
|
- Eigen::MatrixXd pts(points.size(), 2);
|
|
|
-
|
|
|
- for (size_t i = 0; i < points.size(); i++)
|
|
|
- {
|
|
|
- pts(i, 0) = points[i].x;
|
|
|
- pts(i, 1) = points[i].y;
|
|
|
- }
|
|
|
-
|
|
|
- return pts;
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::rvizShowResult(const RechargeResult &result)
|
|
|
-{
|
|
|
- // 与 publishRechargeVisualization 功能相同
|
|
|
- publishRechargeVisualization(result);
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::publishFilteredScan(
|
|
|
- const sensor_msgs::msg::LaserScan::SharedPtr original_msg,
|
|
|
- const std::vector<float> &filtered_ranges)
|
|
|
-{
|
|
|
- auto msg = std::make_shared<sensor_msgs::msg::LaserScan>();
|
|
|
- msg->header = original_msg->header;
|
|
|
- msg->angle_min = original_msg->angle_min;
|
|
|
- msg->angle_max = original_msg->angle_max;
|
|
|
- msg->angle_increment = original_msg->angle_increment;
|
|
|
- msg->time_increment = original_msg->time_increment;
|
|
|
- msg->scan_time = original_msg->scan_time;
|
|
|
- msg->range_min = original_msg->range_min;
|
|
|
- msg->range_max = original_msg->range_max;
|
|
|
- msg->ranges = filtered_ranges;
|
|
|
- msg->intensities = original_msg->intensities;
|
|
|
-
|
|
|
- pub_filtered_->publish(*msg);
|
|
|
- RCLCPP_DEBUG(this->get_logger(), "已发布过滤后的激光数据");
|
|
|
-}
|
|
|
-
|
|
|
-sensor_msgs::msg::LaserScan::SharedPtr WheelchairStateMachine::resetLaserData(
|
|
|
- sensor_msgs::msg::LaserScan::SharedPtr msg,
|
|
|
- int b_index, int e_index)
|
|
|
-{
|
|
|
- std::vector<float> filtered_ranges = msg->ranges;
|
|
|
- std::vector<float> intensities = msg->intensities;
|
|
|
-
|
|
|
- for (int i = 0; i < b_index; i++)
|
|
|
- {
|
|
|
- filtered_ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- intensities[i] = 0.0;
|
|
|
- }
|
|
|
-
|
|
|
- for (int i = e_index; i < static_cast<int>(msg->ranges.size()); i++)
|
|
|
- {
|
|
|
- filtered_ranges[i] = std::numeric_limits<float>::infinity();
|
|
|
- intensities[i] = 0.0;
|
|
|
- }
|
|
|
-
|
|
|
- msg->ranges = filtered_ranges;
|
|
|
- msg->intensities = intensities;
|
|
|
-
|
|
|
- return msg;
|
|
|
-}
|
|
|
-
|
|
|
-bool WheelchairStateMachine::isValid(float value) const
|
|
|
-{
|
|
|
- if (fabs(value) < 0.001)
|
|
|
- return false;
|
|
|
- return !std::isinf(value) && !std::isnan(value);
|
|
|
-}
|
|
|
-
|
|
|
-int WheelchairStateMachine::getFixedIndex(const sensor_msgs::msg::LaserScan::SharedPtr msg, int i) const
|
|
|
-{
|
|
|
- int fixed = getFixed();
|
|
|
- if (fixed == 0)
|
|
|
- return i;
|
|
|
-
|
|
|
- int length = msg->ranges.size();
|
|
|
- int index;
|
|
|
-
|
|
|
- if (i >= length - fixed)
|
|
|
- {
|
|
|
- index = i - length + fixed;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- index = i + fixed;
|
|
|
- }
|
|
|
- return index;
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 充电检测函数 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::setChargingVoltage(bool value)
|
|
|
-{
|
|
|
- detect_charging_voltage_ = value;
|
|
|
- detect_charging_voltage_time_ = this->now();
|
|
|
-
|
|
|
- if (value)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "检测到充电电压");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "充电电压已断开");
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-bool WheelchairStateMachine::getChargingVoltage() const
|
|
|
-{
|
|
|
- auto now = this->now();
|
|
|
- auto diff = now - detect_charging_voltage_time_;
|
|
|
-
|
|
|
- // 如果检测到充电电压且在0.1秒内，认为有效
|
|
|
- if (detect_charging_voltage_ && diff.seconds() < 0.1)
|
|
|
- {
|
|
|
- return true;
|
|
|
- }
|
|
|
- return false;
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::setMode(const std::string &mode)
|
|
|
-{
|
|
|
- last_mode_ = mode;
|
|
|
- if (last_mode_ != "recharge")
|
|
|
- {
|
|
|
- setChargingVoltage(false);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-std::string WheelchairStateMachine::getMode() const
|
|
|
-{
|
|
|
- return last_mode_;
|
|
|
-}
|
|
|
-
|
|
|
-// ==================== 回充控制接口 ====================
|
|
|
-
|
|
|
-void WheelchairStateMachine::startRecharge()
|
|
|
-{
|
|
|
- if (current_state_ == WheelchairState::READY)
|
|
|
- {
|
|
|
- handleIpadPhoneStart();
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "当前状态无法启动回充: %s", getCurrentState().c_str());
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void WheelchairStateMachine::cancelRecharge()
|
|
|
-{
|
|
|
- handleIpadPhoneCancel();
|
|
|
-}
|
xuqian