|
|
@@ -1,1673 +0,0 @@
|
|
|
-#include "recharge/recharge.hpp"
|
|
|
-#include <rclcpp/executors/multi_threaded_executor.hpp>
|
|
|
-#include <rclcpp/qos.hpp>
|
|
|
-#include <tf2/LinearMath/Quaternion.h>
|
|
|
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
|
|
-#include <chrono>
|
|
|
-#include <functional>
|
|
|
-#include <thread>
|
|
|
-#define DEBUG_SCAN 1
|
|
|
-
|
|
|
-using std::placeholders::_1;
|
|
|
-
|
|
|
-// 全局变量
|
|
|
-static std::string last_mode = "";
|
|
|
-static bool detect_charging_voltage = false;
|
|
|
-static rclcpp::Time detect_charging_voltage_time;
|
|
|
-
|
|
|
-// 全局函数实现
|
|
|
-bool isCharging()
|
|
|
-{
|
|
|
- return last_mode == "recharge";
|
|
|
-}
|
|
|
-
|
|
|
-// 在全局函数部分添加
|
|
|
-std::pair<double, double> 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};
|
|
|
-}
|
|
|
-
|
|
|
-void setChargingVoltage(bool value)
|
|
|
-{
|
|
|
- detect_charging_voltage = value;
|
|
|
- detect_charging_voltage_time = rclcpp::Clock().now();
|
|
|
-}
|
|
|
-
|
|
|
-bool getChargingVoltage()
|
|
|
-{
|
|
|
- auto now = rclcpp::Clock().now();
|
|
|
- auto diff = now - detect_charging_voltage_time;
|
|
|
- if (detect_charging_voltage && diff.seconds() > 0.1)
|
|
|
- {
|
|
|
- return true;
|
|
|
- }
|
|
|
- return false;
|
|
|
-}
|
|
|
-
|
|
|
-void setMode(const std::string &mode)
|
|
|
-{
|
|
|
- last_mode = mode;
|
|
|
- if (last_mode != "recharge")
|
|
|
- {
|
|
|
- setChargingVoltage(false);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-std::string getMode()
|
|
|
-{
|
|
|
- return last_mode;
|
|
|
-}
|
|
|
-
|
|
|
-// ToolClass 实现
|
|
|
-double ToolClass::angleBetween(const IndexPoint &p1, const IndexPoint &p2)
|
|
|
-{
|
|
|
- return atan2(p2.y - p1.y, p2.x - p1.x);
|
|
|
-}
|
|
|
-
|
|
|
-double ToolClass::angleDiff(double a1, double a2)
|
|
|
-{
|
|
|
- double diff = fabs(a1 - a2);
|
|
|
- return std::min(diff, 2 * M_PI - diff) * 180.0 / M_PI;
|
|
|
-}
|
|
|
-
|
|
|
-double ToolClass::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>> ToolClass::splitPointsToSegments(
|
|
|
- const std::vector<IndexPoint> &points,
|
|
|
- double angle_threshold_deg,
|
|
|
- int history_window_size)
|
|
|
-{
|
|
|
- RCLCPP_INFO(rclcpp::get_logger("toolclass"),
|
|
|
- "splitPointsToSegments开始: %zu个点", points.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; // 改为 deque 方便操作
|
|
|
-
|
|
|
- current_segment.push_back(points[0]);
|
|
|
-
|
|
|
- for (size_t i = 1; i < points.size(); i++)
|
|
|
- {
|
|
|
- RCLCPP_INFO(rclcpp::get_logger("toolclass"),
|
|
|
- "点[%zu]: (%.3f, %.3f), 角度历史大小: %zu", i, points[i].x, points[i].y, angle_history.size());
|
|
|
- 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);
|
|
|
- RCLCPP_INFO(rclcpp::get_logger("toolclass"),
|
|
|
- " 角度差: %.1f° (阈值: %.1f°)", diff, angle_threshold_deg);
|
|
|
- if (diff > angle_threshold_deg)
|
|
|
- {
|
|
|
- RCLCPP_INFO(rclcpp::get_logger("toolclass"),
|
|
|
- " 检测到角度变化，创建新分段");
|
|
|
- // 发生显著拐弯，分段
|
|
|
- // 将 p_prev 加入当前分段
|
|
|
- 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(p_curr);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- // 正常情况：将当前点加入当前分段
|
|
|
- // current_segment.push_back(p_curr);
|
|
|
- }
|
|
|
- current_segment.push_back(points.back());
|
|
|
- segments.push_back(current_segment);
|
|
|
- RCLCPP_INFO(rclcpp::get_logger("toolclass"),
|
|
|
- "分段完成: 共%zu个分段", segments.size());
|
|
|
- return segments;
|
|
|
-}
|
|
|
-
|
|
|
-// ScanFilterNode 实现
|
|
|
-ScanFilterNode::ScanFilterNode() : Node("scan_filter_node")
|
|
|
-{
|
|
|
-
|
|
|
- // 声明参数
|
|
|
- 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(&ScanFilterNode::scanCallback, this, _1));
|
|
|
-
|
|
|
- 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);
|
|
|
- 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(&ScanFilterNode::onCmdEnable, this, _1));
|
|
|
-
|
|
|
- pub_rviz_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
|
|
|
- "/line_marker_array", 10);
|
|
|
-
|
|
|
- // 初始化控制消息
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
-
|
|
|
- // 创建定时器
|
|
|
- timer_ = this->create_wall_timer(
|
|
|
- 100ms, std::bind(&ScanFilterNode::timerCallback, this));
|
|
|
-
|
|
|
- detect_station_time_ = this->now() - rclcpp::Duration(100.0, 0);
|
|
|
-}
|
|
|
-
|
|
|
-Eigen::MatrixXd ScanFilterNode::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 ScanFilterNode::procData(Result &result)
|
|
|
-{
|
|
|
- RCLCPP_INFO(get_logger(), "=== procData 开始 ===");
|
|
|
- RCLCPP_INFO(get_logger(), "左点数量: %zu", result.left_points.size());
|
|
|
- RCLCPP_INFO(get_logger(), "右点数量: %zu", result.right_points.size());
|
|
|
-
|
|
|
- if (result.left_points.size() < 2 || result.right_points.size() < 2) {
|
|
|
- RCLCPP_WARN(get_logger(), "点数量不足，跳过拟合");
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 打印前几个点用于调试
|
|
|
- for (int i = 0; i < std::min(3, (int)result.left_points.size()); i++) {
|
|
|
- RCLCPP_INFO(get_logger(), "左点[%d]: (%.3f, %.3f)",
|
|
|
- i, result.left_points[i].x, result.left_points[i].y);
|
|
|
- }
|
|
|
-
|
|
|
- for (int i = 0; i < std::min(3, (int)result.right_points.size()); i++) {
|
|
|
- RCLCPP_INFO(get_logger(), "右点[%d]: (%.3f, %.3f)",
|
|
|
- i, result.right_points[i].x, result.right_points[i].y);
|
|
|
- }
|
|
|
-
|
|
|
- // 获取点集矩阵
|
|
|
- Eigen::MatrixXd point_list_left = getPointList(result.left_points);
|
|
|
- Eigen::MatrixXd point_list_right = getPointList(result.right_points);
|
|
|
-
|
|
|
- // 拟合左边直线
|
|
|
- auto [k1, b1] = fitLineLeastSquares(point_list_left);
|
|
|
- RCLCPP_INFO(get_logger(), "左直线拟合: y = %.4fx + %.4f", k1, b1);
|
|
|
-
|
|
|
- // 拟合右边直线
|
|
|
- auto [k2, b2] = fitLineLeastSquares(point_list_right);
|
|
|
- RCLCPP_INFO(get_logger(), "右直线拟合: y = %.4fx + %.4f", k2, b2);
|
|
|
-
|
|
|
- // 检查是否平行
|
|
|
- if (fabs(k1 - k2) < 1e-10) {
|
|
|
- RCLCPP_WARN(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_INFO(get_logger(), "交点: (%.4f, %.4f)", x, y);
|
|
|
-
|
|
|
- // 更新中间点
|
|
|
- result.middle_point = IndexPoint(x, y, 0);
|
|
|
-
|
|
|
- // 计算角平分线
|
|
|
- auto [k3, b3] = calculateAngleBisector(k1, b1, k2, b2);
|
|
|
- RCLCPP_INFO(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);
|
|
|
-
|
|
|
- RCLCPP_INFO(get_logger(), "=== procData 完成 ===");
|
|
|
-}
|
|
|
-
|
|
|
-void ScanFilterNode::ctrlProc(Result &result)
|
|
|
-{
|
|
|
- RCLCPP_INFO(get_logger(), "======== 进入控制模块 ========");
|
|
|
-
|
|
|
- // 检查是否检测到充电电压
|
|
|
- if (getChargingVoltage()) {
|
|
|
- RCLCPP_INFO(get_logger(), "检测到充电电压，停止移动");
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 使用 result 中已经计算好的数据
|
|
|
- double dx = result.middle_point.x;
|
|
|
- double dy = result.middle_point.y;
|
|
|
- double k = result.middle_line.k;
|
|
|
-
|
|
|
- RCLCPP_INFO(get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);
|
|
|
-
|
|
|
- // 调整 dy（与 Python 代码保持一致）
|
|
|
- dy = dy - 0.10;
|
|
|
- RCLCPP_INFO(get_logger(), "调整后 dy: %.4f", dy);
|
|
|
-
|
|
|
- // 计算偏航角误差
|
|
|
- double yaw_error = atan(k);
|
|
|
- RCLCPP_INFO(get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
|
|
|
- yaw_error, yaw_error * 180.0 / M_PI);
|
|
|
-
|
|
|
- // 计算控制速度
|
|
|
- auto [linear_velocity, angular_velocity] = computeDockingVelocity(dx - 0.3, dy, yaw_error);
|
|
|
- RCLCPP_INFO(get_logger(), "基础控制速度 - 线速度: %.4f, 角速度: %.4f",
|
|
|
- linear_velocity, angular_velocity);
|
|
|
-
|
|
|
- // 限制角速度（与 Python 代码保持一致）
|
|
|
- if (angular_velocity > 0.0) {
|
|
|
- angular_velocity = std::min(fabs(angular_velocity), 0.09);
|
|
|
- } else {
|
|
|
- angular_velocity = -std::min(fabs(angular_velocity), 0.09);
|
|
|
- }
|
|
|
-
|
|
|
- // 更新检测时间
|
|
|
- detect_station_time_ = this->now();
|
|
|
-
|
|
|
- // 根据距离调整速度系数（与 Python 代码保持一致）
|
|
|
- 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_INFO(get_logger(), "距离调整系数 - radio_x: %.2f, radio_z: %.2f", radio_x, radio_z);
|
|
|
-
|
|
|
- // 设置控制命令
|
|
|
- if (dx > 0.54) {
|
|
|
- // 计算最终线速度（注意：Python 中使用了 min(linear_velocity * radio_x, -0.04)）
|
|
|
- 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_INFO(get_logger(), "控制命令 - 线速度: %.4f, 角速度: %.4f",
|
|
|
- ctrl_twist_.linear.x, ctrl_twist_.angular.z);
|
|
|
- } else {
|
|
|
- // 到达充电位置
|
|
|
- RCLCPP_INFO(get_logger(), "到达充电位置 (dx=%.4f <= 0.54), 停止移动", dx);
|
|
|
- ctrl_twist_.linear.x = 0.0;
|
|
|
- ctrl_twist_.angular.z = 0.0;
|
|
|
- ctrl_twist_time_ = this->now();
|
|
|
- }
|
|
|
-
|
|
|
- // 更新帧索引
|
|
|
- frame_index_++;
|
|
|
- RCLCPP_INFO(get_logger(), "======== 控制模块结束 ========");
|
|
|
-}
|
|
|
-
|
|
|
-std::vector<std::pair<int, int>> ScanFilterNode::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;
|
|
|
-}
|
|
|
-
|
|
|
-// getSegments函数，与Python代码完全一致
|
|
|
-std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
|
|
|
-ScanFilterNode::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;
|
|
|
- }
|
|
|
-
|
|
|
- // 1. 将点转换为路径（与Python完全一致）
|
|
|
- std::vector<IndexPoint> path;
|
|
|
- for (const auto &point : xy_points_map)
|
|
|
- {
|
|
|
- // Python: data = (value[0], value[1], index)
|
|
|
- path.emplace_back(point.second.first, point.second.second, point.first);
|
|
|
- }
|
|
|
-
|
|
|
- // 2. 使用相同的参数进行分段
|
|
|
- auto segments = ToolClass::splitPointsToSegments(path, 24.0, 4);
|
|
|
-
|
|
|
- // 3. 处理每个分段（与Python完全一致）
|
|
|
- for (size_t i = 0; i < segments.size(); i++)
|
|
|
- {
|
|
|
- if (segments[i].size() < 2)
|
|
|
- continue;
|
|
|
-
|
|
|
- // 获取分段的前两个点（与Python完全一致）
|
|
|
- const auto &s_prev = segments[i][0];
|
|
|
- const auto &s_next = segments[i][1];
|
|
|
-
|
|
|
- // 计算距离和角度（与Python完全一致）
|
|
|
- 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); // 注意：与Python的math.atan2一致
|
|
|
-
|
|
|
- int index_start = s_prev.index;
|
|
|
- int index_end = s_next.index;
|
|
|
-
|
|
|
- // 应用相同的过滤条件（与Python完全一致）
|
|
|
- if (distance > 0.14 && (index_end - index_start) > 3)
|
|
|
- {
|
|
|
- // 创建与Python完全相同的8元组
|
|
|
- // Python: (index_start, index_end, 0.0, value[0], value[1], distance, line_angle, 1)
|
|
|
- // 但这里应该是s_prev的x,y，不是value[0], value[1]
|
|
|
- turn_segments[index_start] = std::make_tuple(
|
|
|
- index_start, // 0: 开始索引
|
|
|
- index_end, // 1: 结束索引
|
|
|
- 0.0, // 2: 0.0（与Python一致）
|
|
|
- s_prev.x, // 3: x坐标
|
|
|
- s_prev.y, // 4: y坐标
|
|
|
- distance, // 5: 距离
|
|
|
- line_angle, // 6: 线角度
|
|
|
- 1 // 7: 1（与Python一致）
|
|
|
- );
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- return turn_segments;
|
|
|
-}
|
|
|
-// filterSegments函数，与Python代码完全一致
|
|
|
-std::map<int, std::tuple<int, int, double, double, double, double, double, int>> ScanFilterNode::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;
|
|
|
-
|
|
|
- // 与Python完全相同的条件
|
|
|
- if (turn_segments.empty() || turn_segments.size() < 2)
|
|
|
- {
|
|
|
- return new_segments;
|
|
|
- }
|
|
|
-
|
|
|
- // 变量命名和逻辑与Python保持一致
|
|
|
- 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;
|
|
|
-
|
|
|
- // 遍历所有分段（与Python完全一致）
|
|
|
- for (const auto &seg : turn_segments)
|
|
|
- {
|
|
|
- const auto &info = seg.second;
|
|
|
- double line_angle = std::get<6>(info); // 获取角度
|
|
|
-
|
|
|
- if (last_line_angle_valid)
|
|
|
- {
|
|
|
- // 计算角度差（与Python完全一致：line_angle - last_line_angle）
|
|
|
- double diff_angle = line_angle - last_line_angle;
|
|
|
-
|
|
|
- // 目标角度计算（与Python完全一致）
|
|
|
- // Python: target_angle = 0.5*math.pi* (120/90.0)
|
|
|
- // Python: target_angle = math.pi - target_angle
|
|
|
- double target_angle = 0.5 * M_PI * (120.0 / 90.0);
|
|
|
- target_angle = M_PI - target_angle;
|
|
|
- double tolerance = 0.2;
|
|
|
-
|
|
|
- // 角度条件判断（与Python完全一致）
|
|
|
- if (diff_angle > (target_angle - tolerance) && diff_angle < (target_angle + tolerance))
|
|
|
- {
|
|
|
- // 索引检查（与Python完全一致）
|
|
|
- // Python: if right_seg[1][0] - left_seg[1][1] < 3:
|
|
|
- // right_seg[1][0] = 右分段的开始索引 = std::get<0>(info)
|
|
|
- // left_seg[1][1] = 左分段的结束索引 = std::get<1>(last_seg)
|
|
|
- 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)
|
|
|
- {
|
|
|
- // 添加到结果（与Python完全一致）
|
|
|
- new_segments[last_key] = last_seg;
|
|
|
- new_segments[seg.first] = info;
|
|
|
- break; // 找到一对就跳出循环
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- // 更新上一个分段信息（与Python完全一致）
|
|
|
- last_line_angle = line_angle;
|
|
|
- last_line_angle_valid = true;
|
|
|
- last_seg = info;
|
|
|
- last_key = seg.first;
|
|
|
- }
|
|
|
-
|
|
|
- return new_segments;
|
|
|
-}
|
|
|
-void ScanFilterNode::onPointPartition(Result &result)
|
|
|
-{
|
|
|
- RCLCPP_INFO(get_logger(), "开始处理点分区");
|
|
|
- if (result.left_points.size() == 0 || result.right_points.size() == 0)
|
|
|
- {
|
|
|
- RCLCPP_ERROR(get_logger(), "左右两侧没有两个以上的点模拟线");
|
|
|
- RCLCPP_ERROR(get_logger(), "左侧点数: %zu, 右侧点数: %zu",
|
|
|
- result.left_points.size(), result.right_points.size());
|
|
|
- return;
|
|
|
- }
|
|
|
- procData(result);
|
|
|
- // 调用控制处理
|
|
|
- ctrlProc(result);
|
|
|
-
|
|
|
- // 显示结果（RViz可视化）
|
|
|
- rvizShowResult(result);
|
|
|
- RCLCPP_INFO(get_logger(), "已发布可视化标记");
|
|
|
-}
|
|
|
-
|
|
|
-void ScanFilterNode::rvizShowResult(const Result &result)
|
|
|
-{
|
|
|
- RCLCPP_INFO(get_logger(), "发布可视化标记 - 坐标系: laser_frame_back1");
|
|
|
- RCLCPP_INFO(get_logger(), "中间点坐标: x=%.3f, y=%.3f",
|
|
|
- result.middle_point.x, result.middle_point.y);
|
|
|
- RCLCPP_INFO(get_logger(), "中线参数: k=%.3f, b=%.3f",
|
|
|
- result.middle_line.k, result.middle_line.b);
|
|
|
- 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;
|
|
|
- }
|
|
|
- double y1 = result.middle_line.k * x1 + result.middle_line.b;
|
|
|
- double y2 = result.middle_line.k * x2 + result.middle_line.b;
|
|
|
- RCLCPP_INFO(get_logger(), "线段端点: (%.3f, %.3f) -> (%.3f, %.3f)",
|
|
|
- x1, y1, x2, y2);
|
|
|
- // 创建标记数组
|
|
|
- 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;
|
|
|
-
|
|
|
- geometry_msgs::msg::Point p1, p2;
|
|
|
- p1.x = x1;
|
|
|
- p1.y = y1;
|
|
|
- p1.z = 0.0;
|
|
|
- p2.x = x2;
|
|
|
- p2.y = y2;
|
|
|
- p2.z = 0.0;
|
|
|
- line_marker.points.push_back(p1);
|
|
|
- line_marker.points.push_back(p2);
|
|
|
-
|
|
|
- marker_array->markers.push_back(line_marker);
|
|
|
-
|
|
|
- // 添加左点标记
|
|
|
- if (!result.left_points.empty())
|
|
|
- {
|
|
|
- visualization_msgs::msg::Marker left_marker;
|
|
|
- left_marker.header.frame_id = "laser_frame_back1";
|
|
|
- left_marker.header.stamp = this->now();
|
|
|
- left_marker.ns = "left_point";
|
|
|
- left_marker.id = 1;
|
|
|
- left_marker.type = visualization_msgs::msg::Marker::SPHERE;
|
|
|
- left_marker.action = visualization_msgs::msg::Marker::ADD;
|
|
|
- left_marker.pose.orientation.w = 1.0;
|
|
|
- left_marker.scale.x = 0.03;
|
|
|
- left_marker.scale.y = 0.03;
|
|
|
- left_marker.color.r = 1.0;
|
|
|
- left_marker.color.a = 1.0;
|
|
|
-
|
|
|
- left_marker.pose.position.x = result.left_points[0].x;
|
|
|
- left_marker.pose.position.y = result.left_points[0].y;
|
|
|
- left_marker.pose.position.z = 0.0;
|
|
|
-
|
|
|
- marker_array->markers.push_back(left_marker);
|
|
|
- }
|
|
|
-
|
|
|
- // 添加右点标记
|
|
|
- if (!result.right_points.empty())
|
|
|
- {
|
|
|
- visualization_msgs::msg::Marker right_marker;
|
|
|
- right_marker.header.frame_id = "laser_frame_back1";
|
|
|
- right_marker.header.stamp = this->now();
|
|
|
- right_marker.ns = "right_point";
|
|
|
- right_marker.id = 2;
|
|
|
- right_marker.type = visualization_msgs::msg::Marker::SPHERE;
|
|
|
- right_marker.action = visualization_msgs::msg::Marker::ADD;
|
|
|
- right_marker.pose.orientation.w = 1.0;
|
|
|
- right_marker.scale.x = 0.03;
|
|
|
- right_marker.scale.y = 0.03;
|
|
|
- right_marker.color.g = 1.0;
|
|
|
- right_marker.color.a = 1.0;
|
|
|
-
|
|
|
- right_marker.pose.position.x = result.right_points.back().x;
|
|
|
- right_marker.pose.position.y = result.right_points.back().y;
|
|
|
- right_marker.pose.position.z = 0.0;
|
|
|
-
|
|
|
- marker_array->markers.push_back(right_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 = 3;
|
|
|
- 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);
|
|
|
-}
|
|
|
-
|
|
|
-void ScanFilterNode::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
|
|
|
-{
|
|
|
- cmd_enable_ = msg->data;
|
|
|
-}
|
|
|
-
|
|
|
-void ScanFilterNode::timerCallback()
|
|
|
-{
|
|
|
- if (!isCharging())
|
|
|
- return;
|
|
|
-
|
|
|
- auto now = this->now();
|
|
|
- auto time_diff = (now - ctrl_twist_time_).seconds();
|
|
|
-
|
|
|
- if (time_diff < 2.0)
|
|
|
- {
|
|
|
- double radio_x = 1.0;
|
|
|
- double radio_z = 1.0;
|
|
|
- geometry_msgs::msg::Twist msg;
|
|
|
- msg.linear.x = ctrl_twist_.linear.x * radio_x;
|
|
|
- msg.angular.z = ctrl_twist_.angular.z * radio_z;
|
|
|
-
|
|
|
- if (cmd_enable_)
|
|
|
- {
|
|
|
- pub_ctrl_->publish(msg);
|
|
|
- }
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-bool ScanFilterNode::isValid(float value)
|
|
|
-{
|
|
|
- if (fabs(value) < 0.001)
|
|
|
- return false;
|
|
|
- return !std::isinf(value) && !std::isnan(value);
|
|
|
-}
|
|
|
-
|
|
|
-int ScanFilterNode::getFixed()
|
|
|
-{
|
|
|
- return 0;
|
|
|
-}
|
|
|
-
|
|
|
-int ScanFilterNode::getFixedIndex(const sensor_msgs::msg::LaserScan::SharedPtr msg, int i)
|
|
|
-{
|
|
|
- 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 ScanFilterNode::filterScanModify0(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
-#if DEBUG_SCAN
|
|
|
- RCLCPP_INFO(get_logger(), "已进入角度偏移校正过滤");
|
|
|
-#endif
|
|
|
- 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 ScanFilterNode::filterScanModify1(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
-#if DEBUG_SCAN
|
|
|
- RCLCPP_INFO(get_logger(), "已进入角度范围过滤");
|
|
|
-#endif
|
|
|
- // 过滤角度，只接受后面90度
|
|
|
- int min_index = 292;
|
|
|
-
|
|
|
- int max_index = 382;
|
|
|
- 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 ScanFilterNode::filterScanModify2(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
- RCLCPP_INFO(get_logger(), "已进入距离过滤");
|
|
|
- // 过滤距离
|
|
|
- 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 ScanFilterNode::filterScanModify3(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
-#if DEBUG_SCAN
|
|
|
- RCLCPP_INFO(get_logger(), "已进入反射强度过滤");
|
|
|
-#endif
|
|
|
- // 过滤强度
|
|
|
- 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> ScanFilterNode::filterScanModify4(sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
-#if DEBUG_SCAN
|
|
|
- RCLCPP_INFO(get_logger(), "已进入点的连续性过滤");
|
|
|
-#endif
|
|
|
- // 过滤连续性
|
|
|
- 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 (msg->ranges.size() - begin_index < 10)
|
|
|
- {
|
|
|
- RCLCPP_ERROR(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>> ScanFilterNode::laserScanToXY(
|
|
|
- const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
|
|
|
-{
|
|
|
- RCLCPP_INFO(get_logger(), "进入xy坐标系转换");
|
|
|
- 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>> ScanFilterNode::laserScanToXY1(
|
|
|
- int begin_index, int end_index,
|
|
|
- const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
|
|
|
-{
|
|
|
- RCLCPP_INFO(get_logger(), "进入xy坐标系转换");
|
|
|
- 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::pair<double, double> ScanFilterNode::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> ScanFilterNode::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};
|
|
|
-}
|
|
|
-
|
|
|
-void ScanFilterNode::scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
|
|
|
-{
|
|
|
-#if DEBUG_SCAN
|
|
|
- RCLCPP_INFO(get_logger(), "已进入scan回调函数");
|
|
|
-#else
|
|
|
- if (!isCharging())
|
|
|
- return;
|
|
|
-#endif
|
|
|
- // 应用各种过滤器
|
|
|
- filterScanModify0(msg);
|
|
|
- filterScanModify1(msg);
|
|
|
- filterScanModify2(msg);
|
|
|
- filterScanModify3(msg);
|
|
|
-
|
|
|
- auto [begin_index, end_index] = filterScanModify4(msg);
|
|
|
- RCLCPP_INFO(get_logger(), "begin:%d,end:%d", begin_index, end_index);
|
|
|
- resetLaserData(msg, begin_index, end_index);
|
|
|
- if (end_index - begin_index < 2)
|
|
|
- {
|
|
|
- RCLCPP_INFO(get_logger(), "没有可连续性的点");
|
|
|
- return;
|
|
|
- }
|
|
|
- if (true) // 测试用，可以改为条件判断
|
|
|
- {
|
|
|
- // 转换到XY坐标系
|
|
|
- auto xy_list = laserScanToXY1(begin_index, end_index, msg);
|
|
|
- if (xy_list.size() < 2)
|
|
|
- {
|
|
|
- RCLCPP_WARN(get_logger(), "XY转换结果点数不足: %zu", xy_list.size());
|
|
|
- return;
|
|
|
- }
|
|
|
- // 获取分段
|
|
|
- auto sections = getSections(begin_index, end_index, msg, xy_list);
|
|
|
-
|
|
|
- if (sections.size() < 1)
|
|
|
- {
|
|
|
- RCLCPP_WARN(get_logger(), "没有获取到分段");
|
|
|
- return;
|
|
|
- }
|
|
|
- RCLCPP_INFO(get_logger(), "XY转换结果点数：%d", sections.size());
|
|
|
- if (true) // 选择最近的分段
|
|
|
- {
|
|
|
- int count = 0;
|
|
|
- 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 x_low = 100.0;
|
|
|
- double x_high = -100.0;
|
|
|
- int avg_count = 0;
|
|
|
- double avg_total = 0.0;
|
|
|
- double avg_value = 0.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;
|
|
|
- x_low = std::min(x_low, x);
|
|
|
- x_high = std::max(x_high, x);
|
|
|
- }
|
|
|
- avg_value = avg_total / avg_count;
|
|
|
-
|
|
|
- if (x_high - x_low < 0.03)
|
|
|
- continue;
|
|
|
-
|
|
|
- if (nearest_x > avg_value)
|
|
|
- {
|
|
|
- nearest_x = avg_value;
|
|
|
- nearest_index = i;
|
|
|
- }
|
|
|
- }
|
|
|
- begin_index = sections[nearest_index].first;
|
|
|
- end_index = sections[nearest_index].second;
|
|
|
- resetLaserData(msg, begin_index, end_index);
|
|
|
- }
|
|
|
- }
|
|
|
- // 进一步处理激光数据
|
|
|
- scanCallback1(msg, begin_index, end_index);
|
|
|
-}
|
|
|
-
|
|
|
-void ScanFilterNode::scanCallback1(
|
|
|
- const sensor_msgs::msg::LaserScan::SharedPtr msg,
|
|
|
- int begin_index, int end_index)
|
|
|
-{
|
|
|
-#if DEBUG_SCAN
|
|
|
- RCLCPP_INFO(get_logger(), "进一步处理过滤数据");
|
|
|
-#else
|
|
|
- if (!isCharging())
|
|
|
- return;
|
|
|
-#endif
|
|
|
-
|
|
|
- // 检查索引有效性
|
|
|
- if (begin_index < 0 || end_index >= static_cast<int>(msg->ranges.size()) || begin_index >= end_index)
|
|
|
- {
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 根据距离调整结束索引
|
|
|
- if (end_index > 0 && end_index < static_cast<int>(msg->ranges.size()) && msg->ranges[end_index - 1] > 0.9)
|
|
|
- {
|
|
|
- int count = end_index - begin_index;
|
|
|
- end_index = end_index - static_cast<int>(count * 0.16);
|
|
|
- }
|
|
|
- // 发布过滤后的数据
|
|
|
- publishFilteredScan(msg, msg->ranges);
|
|
|
- // 转换到XY坐标系
|
|
|
- auto xy_list = laserScanToXY(msg);
|
|
|
- if (xy_list.size() < 2)
|
|
|
- {
|
|
|
- RCLCPP_ERROR(get_logger(), "xy坐标系没有转换成功");
|
|
|
- return;
|
|
|
- }
|
|
|
- RCLCPP_INFO(get_logger(), "xy坐标系成功转换");
|
|
|
- RCLCPP_INFO(get_logger(), "XY转换结果: %zu 个点", xy_list.size());
|
|
|
- // 获取分段
|
|
|
- auto turn_segments = getSegments(xy_list);
|
|
|
- // 在scanCallback1中，获取分段后添加：
|
|
|
- RCLCPP_INFO(get_logger(), "原始分段数: %zu", turn_segments.size());
|
|
|
-
|
|
|
- // 打印每个分段的信息
|
|
|
- for (const auto &seg : turn_segments)
|
|
|
- {
|
|
|
- const auto &info = seg.second;
|
|
|
- int start_idx = std::get<0>(info);
|
|
|
- int end_idx = std::get<1>(info);
|
|
|
- double distance = std::get<5>(info);
|
|
|
- double angle = std::get<6>(info);
|
|
|
-
|
|
|
- RCLCPP_INFO(get_logger(), "分段 %d: 点数=%d, 距离=%.3f, 角度=%.3f度",
|
|
|
- seg.first, end_idx - start_idx + 1, distance, angle * 180.0 / M_PI);
|
|
|
- }
|
|
|
- turn_segments = filterSegments(turn_segments);
|
|
|
- RCLCPP_INFO(get_logger(), "提取分段成功");
|
|
|
- RCLCPP_INFO(get_logger(), "过滤后分段数: %zu", turn_segments.size());
|
|
|
- // 创建结果对象
|
|
|
- Result result;
|
|
|
- result.frameid = frame_index_;
|
|
|
- result.laserScanData = msg;
|
|
|
-
|
|
|
- // 处理每个分段
|
|
|
- int segment_index = 0;
|
|
|
- for (const auto &seg : turn_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.find(index) == xy_list.end())
|
|
|
- continue;
|
|
|
-
|
|
|
- double x = xy_list[index].first;
|
|
|
- double y = xy_list[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++;
|
|
|
- }
|
|
|
-
|
|
|
- // 处理点分区
|
|
|
- onPointPartition(result);
|
|
|
-}
|
|
|
-
|
|
|
-sensor_msgs::msg::LaserScan::SharedPtr ScanFilterNode::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;
|
|
|
-}
|
|
|
-
|
|
|
-void ScanFilterNode::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_INFO(get_logger(), "已成功发布过滤后的数据");
|
|
|
-}
|
|
|
-
|
|
|
-// ChargingManager 实现
|
|
|
-ChargingManager::ChargingManager() : Node("scan_filter")
|
|
|
-{
|
|
|
- sub_ctrl_ = this->create_subscription<std_msgs::msg::String>(
|
|
|
- "/recharge/ctrl_1", 10,
|
|
|
- std::bind(&ChargingManager::onCtrl, this, _1));
|
|
|
-
|
|
|
- state_ = ChargingState::IDLE;
|
|
|
- last_station_detection_time_ = this->now();
|
|
|
- last_voltage_detection_time_ = this->now();
|
|
|
- setChargingVoltage(false);
|
|
|
-}
|
|
|
-
|
|
|
-bool ChargingManager::detectStation()
|
|
|
-{
|
|
|
- // 这里需要实现基站检测逻辑
|
|
|
- // 暂时使用简单实现
|
|
|
- auto now = this->now();
|
|
|
- // 假设有一个全局的 ScanFilterNode 实例
|
|
|
- // 在实际中，需要通过节点间通信获取这个信息
|
|
|
- return false; // 需要根据实际情况实现
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::onCtrl(const std_msgs::msg::String::SharedPtr msg)
|
|
|
-{
|
|
|
- if (msg->data == "start")
|
|
|
- {
|
|
|
- setChargingVoltage(false);
|
|
|
- startCharging();
|
|
|
- }
|
|
|
- else if (msg->data == "stop")
|
|
|
- {
|
|
|
- stopCharging();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::startCharging()
|
|
|
-{
|
|
|
- if (state_ != ChargingState::IDLE)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "当前状态无法启动回充");
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- changeState(ChargingState::STARTING);
|
|
|
- last_station_detection_time_ = this->now();
|
|
|
-
|
|
|
- // 检测基站
|
|
|
- bool station_detected = false;
|
|
|
- for (int i = 0; i < 80; i++)
|
|
|
- {
|
|
|
- if (detectStation())
|
|
|
- {
|
|
|
- station_detected = true;
|
|
|
- break;
|
|
|
- }
|
|
|
- rclcpp::sleep_for(100ms);
|
|
|
- }
|
|
|
-
|
|
|
- if (!station_detected)
|
|
|
- {
|
|
|
- changeState(ChargingState::UNAVAILABLE);
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 进入回充控制流程
|
|
|
- executeChargingControl();
|
|
|
- changeState(ChargingState::CHARGING);
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::stopCharging()
|
|
|
-{
|
|
|
- if (state_ == ChargingState::IDLE)
|
|
|
- {
|
|
|
- RCLCPP_WARN(this->get_logger(), "当前状态无法停止回充");
|
|
|
- return;
|
|
|
- }
|
|
|
- changeState(ChargingState::IDLE);
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::executeChargingControl()
|
|
|
-{
|
|
|
- changeState(ChargingState::NAVIGATING);
|
|
|
- last_station_detection_time_ = this->now();
|
|
|
-
|
|
|
- while (state_ == ChargingState::NAVIGATING)
|
|
|
- {
|
|
|
- // 检测基站
|
|
|
- if (detectStation())
|
|
|
- {
|
|
|
- last_station_detection_time_ = this->now();
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- auto now = this->now();
|
|
|
- auto diff = (now - last_station_detection_time_).seconds();
|
|
|
- if (diff > station_lost_timeout_)
|
|
|
- {
|
|
|
- changeState(ChargingState::UNAVAILABLE);
|
|
|
- return;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- // 检测充电电压
|
|
|
- if (detectChargingVoltage())
|
|
|
- {
|
|
|
- changeState(ChargingState::CHARGING);
|
|
|
- monitorCharging();
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- // 生成并应用控制命令
|
|
|
- auto [linear_vel, angular_vel] = generateControlCommand();
|
|
|
- applyControlCommand(linear_vel, angular_vel);
|
|
|
-
|
|
|
- rclcpp::sleep_for(std::chrono::milliseconds(int(control_interval_ * 1000)));
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::monitorCharging()
|
|
|
-{
|
|
|
- last_voltage_detection_time_ = this->now();
|
|
|
-
|
|
|
- while (state_ == ChargingState::CHARGING)
|
|
|
- {
|
|
|
- if (!detectChargingVoltage())
|
|
|
- {
|
|
|
- auto now = this->now();
|
|
|
- auto diff = (now - last_voltage_detection_time_).seconds();
|
|
|
- if (diff > station_lost_timeout_)
|
|
|
- {
|
|
|
- changeState(ChargingState::COMPLETED);
|
|
|
- return;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- last_voltage_detection_time_ = this->now();
|
|
|
- }
|
|
|
- rclcpp::sleep_for(std::chrono::milliseconds(int(control_interval_ * 1000)));
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::changeState(ChargingState new_state)
|
|
|
-{
|
|
|
- state_ = new_state;
|
|
|
- reportState(new_state);
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::reportState(ChargingState state)
|
|
|
-{
|
|
|
- std::string state_str;
|
|
|
- int other_status = 0;
|
|
|
-
|
|
|
- switch (state)
|
|
|
- {
|
|
|
- case ChargingState::IDLE:
|
|
|
- state_str = "空闲";
|
|
|
- other_status = 5; // 注意：需要根据实际情况调整
|
|
|
- break;
|
|
|
- case ChargingState::STARTING:
|
|
|
- state_str = "启动中";
|
|
|
- other_status = 1;
|
|
|
- break;
|
|
|
- case ChargingState::NAVIGATING:
|
|
|
- state_str = "回充行驶";
|
|
|
- other_status = 3;
|
|
|
- break;
|
|
|
- case ChargingState::CHARGING:
|
|
|
- state_str = "充电中";
|
|
|
- other_status = 5;
|
|
|
- break;
|
|
|
- case ChargingState::UNAVAILABLE:
|
|
|
- state_str = "回充不可用";
|
|
|
- other_status = 2;
|
|
|
- break;
|
|
|
- case ChargingState::COMPLETED:
|
|
|
- state_str = "充电结束";
|
|
|
- other_status = 5;
|
|
|
- break;
|
|
|
- }
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "状态上报: %s", state_str.c_str());
|
|
|
-}
|
|
|
-
|
|
|
-bool ChargingManager::detectChargingVoltage()
|
|
|
-{
|
|
|
- return getChargingVoltage();
|
|
|
-}
|
|
|
-
|
|
|
-std::pair<double, double> ChargingManager::generateControlCommand()
|
|
|
-{
|
|
|
- // 这里需要实现控制命令生成逻辑
|
|
|
- return {0.2, 0.0};
|
|
|
-}
|
|
|
-
|
|
|
-void ChargingManager::applyControlCommand(double linear_vel, double angular_vel)
|
|
|
-{
|
|
|
- // 应用控制命令
|
|
|
- // 发布到控制主题
|
|
|
- auto msg = geometry_msgs::msg::Twist();
|
|
|
- msg.linear.x = linear_vel;
|
|
|
- msg.angular.z = angular_vel;
|
|
|
-
|
|
|
- // 需要有一个发布者，或者通过其他方式传递控制命令
|
|
|
- // 例如: pub_ctrl_->publish(msg);
|
|
|
-}
|
|
|
-
|
|
|
-RechargerRecv::RechargerRecv() : Node("RechargerRecv")
|
|
|
-{
|
|
|
- setChargingVoltage(false);
|
|
|
-
|
|
|
- last_report_status_ = -1;
|
|
|
- last_status_ = 1;
|
|
|
- running_ = false;
|
|
|
-
|
|
|
- // 创建发布者和订阅者
|
|
|
- pub_feed_ = this->create_publisher<std_msgs::msg::Int32>("/recharge/feed", 10);
|
|
|
- client_ = this->create_client<interface::srv::SceneControls>("/feed/recharge/ctrl");
|
|
|
-
|
|
|
- getcharging_sub_ = this->create_subscription<std_msgs::msg::Int32>(
|
|
|
- "/sensor/getcharging", 10,
|
|
|
- std::bind(&RechargerRecv::onGetCharging, this, _1));
|
|
|
-
|
|
|
- sub_mode_change_ = this->create_subscription<std_msgs::msg::String>(
|
|
|
- "/scene_services/mode", 10,
|
|
|
- std::bind(&RechargerRecv::onModeChange, this, _1));
|
|
|
-
|
|
|
- // 创建定时器
|
|
|
- auto timer_callback = [this]() -> void
|
|
|
- {
|
|
|
- this->reportLoop();
|
|
|
- };
|
|
|
-
|
|
|
- auto report_timer = this->create_wall_timer(
|
|
|
- std::chrono::seconds(2), timer_callback);
|
|
|
-
|
|
|
- // 启动线程
|
|
|
- thread_ = std::thread(&RechargerRecv::reportLoop, this);
|
|
|
-}
|
|
|
-
|
|
|
-RechargerRecv::~RechargerRecv()
|
|
|
-{
|
|
|
- running_ = false;
|
|
|
- if (thread_.joinable())
|
|
|
- {
|
|
|
- thread_.join();
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void RechargerRecv::onGetCharging(const std_msgs::msg::Int32::SharedPtr msg)
|
|
|
-{
|
|
|
- int status = msg->data;
|
|
|
- bool local_detect_charging_voltage = false;
|
|
|
-
|
|
|
- if (status == 0)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "设备接触到充电区");
|
|
|
- local_detect_charging_voltage = true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- local_detect_charging_voltage = false;
|
|
|
- }
|
|
|
-
|
|
|
- setChargingVoltage(local_detect_charging_voltage);
|
|
|
-}
|
|
|
-
|
|
|
-void RechargerRecv::onModeChange(const std_msgs::msg::String::SharedPtr msg)
|
|
|
-{
|
|
|
- std::string mode = msg->data;
|
|
|
- std::string last_mode_str = last_mode;
|
|
|
-
|
|
|
- if (last_mode_str != mode)
|
|
|
- {
|
|
|
- setMode(mode); // 需要添加这个函数
|
|
|
- RCLCPP_INFO(this->get_logger(), "新模式: %s", mode.c_str());
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void RechargerRecv::reportLoop()
|
|
|
-{
|
|
|
- while (running_)
|
|
|
- {
|
|
|
- if (!isCharging())
|
|
|
- {
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- // 报告状态
|
|
|
- if (last_report_status_ != last_status_)
|
|
|
- {
|
|
|
- callService(last_status_);
|
|
|
- last_report_status_ = last_status_;
|
|
|
- }
|
|
|
-
|
|
|
- std::this_thread::sleep_for(std::chrono::seconds(2));
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void RechargerRecv::callService(int start)
|
|
|
-{
|
|
|
- // 发布到主题
|
|
|
- auto msg = std_msgs::msg::Int32();
|
|
|
- msg.data = start;
|
|
|
-
|
|
|
- RCLCPP_INFO(this->get_logger(), "发布回充反馈: %d", start);
|
|
|
- if (start != 3)
|
|
|
- {
|
|
|
- pub_feed_->publish(msg);
|
|
|
- }
|
|
|
-
|
|
|
- // 或者调用服务（如果使用服务）
|
|
|
- // auto request = std::make_shared<interface::srv::SceneControls::Request>();
|
|
|
- // request->start = start;
|
|
|
- // auto future = client_->async_send_request(request);
|
|
|
-}
|
|
|
-
|
|
|
-void RechargerRecv::changeStatus(int status)
|
|
|
-{
|
|
|
- if (last_status_ != status)
|
|
|
- {
|
|
|
- RCLCPP_INFO(this->get_logger(), "回充状态变更 %d --> %d", last_status_, status);
|
|
|
- callService(status);
|
|
|
- last_status_ = status;
|
|
|
- }
|
|
|
-}
|
xuqian