|
|
@@ -0,0 +1,2366 @@
|
|
|
+// 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();
|
|
|
+}
|
sousenw