diff --git a/src/autonomy/fu/config/fu_params.yaml b/src/autonomy/fu/config/fu_params.yaml index 758cb04..8f2c476 100644 --- a/src/autonomy/fu/config/fu_params.yaml +++ b/src/autonomy/fu/config/fu_params.yaml @@ -4,7 +4,7 @@ FU: enable_obstacle_stop: True # 绕障功能 False or True - enable_obstacle_avoid: True + enable_obstacle_avoid: False # 可视化栅格 False or True enable_visualize_grid: False @@ -29,4 +29,19 @@ FU: # 右侧扩展检测区域 int right_area_extend: 2 + + # 路点到达容差(米) + waypoint_tolerance: 0.1 + + # 横向偏移量(栅格数) + lateral_offset: 2 + + # 平行前进距离(栅格数) + parallel_distance: 3 + + # 可绕过的最大障碍物宽度(栅格数) + avoid_able_width: 6 + + # 绕过所需最小空间(栅格数) + min_free_space: 6 \ No newline at end of file diff --git a/src/autonomy/fu/src/fu_node copy.cpp b/src/autonomy/fu/src/fu_node copy.cpp new file mode 100644 index 0000000..a16b9d1 --- /dev/null +++ b/src/autonomy/fu/src/fu_node copy.cpp @@ -0,0 +1,1643 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "logger/logger.h" +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/fu.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" +#include "sweeper_interfaces/msg/pl.hpp" +#include "sweeper_interfaces/msg/sub.hpp" +using namespace std; + +// 栅格值定义 +const int EMPTY = 0; // 空区域 +const int OBSTACLE = 1; // 障碍物 +const int VEHICLE = 2; // 车体 + +int located = 0; +bool detected_line = false; +float nl_adjust = 0; +float remain_adjust = -1; +int is_start = 0; + +class fu_node : public rclcpp::Node +{ + public: + fu_node(std::string name) : Node(name) + { + LOG_INFO("%s节点已经启动.", name.c_str()); + + // 声明并获取参数（默认开启遇障停车和绕障） + this->declare_parameter("enable_obstacle_stop", true); // 是否是否开启遇障停车 + this->declare_parameter("enable_obstacle_avoid", true); // 是否开启绕障 + this->declare_parameter("enable_visualize_grid", true); // 是否可视化栅格 + this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3); // 车前必停区行数 + this->declare_parameter("REAR_STOP_ZONE_ROWS", 1); // 车后必停区行数 + this->declare_parameter("LEFT_STOP_ZONE_COLS", 3); // 车左必停区列数 + this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3); // 车右必停区列数 + this->declare_parameter("front_area_extend", 7); // 前方扩展检测区域 + this->declare_parameter("left_area_extend", 2); // 左侧扩展检测区域 + this->declare_parameter("right_area_extend", 2); // 右侧扩展检测区域 + + this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_); + this->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid_); + this->get_parameter("enable_visualize_grid", enable_visualize_grid_); + this->get_parameter("FRONT_STOP_ZONE_ROWS", FRONT_STOP_ZONE_ROWS_); + this->get_parameter("REAR_STOP_ZONE_ROWS", REAR_STOP_ZONE_ROWS_); + this->get_parameter("LEFT_STOP_ZONE_COLS", LEFT_STOP_ZONE_COLS_); + this->get_parameter("RIGHT_STOP_ZONE_COLS", RIGHT_STOP_ZONE_COLS_); + this->get_parameter("front_area_extend", front_area_extend_); + this->get_parameter("left_area_extend", left_area_extend_); + this->get_parameter("right_area_extend", right_area_extend_); + + // 创建参数回调，支持动态修改 + parameter_callback_handle_ = + this->add_on_set_parameters_callback(std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); + + // 创建订阅器 + subscription_grid = this->create_subscription( + "grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1)); + + msg_subscribe_pl = this->create_subscription( + "pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1)); + + // 发布控制指令 + pub_mc = this->create_publisher("auto_mc_ctrl", 10); + + // 创建定时器，100ms为周期 + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&fu_node::timer_callback, this)); + + // 避障相关参数初始化 + avoid_counter_ = 0; + avoid_state_ = AVOID_NONE; + avoid_direction_ = 0; + + // 初始化车体位置 + vehicle_min_x_ = -1; + vehicle_max_x_ = -1; + vehicle_min_y_ = -1; + vehicle_max_y_ = -1; + + // 使用统一的时钟源 + node_clock_ = this->get_clock(); + + // 打印所有参数值（添加到构造函数末尾） + LOG_INFO( + "\n\n---------- FU节点参数配置 ----------\n 遇障停车功能: %s\n 绕障功能: %s\n 可视化栅格: %s\n " + "车前必停区行数: %d\n 车后必停区行数: %d\n 车左必停区列数: %d\n 车右必停区列数: %d\n 前方扩展检测区域: " + "%d\n 左侧扩展检测区域: %d\n 右侧扩展检测区域: %d\n--------------------------------------------\n", + (enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"), + (enable_visualize_grid_ ? "是" : "否"), FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_, LEFT_STOP_ZONE_COLS_, + RIGHT_STOP_ZONE_COLS_, front_area_extend_, left_area_extend_, right_area_extend_); + } + + private: + rclcpp::Clock::SharedPtr node_clock_; + + enum AvoidState + { + AVOID_NONE, // 无绕障 + AVOID_WAITING, // 等待决策 + AVOID_LATERAL_SHIFT, // 阶段1: 横向偏移 + AVOID_PARALLEL_MOVE, // 阶段2: 平行前进 + AVOID_RETURN_TO_PATH // 阶段3: 回归轨迹 + }; + + // 避障状态管理 + struct AvoidanceContext + { + AvoidState state = AVOID_NONE; + int direction = 0; // -1=左, 1=右 + int counter = 0; + + bool start_time_set = false; + bool last_state_change_time_set = false; + rclcpp::Time start_time; + rclcpp::Time last_state_change_time; + + // 路径记录 + std::vector angle_history; + int original_angle = 0; // 原始角度 + + // 阶段1参数 + int lateral_shift_distance = 0; // 需要横向偏移的距离(栅格数) + int lateral_shift_achieved = 0; // 已完成的横向偏移 + + // 阶段2参数 + double parallel_start_obstacle_x = -1; // 开始平行前进时障碍物的X坐标 + + // 阶段3参数 + bool obstacle_passed = false; // 障碍物是否已通过 + + // 动态参数 + int max_avoid_angle = 25; + double min_lateral_time = 2.0; // 最小横向偏移时间 + double min_parallel_time = 1.5; // 最小平行前进时间 + + void reset() + { + state = AVOID_NONE; + direction = 0; + counter = 0; + angle_history.clear(); + original_angle = 0; + start_time_set = false; + last_state_change_time_set = false; + lateral_shift_distance = 0; + lateral_shift_achieved = 0; + parallel_start_obstacle_x = -1; + obstacle_passed = false; + } + + void addAngleHistory(int angle) + { + angle_history.push_back(angle); + if (angle_history.size() > 20) + { + angle_history.erase(angle_history.begin()); + } + } + + double getElapsedTime(const rclcpp::Time& start, const rclcpp::Time& current) const + { + try + { + auto duration = current - start; + return duration.seconds(); + } + catch (const std::runtime_error& e) + { + return 0.0; + } + } + }; + + AvoidanceContext avoid_ctx_; + + // 障碍物分析结果结构体 + struct ObstacleAnalysis + { + bool has_front_critical = false; // 前方必停区 + bool has_front_area = false; // 前方扩展区域 + bool has_left_critical = false; // 左方必停区 + bool has_right_critical = false; // 右方必停区 + + int obstacle_distance = -1; // 最近障碍物距离 + int obstacle_width = 0; // 障碍物宽度 + int left_edge = -1, right_edge = -1; // 障碍物边缘 + int free_space_left = 0; // 左侧可用空间 + int free_space_right = 0; // 右侧可用空间 + + // 动态安全距离计算 + int calculateSafeDistance(int speed) const + { + // 基于速度计算安全距离：速度越快，需要的距离越远 + return std::max(3, speed / 400 + 2); // 最少3格，速度1500时为5格 + } + + // 评估绕障可行性 + bool canAvoid() const + { + return obstacle_width > 0 && obstacle_width < 10 && (free_space_left >= 3 || free_space_right >= 3); + } + + // 评估是否需要紧急制动 + bool needEmergencyBrake(int speed) const + { + if (!has_front_critical) return false; + int safe_distance = calculateSafeDistance(speed); + return obstacle_distance <= safe_distance; + } + }; + + ObstacleAnalysis obstacle_analysis_; + + // 新增：控制参数 + bool enable_obstacle_stop_; // 遇障停车使能 + bool enable_obstacle_avoid_; // 绕障功能使能 + bool enable_visualize_grid_; // 可视化栅格使能 + + // 必停区范围参数 + int FRONT_STOP_ZONE_ROWS_; // 车前必停区行数 + int REAR_STOP_ZONE_ROWS_; // 车后必停区行数 + int LEFT_STOP_ZONE_COLS_; // 车左必停区列数 + int RIGHT_STOP_ZONE_COLS_; // 车右必停区列数 + + // 分析前方更宽区域的障碍物 + int front_area_extend_; // 前方扩展检测区域 + int left_area_extend_; // 左侧扩展检测区域 + int right_area_extend_; // 右侧扩展检测区域 + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + + // 接收PL消息的参数 + double x = 0.0; // 负数表示左，正数表示右 + double y = 0.0; // 负数表示后，正数表示前 + int speed = 0; // 从PL消息获取的速度 + int reliability = 0; // RTK信号质量 1行 0不行 + + // 发布消息的参数 + int publish_speed = 0; // 转速占空比 0-1500 + int publish_angle = 0; // [-40.0,40.0]，负数表示左转，正数表示右转 + + // 车体矩形边界（栅格坐标） + int vehicle_min_x_; // 车体最小行坐标（顶部） + int vehicle_max_x_; // 车体最大行坐标（底部） + int vehicle_min_y_; // 车体最小列坐标（左侧） + int vehicle_max_y_; // 车体最大列坐标（右侧） + + // 必停区定义（栅格坐标）- 动态计算 + struct SafetyZone + { + int min_x, max_x; // X轴范围（行） + int min_y, max_y; // Y轴范围（列） + }; + + SafetyZone front_stop_zone_; // 前方必停区 + SafetyZone rear_stop_zone_; // 后方必停区 + SafetyZone left_stop_zone_; // 左方必停区 + SafetyZone right_stop_zone_; // 右方必停区 + + AvoidState avoid_state_; // 当前避障状态 + int avoid_counter_; // 避障计数器 + int avoid_direction_; // 避障方向（-1=左，1=右） + int avoid_angle_; // 避障角度 + rclcpp::Time last_avoid_time_; // 上次避障时间 + + // 障碍物检测结果 + bool obstacle_in_front_ = false; // 前方必停区有障碍物 + bool obstacle_in_rear_ = false; // 后方必停区有障碍物 + bool obstacle_in_left_ = false; // 左方必停区有障碍物 + bool obstacle_in_right_ = false; // 右方必停区有障碍物 + bool obstacle_in_front_area_ = false; // 前方区域有障碍物 + int obstacle_left_edge_ = -1; // 障碍物左边缘 + int obstacle_right_edge_ = -1; // 障碍物右边缘 + int obstacle_max_x_ = 100; // 障碍物距离车体最近的X坐标 + int free_space_left_ = 0; // 左侧可用空间 + int free_space_right_ = 0; // 右侧可用空间 + + // 声明订阅者和发布者 + rclcpp::Subscription::SharedPtr subscription_grid; + rclcpp::Subscription::SharedPtr msg_subscribe_pl; + rclcpp::Subscription::SharedPtr subscription_; + + rclcpp::Publisher::SharedPtr pub_mc; + rclcpp::TimerBase::SharedPtr timer_; + + // 参数动态更新回调 + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector& parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto& param : parameters) + { + if (param.get_name() == "enable_obstacle_stop") + { + enable_obstacle_stop_ = param.as_bool(); + LOG_INFO("遇障停车功能 %s", enable_obstacle_stop_ ? "开启" : "关闭"); + } + else if (param.get_name() == "enable_obstacle_avoid") + { + enable_obstacle_avoid_ = param.as_bool(); + LOG_INFO("绕障功能 %s", enable_obstacle_avoid_ ? "开启" : "关闭"); + // 关闭绕障时重置避障状态 + if (!enable_obstacle_avoid_) + { + avoid_ctx_.state = AVOID_NONE; + avoid_counter_ = 0; + } + } + } + return result; + } + + // 根据目标点x,y计算方向盘角度 + float calculate_the_angle(double x, double y) + { + // 处理x为0的情况 + if (x == 0) + { + return 90.0f; + } + + // 计算目标点的角度 + float angle_f = atan2(y, x) * 180 / M_PI; + + // 计算方向盘角度 + float steering_angle; + if (angle_f <= 90.0f) + { + steering_angle = 90.0f - angle_f; + } + else + { + steering_angle = -(angle_f - 90.0f); + } + + steering_angle = std::clamp(steering_angle, -50.0f, 50.0f); + return steering_angle; + } + + // 查找车体在栅格中的矩形边界 + void findVehicleRectangle(const std::vector>& grid) + { + // 重置车体边界 + vehicle_min_x_ = -1; + vehicle_max_x_ = -1; + vehicle_min_y_ = -1; + vehicle_max_y_ = -1; + + // 遍历栅格查找所有车体栅格 + for (size_t i = 0; i < grid.size(); ++i) + { + for (size_t j = 0; j < grid[i].size(); ++j) + { + if (grid[i][j] == VEHICLE) + { + // 更新最小和最大行坐标 + if (vehicle_min_x_ == -1 || static_cast(i) < vehicle_min_x_) vehicle_min_x_ = i; + if (vehicle_max_x_ == -1 || static_cast(i) > vehicle_max_x_) vehicle_max_x_ = i; + + // 更新最小和最大列坐标 + if (vehicle_min_y_ == -1 || static_cast(j) < vehicle_min_y_) vehicle_min_y_ = j; + if (vehicle_max_y_ == -1 || static_cast(j) > vehicle_max_y_) vehicle_max_y_ = j; + } + } + } + + // 如果找到车体，计算必停区域 + if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && vehicle_min_y_ != -1 && vehicle_max_y_ != -1) + { + calculateSafetyZones(grid.size(), grid[0].size()); + } + } + + // 基于矩形车体计算必停区域 + void calculateSafetyZones(int grid_height, int grid_width) + { + // 前方必停区：车体前方FRONT_STOP_ZONE_ROWS行，与车体同宽 + front_stop_zone_.min_x = max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_); + front_stop_zone_.max_x = vehicle_min_x_ - 1; // 紧接车体前方 + front_stop_zone_.min_y = vehicle_min_y_; + front_stop_zone_.max_y = vehicle_max_y_; + + // 后方必停区：车体后方REAR_STOP_ZONE_ROWS行，与车体同宽 + rear_stop_zone_.min_x = vehicle_max_x_ + 1; // 紧接车体后方 + rear_stop_zone_.max_x = min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_); + rear_stop_zone_.min_y = vehicle_min_y_; + rear_stop_zone_.max_y = vehicle_max_y_; + + // 左方必停区：车体左侧LEFT_STOP_ZONE_COLS列，与车体同高 + left_stop_zone_.min_x = vehicle_min_x_; + left_stop_zone_.max_x = vehicle_max_x_; + left_stop_zone_.min_y = max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_); + left_stop_zone_.max_y = vehicle_min_y_ - 1; // 紧接车体左侧 + + // 右方必停区：车体右侧RIGHT_STOP_ZONE_COLS列，与车体同高 + right_stop_zone_.min_x = vehicle_min_x_; + right_stop_zone_.max_x = vehicle_max_x_; + right_stop_zone_.min_y = vehicle_max_y_ + 1; // 紧接车体右侧 + right_stop_zone_.max_y = min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_); + } + + void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) + { + // 检查消息是否包含维度信息 + if (msg->layout.dim.size() != 2) + { + LOG_ERROR("Received grid message with invalid dimensions!"); + return; + } + + // 获取栅格尺寸 + size_t height = msg->layout.dim[0].size; + size_t width = msg->layout.dim[1].size; + + // 检查数据长度是否匹配 + if (msg->data.size() != height * width) + { + LOG_ERROR("Grid data size mismatch! Expected %zu, got %zu", height * width, msg->data.size()); + return; + } + + // 重构二维数组 + std::vector> grid(height, std::vector(width)); + + // 填充数据 + for (size_t i = 0; i < height; ++i) + { + for (size_t j = 0; j < width; ++j) + { + grid[i][j] = msg->data[i * width + j]; + } + } + + // 查找车体矩形边界并计算必停区 + findVehicleRectangle(grid); + + // 障碍物检测与分析 + analyzeObstacles(grid); + + // 可视化栅格 + if (enable_visualize_grid_) visualizeGridInTerminal(grid); + + cacheGridData(grid); // 缓存栅格数据 + } + + std::vector> cached_grid_; + bool grid_data_valid_ = false; + + void cacheGridData(const std::vector>& grid) + { + cached_grid_ = grid; + grid_data_valid_ = true; + } + + // 分析障碍物分布 + void analyzeObstacles(const std::vector>& grid) + { + // 重置检测结果 + obstacle_in_front_ = false; + obstacle_in_rear_ = false; + obstacle_in_left_ = false; + obstacle_in_right_ = false; + obstacle_in_front_area_ = false; + obstacle_left_edge_ = -1; + obstacle_right_edge_ = -1; + free_space_left_ = 0; + free_space_right_ = 0; + obstacle_max_x_ = 100; + + // 如果未找到车体位置，不进行检测 + if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1 || vehicle_min_y_ == -1 || vehicle_max_y_ == -1) return; + + // 检查前方必停区 + bool found_front_obstacle = false; + for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x && !found_front_obstacle; --i) + { + for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_front_ = true; + obstacle_max_x_ = vehicle_min_x_ - i; + found_front_obstacle = true; + break; + } + } + } + + // 检查其他必停区（后方、左方、右方） + checkOtherStopZones(grid); + + // 分析前方扩展区域 + analyzeFrontExtendedArea(grid); + + // 如果没有检测到前方障碍物，设置为-1 + if (obstacle_max_x_ == 100 && !obstacle_in_front_ && !obstacle_in_front_area_) + { + obstacle_max_x_ = -1; + } + } + + // 检查其他必停区 + void checkOtherStopZones(const std::vector>& grid) + { + // 检查后方必停区 + for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x && !obstacle_in_rear_; ++i) + { + for (int j = rear_stop_zone_.min_y; j <= rear_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_rear_ = true; + break; + } + } + } + + // 检查左方必停区 + for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x && !obstacle_in_left_; ++i) + { + for (int j = left_stop_zone_.min_y; j <= left_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_left_ = true; + break; + } + } + } + + // 检查右方必停区 + for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x && !obstacle_in_right_; ++i) + { + for (int j = right_stop_zone_.min_y; j <= right_stop_zone_.max_y; ++j) + { + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && + grid[i][j] == OBSTACLE) + { + obstacle_in_right_ = true; + break; + } + } + } + } + + // 分析前方扩展区域 - 优化策略：只检测车辆宽度范围的前方 + void analyzeFrontExtendedArea(const std::vector>& grid) + { + size_t front_area_min_x = static_cast(max(0, vehicle_min_x_ - front_area_extend_)); + size_t front_area_max_x = static_cast(vehicle_min_x_ - 1); + // 按照你的优化：只检测车辆宽度范围，避免不必要的绕障 + size_t front_area_min_y = static_cast(max(0, vehicle_min_y_)); + size_t front_area_max_y = static_cast(min(static_cast(grid[0].size() - 1), vehicle_max_y_)); + + // 重置边缘检测 + int leftmost_obstacle = -1; + int rightmost_obstacle = -1; + + // 遍历前方核心区域（车辆宽度范围） + for (size_t i = front_area_min_x; i <= front_area_max_x; ++i) + { + for (size_t j = front_area_min_y; j <= front_area_max_y; ++j) + { + if (grid[i][j] == OBSTACLE) + { + obstacle_in_front_area_ = true; + + // 更新最左和最右障碍物位置 + if (leftmost_obstacle == -1 || static_cast(j) < leftmost_obstacle) + leftmost_obstacle = static_cast(j); + if (rightmost_obstacle == -1 || static_cast(j) > rightmost_obstacle) + rightmost_obstacle = static_cast(j); + + // 更新最近障碍物X坐标 + int distance = vehicle_min_x_ - static_cast(i); + if (distance < obstacle_max_x_) + { + obstacle_max_x_ = distance; + } + } + } + } + + // 重新计算可用空间 - 基于更大的绕障检测范围 + calculateAvoidanceSpace(grid, leftmost_obstacle, rightmost_obstacle); + } + + // 计算绕障可用空间 - 在更大范围内分析左右通道 + void calculateAvoidanceSpace(const std::vector>& grid, int core_left_obstacle, + int core_right_obstacle) + { + if (obstacle_in_front_area_) + { + obstacle_left_edge_ = core_left_obstacle; + obstacle_right_edge_ = core_right_obstacle; + + // 为了绕障决策，需要检查更宽的左右区域 + size_t avoidance_min_x = static_cast(max(0, vehicle_min_x_ - front_area_extend_)); + size_t avoidance_max_x = static_cast(vehicle_min_x_ - 1); + + // 左侧绕障空间检查：从 车辆左边界 到 障碍物左边界 + int left_check_start = max(0, vehicle_min_y_ - left_area_extend_); + int left_check_end = obstacle_left_edge_ - 1; + free_space_left_ = + calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, left_check_start, left_check_end); + + // 右侧绕障空间检查：从 障碍物右边界 到 车辆右边界 + int right_check_start = obstacle_right_edge_ + 1; + int right_check_end = min(static_cast(grid[0].size() - 1), vehicle_max_y_ + right_area_extend_); + free_space_right_ = + calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, right_check_start, right_check_end); + } + else + { + // 没有前方障碍物时，设置较大的可用空间 左右扩展+车宽 + free_space_left_ = left_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1); + free_space_right_ = right_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1); + obstacle_left_edge_ = -1; + obstacle_right_edge_ = -1; + } + } + + // 计算指定方向的可用空间 + int calculateSpaceInDirection(const std::vector>& grid, size_t min_x, size_t max_x, + int check_start_y, int check_end_y) + { + if (check_start_y > check_end_y) return 0; + + int available_space = 0; + + // 从起始位置开始，逐列检查直到遇到障碍物 + for (int j = check_start_y; j <= check_end_y; ++j) + { + if (j < 0 || j >= static_cast(grid[0].size())) break; + + bool column_clear = true; + // 检查该列在检测范围内是否有障碍物 + for (size_t i = min_x; i <= max_x; ++i) + { + if (i >= grid.size()) break; + if (grid[i][j] == OBSTACLE) + { + column_clear = false; + break; + } + } + + if (column_clear) + { + available_space++; + } + else + { + break; // 遇到障碍物，停止计算 + } + } + + return available_space; + } + + void visualizeGridInTerminal(const std::vector>& grid) + { + std::stringstream ss; + + ss << " --------------------障碍物矩阵-------------------- " << std::endl; + + // 打印顶部边框 + ss << " " << std::string(grid[0].size(), '-') << std::endl; + + // 打印列号（仅0-9） + ss << " "; + for (size_t i = 0; i < grid[0].size(); i++) + { + ss << (i % 10); + } + ss << std::endl; + + // 打印栅格内容（行号 + 栅格） + for (size_t i = 0; i < grid.size(); i++) + { + // 行号显示（两位数，右对齐） + ss << (i < 10 ? " " : "") << i << "|"; + + // 栅格内容 + for (size_t j = 0; j < grid[i].size(); j++) + { + // 检查当前位置是否在某个必停区内 + bool in_front_zone = (i >= static_cast(front_stop_zone_.min_x) && + i <= static_cast(front_stop_zone_.max_x) && + j >= static_cast(front_stop_zone_.min_y) && + j <= static_cast(front_stop_zone_.max_y)); + + bool in_left_zone = (i >= static_cast(left_stop_zone_.min_x) && + i <= static_cast(left_stop_zone_.max_x) && + j >= static_cast(left_stop_zone_.min_y) && + j <= static_cast(left_stop_zone_.max_y)); + + bool in_right_zone = (i >= static_cast(right_stop_zone_.min_x) && + i <= static_cast(right_stop_zone_.max_x) && + j >= static_cast(right_stop_zone_.min_y) && + j <= static_cast(right_stop_zone_.max_y)); + + // 根据栅格值和必停区状态选择显示符号 + if (grid[i][j] == OBSTACLE) + ss << "@"; + else if (grid[i][j] == VEHICLE) + ss << "V"; + else if (in_front_zone) + ss << "F"; // 前方必停区 + else if (in_left_zone) + ss << "L"; // 左方必停区 + else if (in_right_zone) + ss << "R"; // 右方必停区 + else + ss << "."; + } + + ss << "|" << std::endl; + } + + // 打印底部边框 + ss << " " << std::string(grid[0].size(), '-') << std::endl; + + LOG_INFO("%s", ss.str().c_str()); + } + + void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg) + { + x = msg->x; + y = msg->y; + speed = (int)msg->speed; // 获取PL消息中的速度 + reliability = msg->reliability; + located = msg->enter_range; + is_start = msg->is_start; + + LOG_INFO("目标点位置： x -- %.2f , y -- %.2f", x, y); + } + + void timer_callback() + { + if (speed == 0) + { + publish_speed = 0; + publish_angle = 0; + LOG_INFO("PL速度为0，已停车"); + } + else if (is_start) + { + // 障碍物检测与避障决策 + applyObstacleAvoidance(); + + // 发布控制指令 + sweeper_interfaces::msg::McCtrl message; + message.sweep = true; + message.brake = 0; // 0开;1关 + message.gear = 2; // 0空挡;1后退;2前进 + + if (reliability == 1) + message.rpm = publish_speed; + else + { + message.rpm = 0; + LOG_ERROR("rtk信号差,停车！！！"); + } + + message.angle = publish_angle; // 负数表示左转，正数表示右转 + message.angle_speed = 800; + + pub_mc->publish(message); + + // 改进的状态显示 + std::string state_str; + switch (avoid_ctx_.state) + { + case AVOID_NONE: + state_str = "正常行驶"; + break; + case AVOID_WAITING: + state_str = "等待决策"; + break; + case AVOID_LATERAL_SHIFT: + state_str = "横向偏移"; + break; + case AVOID_PARALLEL_MOVE: + state_str = "平行前进"; + break; + case AVOID_RETURN_TO_PATH: + state_str = "回归轨迹"; + break; + default: + state_str = "未知状态"; + } + + LOG_INFO("控制: 速度=%d, 角度=%d, 状态=%s, 前方必停区%s障碍物, 前方扩展区%s障碍物", publish_speed, + publish_angle, state_str.c_str(), (obstacle_in_front_ ? "有" : "无"), + (obstacle_in_front_area_ ? "有" : "无")); + } + else + { + // 未启动状态，停车 + publish_speed = 0; + publish_angle = 0; + } + } + + // 避障决策与控制 + void applyObstacleAvoidance() + { + // 计算基础目标角度 + float target_angle = calculate_the_angle(x, y); + + // 初始化控制参数 + publish_speed = 800; + publish_angle = static_cast(target_angle); + + // 安全检查：未检测到车体位置 + if (vehicle_min_x_ == -1) + { + publish_speed = 0; + publish_angle = 0; + avoid_ctx_.reset(); + LOG_WARN("未检测到车体位置，安全停车"); + return; + } + + // 更新障碍物分析 + updateObstacleAnalysis(); + + // 执行分层避障策略 + if (!executeEmergencyStop()) + { + if (!executeTurnProtection()) + { + executeSmartAvoidance(); + } + } + + // 记录角度历史 + avoid_ctx_.addAngleHistory(publish_angle); + } + + void updateObstacleAnalysis() + { + obstacle_analysis_ = ObstacleAnalysis{}; // 重置 + + obstacle_analysis_.has_front_critical = obstacle_in_front_; + obstacle_analysis_.has_front_area = obstacle_in_front_area_; + obstacle_analysis_.has_left_critical = obstacle_in_left_; + obstacle_analysis_.has_right_critical = obstacle_in_right_; + + obstacle_analysis_.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; + obstacle_analysis_.left_edge = obstacle_left_edge_; + obstacle_analysis_.right_edge = obstacle_right_edge_; + LOG_INFO("障碍物左侧=%d，障碍物右侧=%d", obstacle_analysis_.left_edge, obstacle_analysis_.right_edge); + obstacle_analysis_.free_space_left = free_space_left_; + obstacle_analysis_.free_space_right = free_space_right_; + + if (obstacle_left_edge_ != -1 && obstacle_right_edge_ != -1) + { + obstacle_analysis_.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1; + } + } + + // 第一优先级：紧急停车策略 + bool executeEmergencyStop() + { + if (!enable_obstacle_stop_) return false; + + // 前方必停区有障碍物，立即停车 + if (obstacle_analysis_.has_front_critical) + { + publish_speed = 0; + publish_angle = 0; + avoid_ctx_.state = AVOID_WAITING; + avoid_ctx_.counter++; + + // 每2秒打印一次状态 + if (avoid_ctx_.counter % 20 == 0) + { + LOG_WARN("紧急停车：前方必停区有障碍物，距离=%d格", obstacle_analysis_.obstacle_distance); + } + return true; + } + + // 动态紧急制动：基于速度和距离 + if (obstacle_analysis_.needEmergencyBrake(publish_speed)) + { + publish_speed = 0; + publish_angle = 0; + avoid_ctx_.state = AVOID_WAITING; + LOG_WARN("动态紧急制动：障碍物距离%d格，速度%d", obstacle_analysis_.obstacle_distance, publish_speed); + return true; + } + + return false; + } + + // 第二优先级：转向保护策略 + bool executeTurnProtection() + { + if (!enable_obstacle_stop_) return false; + + // 检查转向方向的安全性 + if (publish_angle < -10 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + publish_angle = 0; + LOG_WARN("转向保护：左转时左侧有障碍物"); + return true; + } + + if (publish_angle > 10 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + publish_angle = 0; + LOG_WARN("转向保护：右转时右侧有障碍物"); + return true; + } + + return false; + } + + // 处理无法绕行的障碍物 + void handleUnpassableObstacle() + { + int safe_distance = obstacle_analysis_.calculateSafeDistance(publish_speed); + + if (obstacle_analysis_.obstacle_distance <= safe_distance) + { + // 太近，必须停车 + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + avoid_ctx_.counter++; + + if (avoid_ctx_.counter % 30 == 0) + { // 每3秒打印一次 + LOG_INFO("障碍物无法绕行(宽度=%d格)，等待通过...", obstacle_analysis_.obstacle_width); + } + } + else + { + // 距离足够，可以减速接近 + publish_speed = std::max(600, publish_speed / 2); + LOG_INFO("障碍物较宽，减速接近中，距离=%d格", obstacle_analysis_.obstacle_distance); + } + } + + // 启动绕障 + void initiateAvoidance() + { + auto current_time = node_clock_->now(); + + // 选择最优绕障方向 + int direction = selectOptimalDirection(); + + // 计算需要的横向偏移距离 + int required_shift = calculateRequiredLateralShift(direction); + + avoid_ctx_.state = AVOID_LATERAL_SHIFT; + avoid_ctx_.direction = direction; + avoid_ctx_.counter = 0; + avoid_ctx_.start_time = current_time; + avoid_ctx_.start_time_set = true; + avoid_ctx_.last_state_change_time = current_time; + avoid_ctx_.last_state_change_time_set = true; + avoid_ctx_.original_angle = static_cast(calculate_the_angle(x, y)); + avoid_ctx_.lateral_shift_distance = required_shift; + avoid_ctx_.lateral_shift_achieved = 0; + avoid_ctx_.parallel_start_obstacle_x = -1; + avoid_ctx_.obstacle_passed = false; + + LOG_INFO("启动绕障: 方向=%s, 需要偏移=%d格, 障碍物宽度=%d, 左侧空间=%d, 右侧空间=%d", + (direction == -1) ? "左" : "右", required_shift, obstacle_analysis_.obstacle_width, + obstacle_analysis_.free_space_left, obstacle_analysis_.free_space_right); + } + + // 方向选择算法 + int selectOptimalDirection() + { + int left_score = 0; + int right_score = 0; + + // 1. 可用空间评分(主要因素) + left_score += obstacle_analysis_.free_space_left * 20; + right_score += obstacle_analysis_.free_space_right * 20; + + // 2. 安全性评分(关键因素) + if (obstacle_analysis_.has_left_critical) left_score -= 1000; + if (obstacle_analysis_.has_right_critical) right_score -= 1000; + + // 3. 障碍物位置分析 - 基于车辆边界而非中心 + if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) + { + // 计算障碍物与车辆的相对位置 + int vehicle_width = vehicle_max_y_ - vehicle_min_y_ + 1; + + // 左侧可用空间 = 车辆左边界到障碍物左边界的距离 + int left_clearance = obstacle_analysis_.left_edge - vehicle_min_y_; + // 右侧可用空间 = 障碍物右边界到车辆右边界的距离 + int right_clearance = vehicle_max_y_ - obstacle_analysis_.right_edge; + + LOG_INFO("位置分析: 左侧间隙=%d, 右侧间隙=%d, 车宽=%d", left_clearance, right_clearance, vehicle_width); + + // 基于实际间隙大小评分 + if (left_clearance > right_clearance + 2) + { + left_score += 25; // 左侧明显更宽松 + } + else if (right_clearance > left_clearance + 2) + { + right_score += 25; // 右侧明显更宽松 + } + + // 如果障碍物严重偏向一侧,反方向绕行 + if (obstacle_analysis_.right_edge < vehicle_min_y_) + { + // 障碍物完全在车辆左侧,应该右绕 + right_score += 30; + LOG_INFO("障碍物在左侧,倾向右绕"); + } + else if (obstacle_analysis_.left_edge > vehicle_max_y_) + { + // 障碍物完全在车辆右侧,应该左绕 + left_score += 30; + LOG_INFO("障碍物在右侧,倾向左绕"); + } + } + + // 4. 目标方向偏好 - 降低权重,避免过度影响 + float target_angle = calculate_the_angle(x, y); + if (target_angle < -15) // 阈值从10提高到15 + left_score += 15; // 权重从20降低到15 + if (target_angle > 15) right_score += 15; + + // 5. 历史趋势稳定性(避免频繁切换) - 权重提高 + if (!avoid_ctx_.angle_history.empty() && avoid_ctx_.angle_history.size() >= 5) // 从3提高到5 + { + int recent_avg = 0; + int count = std::min(5, static_cast(avoid_ctx_.angle_history.size())); + + for (int i = avoid_ctx_.angle_history.size() - count; i < static_cast(avoid_ctx_.angle_history.size()); + i++) + { + recent_avg += avoid_ctx_.angle_history[i]; + } + recent_avg /= count; + + if (recent_avg < -8) // 阈值从5提高到8 + left_score += 15; // 权重从10提高到15 + if (recent_avg > 8) right_score += 15; + } + + int selected_direction = (left_score > right_score) ? -1 : 1; + + LOG_INFO("方向选择 - 左评分:%d 右评分:%d → %s (左空间:%d 右空间:%d)", left_score, right_score, + (selected_direction == -1) ? "左绕" : "右绕", obstacle_analysis_.free_space_left, + obstacle_analysis_.free_space_right); + + return selected_direction; + } + + // 计算需要的横向偏移距离 + int calculateRequiredLateralShift(int direction) + { + if (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.right_edge == -1) + { + return 3; // 默认偏移3格 + } + + int safety_margin = 2; // 安全余量 + int required_shift = 0; + + if (direction == -1) // 左绕 + { + // 车体右边界需要绕过障碍物右边界 + required_shift = vehicle_max_y_ - obstacle_analysis_.right_edge + safety_margin; + } + else // 右绕 + { + // 车体左边界需要绕过障碍物左边界 + required_shift = obstacle_analysis_.left_edge - vehicle_min_y_ + safety_margin; + } + + // 限制在合理范围内 + required_shift = std::max(2, std::min(5, required_shift)); + + return required_shift; + } + + // 执行主动绕障 - 三阶段状态机 + void executeSmartAvoidance() + { + if (!enable_obstacle_avoid_ || !obstacle_analysis_.has_front_area) + { + // 无障碍物或绕障功能关闭 + if (avoid_ctx_.state != AVOID_NONE) + { + avoid_ctx_.reset(); + LOG_INFO("前方无障碍物，重置绕障状态"); + } + return; + } + + // 评估是否需要绕障 + if (!obstacle_analysis_.canAvoid()) + { + LOG_INFO("障碍物无法绕行，减速靠近"); + handleUnpassableObstacle(); + return; + } + + // 三阶段状态机 + switch (avoid_ctx_.state) + { + case AVOID_NONE: + case AVOID_WAITING: + initiateAvoidance(); + break; + + case AVOID_LATERAL_SHIFT: + executeLateralShift(); + break; + + case AVOID_PARALLEL_MOVE: + executeParallelMove(); + break; + + case AVOID_RETURN_TO_PATH: + executeReturnToPath(); + break; + } + } + + // 阶段1: 横向偏移 + void executeLateralShift() + { + auto current_time = node_clock_->now(); + double shift_time = 0.0; + + if (avoid_ctx_.last_state_change_time_set) + { + shift_time = avoid_ctx_.getElapsedTime(avoid_ctx_.last_state_change_time, current_time); + } + + // 速度控制:横向偏移时限速 + publish_speed = 600; + + // 角度控制:保持较大角度进行横向偏移 + int shift_angle = calculateLateralShiftAngle(); + publish_angle = avoid_ctx_.direction * shift_angle; + + // 安全检查 + if (checkAvoidanceSafety()) + { + return; + } + + // 估算已完成的横向偏移 + avoid_ctx_.lateral_shift_achieved = static_cast(shift_time * 1.5); + + // 完成条件 + bool time_sufficient = (shift_time >= avoid_ctx_.min_lateral_time); + bool distance_sufficient = (avoid_ctx_.lateral_shift_achieved >= avoid_ctx_.lateral_shift_distance); + bool lateral_clearance_achieved = checkLateralClearance(); + + // 验证是否真正远离障碍物 + bool safe_distance_achieved = verifySafeDistance(); + + // 所有条件都满足才进入下一阶段 + bool shift_complete = + time_sufficient && distance_sufficient && lateral_clearance_achieved && safe_distance_achieved; + + if (shift_complete) + { + avoid_ctx_.state = AVOID_PARALLEL_MOVE; + avoid_ctx_.last_state_change_time = current_time; + avoid_ctx_.last_state_change_time_set = true; + avoid_ctx_.parallel_start_obstacle_x = obstacle_max_x_; + + LOG_INFO("阶段1完成: 横向偏移完成(%.1fs, %d格), 进入阶段2: 平行前进", shift_time, + avoid_ctx_.lateral_shift_achieved); + } + else + { + if (avoid_ctx_.counter % 10 == 0) + { + LOG_INFO("阶段1进行中: 时间%.1fs/%s, 距离%d/%d格, 间隙%s, 安全距离%s", shift_time, + time_sufficient ? "✓" : "✗", avoid_ctx_.lateral_shift_achieved, + avoid_ctx_.lateral_shift_distance, lateral_clearance_achieved ? "✓" : "✗", + safe_distance_achieved ? "✓" : "✗"); + } + } + + avoid_ctx_.counter++; + } + + // 验证安全距离 + bool verifySafeDistance() + { + // 检查当前是否已经没有前方障碍物警告 + if (obstacle_analysis_.has_front_critical || obstacle_analysis_.has_front_area) + { + LOG_DEBUG("前方仍有障碍区域"); + return false; + } + + // 检查绕障方向的侧方是否有障碍物 + if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) + { + LOG_DEBUG("左绕时左侧仍有障碍"); + return false; + } + + if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) + { + LOG_DEBUG("右绕时右侧仍有障碍"); + return false; + } + + // 检查障碍物是否已经移到侧后方 + if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) + { + if (avoid_ctx_.direction == -1) // 左绕,障碍物应该在右侧 + { + if (obstacle_analysis_.left_edge <= vehicle_max_y_) + { + LOG_DEBUG("左绕时障碍物左边界%d仍在车右边界%d范围内", obstacle_analysis_.left_edge, vehicle_max_y_); + return false; + } + } + else // 右绕,障碍物应该在左侧 + { + if (obstacle_analysis_.right_edge >= vehicle_min_y_) + { + LOG_DEBUG("右绕时障碍物右边界%d仍在车左边界%d范围内", obstacle_analysis_.right_edge, + vehicle_min_y_); + return false; + } + } + } + + return true; + } + + // 计算横向偏移角度 + int calculateLateralShiftAngle() + { + // 基于可用空间和需求偏移距离计算角度 + int available_space = + (avoid_ctx_.direction == -1) ? obstacle_analysis_.free_space_left : obstacle_analysis_.free_space_right; + + int base_angle = 20; // 基础角度 + + if (available_space >= 4) + { + base_angle = 25; // 空间充足，可以大角度 + } + else if (available_space >= 3) + { + base_angle = 20; + } + else + { + base_angle = 15; // 空间紧张，小角度谨慎 + } + + // 距离修正：障碍物越近，角度越小 + if (obstacle_analysis_.obstacle_distance > 0 && obstacle_analysis_.obstacle_distance < 3) + { + base_angle = static_cast(base_angle * 0.7); + } + + return std::max(10, std::min(avoid_ctx_.max_avoid_angle, base_angle)); + } + + // 检查横向间隙是否足够 + bool checkLateralClearance() + { + // if (!grid_data_valid_ || cached_grid_.empty()) return false; + + // 最小偏移阈值 + const int MIN_SHIFT_THRESHOLD = 2; // 2格 + if (avoid_ctx_.lateral_shift_achieved < MIN_SHIFT_THRESHOLD) + { + LOG_DEBUG("横移距离不足: %d < %d", avoid_ctx_.lateral_shift_achieved, MIN_SHIFT_THRESHOLD); + return false; + } + + // 安全余量 + const int SAFE_MARGIN = 1; // 1格 + // ---------------------------- + // 1) 基于障碍物边界判断横向绕出 + // ---------------------------- + if (avoid_ctx_.direction == -1) // 左绕 + { + int required_position = obstacle_analysis_.left_edge - SAFE_MARGIN; + if (vehicle_max_y_ > required_position) // 右边界 + { + LOG_DEBUG("左绕未完成: 车右边界%d > 目标位置%d", vehicle_max_y_, required_position); + return false; + } + } + else // 右绕 + { + int required_position = obstacle_analysis_.right_edge + SAFE_MARGIN; + if (vehicle_min_y_ < required_position) + { + LOG_DEBUG("右绕未完成: 车左边界%d < 目标位置%d", vehicle_min_y_, required_position); + return false; + } + } + + // ---------------------------- + // 2) 检查前方更深的区域是否清空 + // ---------------------------- + // const int CHECK_DEPTH = 10; // 10行 + // for (int depth = 1; depth <= CHECK_DEPTH; depth++) + // { + // int check_row = vehicle_min_x_ - depth; + // if (check_row < 0) break; + + // // 检查整个车宽范围 + // for (int col = vehicle_min_y_; col <= vehicle_max_y_; col++) + // { + // if (col < 0 || col >= static_cast(cached_grid_[0].size())) continue; + + // if (cached_grid_[check_row][col] == OBSTACLE) + // { + // LOG_DEBUG("前方第%d行仍有障碍,位置[%d,%d]", depth, check_row, col); + // return false; + // } + // } + // } + + // ---------------------------- + // 3) 检查侧方是否真正清空 + // ---------------------------- + // if (avoid_ctx_.direction == -1) // 左绕,检查右侧 + // { + // for (int depth = 1; depth <= 5; depth++) + // { + // int check_row = vehicle_min_x_ - depth; + // if (check_row < 0) break; + + // // 检查车辆右侧若干列 + // for (int offset = 1; offset <= 3; offset++) + // { + // int check_col = vehicle_max_y_ + offset; + // if (check_col >= static_cast(cached_grid_[0].size())) break; + + // if (cached_grid_[check_row][check_col] == OBSTACLE) + // { + // LOG_DEBUG("右侧仍有障碍[%d,%d],左绕未完全清空", check_row, check_col); + // return false; + // } + // } + // } + // } + // else // 右绕,检查左侧 + // { + // for (int depth = 1; depth <= 5; depth++) + // { + // int check_row = vehicle_min_x_ - depth; + // if (check_row < 0) break; + + // // 检查车辆左侧若干列 + // for (int offset = 1; offset <= 3; offset++) + // { + // int check_col = vehicle_min_y_ - offset; + // if (check_col < 0) break; + + // if (cached_grid_[check_row][check_col] == OBSTACLE) + // { + // LOG_DEBUG("左侧仍有障碍[%d,%d],右绕未完全清空", check_row, check_col); + // return false; + // } + // } + // } + // } + + LOG_INFO("横向间隙检查通过: 方向=%s, 已移动=%d格", (avoid_ctx_.direction == -1) ? "左" : "右", + avoid_ctx_.lateral_shift_achieved); + + return true; + } + + // 阶段2: 平行前进 + void executeParallelMove() + { + auto current_time = node_clock_->now(); + double parallel_time = 0.0; + + if (avoid_ctx_.last_state_change_time_set) + { + parallel_time = avoid_ctx_.getElapsedTime(avoid_ctx_.last_state_change_time, current_time); + } + + // 速度控制：平行前进时可以适当提速 + publish_speed = 700; + + // 角度控制：回正，保持直行 + publish_angle = 0; + + // 安全检查 + if (checkParallelMoveSafety()) + { + return; // 检测到危险 + } + + // 判断障碍物是否已经与车头持平或更后 + bool obstacle_aligned_or_behind = checkObstaclePosition(); + + // 最小平行前进时间保护 + bool min_time_met = (parallel_time >= avoid_ctx_.min_parallel_time); + + if (obstacle_aligned_or_behind && min_time_met) + { + avoid_ctx_.state = AVOID_RETURN_TO_PATH; + avoid_ctx_.last_state_change_time = current_time; + avoid_ctx_.last_state_change_time_set = true; + avoid_ctx_.obstacle_passed = true; + + LOG_INFO("阶段2完成: 障碍物已在车身后方(%.1fs), 进入阶段3: 回归轨迹", parallel_time); + } + else + { + if (avoid_ctx_.counter % 10 == 0) + { + LOG_INFO("阶段2进行中: 平行前进 %.1fs, 障碍物位置检查=%s", parallel_time, + obstacle_aligned_or_behind ? "已通过" : "未通过"); + } + } + + avoid_ctx_.counter++; + } + + // 检查障碍物位置（是否与车头持平或在车身后方） + bool checkObstaclePosition() + { + // 方法1: 检查前方核心区域是否清空 + bool front_core_clear = !obstacle_analysis_.has_front_area && !obstacle_analysis_.has_front_critical; + + // 方法2: 检查障碍物是否不再在绕障侧的检测范围内 + bool lateral_clear = false; + if (avoid_ctx_.direction == -1) // 左绕 + { + // 检查右侧区域是否清空（障碍物应该在左后方） + lateral_clear = !obstacle_analysis_.has_right_critical && + (obstacle_analysis_.right_edge == -1 || obstacle_analysis_.right_edge < vehicle_min_y_); + } + else // 右绕 + { + // 检查左侧区域是否清空（障碍物应该在右后方） + lateral_clear = !obstacle_analysis_.has_left_critical && + (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.left_edge > vehicle_max_y_); + } + + // 综合判断 + bool obstacle_passed = front_core_clear && lateral_clear; + + if (avoid_ctx_.counter % 10 == 0) + { + LOG_DEBUG("障碍物位置检查: 前方清空=%s, 侧方清空=%s => %s", front_core_clear ? "是" : "否", + lateral_clear ? "是" : "否", obstacle_passed ? "已通过" : "未通过"); + } + + return obstacle_passed; + } + + // 平行前进安全检查 + bool checkParallelMoveSafety() + { + // 检查前方是否出现新障碍物 + if (obstacle_analysis_.has_front_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + LOG_ERROR("平行前进时前方出现障碍物！"); + return true; + } + + // 检查绕障侧是否有障碍物（防止剐蹭） + if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + LOG_ERROR("平行前进时左侧出现障碍物！"); + return true; + } + + if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + LOG_ERROR("平行前进时右侧出现障碍物！"); + return true; + } + + return false; + } + + // 阶段3: 回归原轨迹 + void executeReturnToPath() + { + auto current_time = node_clock_->now(); + double return_time = 0.0; + + if (avoid_ctx_.last_state_change_time_set) + { + return_time = avoid_ctx_.getElapsedTime(avoid_ctx_.last_state_change_time, current_time); + } + + // 计算目标角度 + float target_angle = calculate_the_angle(x, y); + int target_angle_int = static_cast(target_angle); + + // 1. 转向保护 - 有障碍就停 + if (checkReturnSafety(target_angle_int)) + { + return; + } + + // 2. 缓慢回正 + int angle_diff = target_angle_int - publish_angle; + const int ANGLE_TOLERANCE = 3; // ±3度容差 + const int RETURN_RATE = 1; // 固定回正速率 + + if (std::abs(angle_diff) <= ANGLE_TOLERANCE) + { + // 回正完成 + publish_angle = target_angle_int; + publish_speed = 800; + avoid_ctx_.reset(); + + LOG_INFO("✓ 回正完成(%.1fs), 绕障结束! 目标角度=%d°", return_time, target_angle_int); + } + else + { + // 缓慢调整角度 + if (angle_diff > 0) + { + publish_angle += std::min(RETURN_RATE, angle_diff); + } + else + { + publish_angle -= std::min(RETURN_RATE, -angle_diff); + } + + publish_speed = 700; // 回正时保持低速 + + if (avoid_ctx_.counter % 10 == 0) + { + LOG_INFO("回正中: 当前角度=%d°, 目标角度=%d°, 差值=%d°", publish_angle, target_angle_int, angle_diff); + } + } + + avoid_ctx_.counter++; + } + + // 回归安全检查 + bool checkReturnSafety(int target_angle) + { + // 检查目标转向方向是否有障碍 + if (target_angle < -10 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + publish_angle = 0; + LOG_WARN("回正暂停: 需要左转但左侧有障碍"); + return true; + } + + if (target_angle > 10 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + publish_angle = 0; + LOG_WARN("回正暂停: 需要右转但右侧有障碍"); + return true; + } + + // 检查前方是否有新障碍 + if (obstacle_analysis_.has_front_critical) + { + publish_speed = 0; + publish_angle = 0; + LOG_WARN("回正暂停: 前方出现障碍"); + avoid_ctx_.state = AVOID_WAITING; + return true; + } + + return false; + } + + // 绕障安全检查 + bool checkAvoidanceSafety() + { + // 检查绕障方向是否出现新障碍物 + if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + LOG_ERROR("左绕过程中左侧出现障碍物！"); + return true; + } + + if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + LOG_ERROR("右绕过程中右侧出现障碍物！"); + return true; + } + + return false; + } +}; + +int main(int argc, char** argv) +{ + // 初始化日志系统 + logger::Logger::Init("fu", "./nodes_log"); + + rclcpp::init(argc, argv); + auto node = std::make_shared("fu_node"); + rclcpp::spin(node); + rclcpp::shutdown(); + // 关闭日志系统 + logger::Logger::Shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index ead41fb..f5978cb 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -16,39 +17,237 @@ #include "sweeper_interfaces/msg/fu.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp" #include "sweeper_interfaces/msg/pl.hpp" +#include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/sub.hpp" + using namespace std; +using namespace std::chrono; -// 栅格值定义 -const int EMPTY = 0; // 空区域 -const int OBSTACLE = 1; // 障碍物 -const int VEHICLE = 2; // 车体 +// ================== RTK全局变量 ================== +system_clock::time_point last_rtk_time; +const duration rtk_timeout = duration(0.5); -int located = 0; -bool detected_line = false; -float nl_adjust = 0; -float remain_adjust = -1; +struct struct_rtk +{ + double lon; + double lat; + double direction; + double speed; + int reliability; +}; +struct_rtk g_rtk; + +// ================== GPS转相对坐标的常数 ================== +#define Lat_Origin 32.045062 +#define Lon_Origin 118.845200366 +#define deg_rad (0.01745329252) +#define R_LATI (6378137) +#define R_LONT (5407872) + +// ================== 枚举定义 ================== +const int EMPTY = 0; +const int OBSTACLE = 1; +const int VEHICLE = 2; int is_start = 0; +// ================== 路点结构体 ================== +struct Waypoint +{ + double x, y; + int speed; + double lon, lat; // 路点的绝对坐标（GPS） + Waypoint() : x(0), y(0), speed(300), lon(0), lat(0) {} + Waypoint(double px, double py, int s = 300, double plon = 0, double plat = 0) + : x(px), y(py), speed(s), lon(plon), lat(plat) + { + } +}; + +// ================== 障碍物分析结构体 ================== +struct ObstacleAnalysis +{ + bool has_front_critical = false; + bool has_front_area = false; + bool has_left_critical = false; + bool has_right_critical = false; + + int obstacle_distance = -1; + int obstacle_width = 0; + int left_edge = -1, right_edge = -1; + int free_space_left = 0; + int free_space_right = 0; + + int calculateSafeDistance(int speed) const { return std::max(3, speed / 400 + 2); } + bool needEmergencyBrake(int speed) const + { + if (!has_front_critical) return false; + return obstacle_distance <= calculateSafeDistance(speed); + } +}; + +// ================== GPS坐标转换函数 ================== +// 计算两点间距离(米) +double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) +{ + double x, y, length; + x = (R_LONT) * (lonti2 - lonti1) * deg_rad; + y = (R_LATI) * (lati2 - lati1) * deg_rad; + length = sqrt(x * x + y * y); + return length; +} + +// GPS转相对坐标(米) +// Cur_navAngle: 当前航向角(度) +// Cur_lonti, Cur_lati: 当前位置GPS坐标 +// Dest_lonti, Dest_lati: 目标位置GPS坐标 +// x_diff, y_diff: 输出的相对坐标(米) +int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, + double* x_diff, double* y_diff) +{ + double Navi_rad, x, y; + float k1, k2, k3, k4; + Navi_rad = Cur_navAngle * deg_rad; + + x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT; + y = (Dest_lati - Cur_lati) * deg_rad * R_LATI; + + k1 = cos(Navi_rad); + k2 = (-1) * sin(Navi_rad); + k3 = (-1) * k2; + k4 = k1; + + *x_diff = x * k1 + y * k2; + *y_diff = x * k3 + y * k4; + + return 0; +} + +// ================== 状态接口定义 ================== +class StateMachine; + +class AvoidanceState +{ + public: + virtual ~AvoidanceState() = default; + virtual void enter(StateMachine& sm) {} + virtual void execute(StateMachine& sm) = 0; + virtual void exit(StateMachine& sm) {} +}; + +// ================== 状态机核心类 ================== +class StateMachine +{ + public: + enum State + { + NORMAL, + WAITING, + LATERAL_SHIFT, + PARALLEL_MOVE + }; + + StateMachine() { current_state_ = NORMAL; } + + void setState(State new_state) + { + if (new_state != current_state_) + { + exit(current_state_); + current_state_ = new_state; + enter(new_state); + } + } + + void execute() {} + + State getCurrentState() const { return current_state_; } + + std::string getStateString() const + { + switch (current_state_) + { + case NORMAL: + return "正常行驶"; + case WAITING: + return "等待决策"; + case LATERAL_SHIFT: + return "横向偏移"; + case PARALLEL_MOVE: + return "平行前进"; + default: + return "未知状态"; + } + } + + ObstacleAnalysis obstacle_analysis; + int publish_speed = 0; + float publish_angle = 0.0f; + int counter = 0; + int direction = 0; + std::vector angle_history; + rclcpp::Time start_time; + bool start_time_set = false; + + private: + State current_state_; + + void enter(State state) + { + switch (state) + { + case NORMAL: + counter = 0; + direction = 0; + break; + case WAITING: + publish_speed = 0; + publish_angle = 0.0f; + counter = 0; + break; + case LATERAL_SHIFT: + publish_speed = 300; + counter = 0; + break; + case PARALLEL_MOVE: + publish_speed = 300; + counter = 0; + break; + default: + break; + } + } + + void exit(State state) {} +}; + +// ================== FU节点类 ================== class fu_node : public rclcpp::Node { public: - fu_node(std::string name) : Node(name) + fu_node(std::string name) : Node(name), state_machine_(std::make_unique()) { LOG_INFO("%s节点已经启动.", name.c_str()); + // 初始化最后接收时间为启动时间（避免初始状态直接超时） + last_rtk_time = system_clock::now(); - // 声明并获取参数（默认开启遇障停车和绕障） - this->declare_parameter("enable_obstacle_stop", true); // 是否是否开启遇障停车 - this->declare_parameter("enable_obstacle_avoid", true); // 是否开启绕障 - this->declare_parameter("enable_visualize_grid", true); // 是否可视化栅格 - this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3); // 车前必停区行数 - this->declare_parameter("REAR_STOP_ZONE_ROWS", 1); // 车后必停区行数 - this->declare_parameter("LEFT_STOP_ZONE_COLS", 3); // 车左必停区列数 - this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3); // 车右必停区列数 - this->declare_parameter("front_area_extend", 7); // 前方扩展检测区域 - this->declare_parameter("left_area_extend", 2); // 左侧扩展检测区域 - this->declare_parameter("right_area_extend", 2); // 右侧扩展检测区域 + // ======== 参数声明 ======== + this->declare_parameter("enable_obstacle_stop", true); + this->declare_parameter("enable_obstacle_avoid", true); + this->declare_parameter("enable_visualize_grid", true); + this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3); + this->declare_parameter("REAR_STOP_ZONE_ROWS", 1); + this->declare_parameter("LEFT_STOP_ZONE_COLS", 3); + this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3); + this->declare_parameter("front_area_extend", 7); + this->declare_parameter("left_area_extend", 2); + this->declare_parameter("right_area_extend", 2); + this->declare_parameter("waypoint_tolerance", 2.0); + this->declare_parameter("lateral_offset", 2); + this->declare_parameter("parallel_distance", 3); + this->declare_parameter("avoid_able_width", 6); + this->declare_parameter("min_free_space", 6); + // ======== 参数读取 ======== this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_); this->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid_); this->get_parameter("enable_visualize_grid", enable_visualize_grid_); @@ -59,1584 +258,763 @@ class fu_node : public rclcpp::Node this->get_parameter("front_area_extend", front_area_extend_); this->get_parameter("left_area_extend", left_area_extend_); this->get_parameter("right_area_extend", right_area_extend_); + this->get_parameter("waypoint_tolerance", waypoint_tolerance_); + this->get_parameter("lateral_offset", lateral_offset_); + this->get_parameter("parallel_distance", parallel_distance_); + this->get_parameter("avoid_able_width", avoid_able_width_); + this->get_parameter("min_free_space", min_free_space_); - // 创建参数回调，支持动态修改 parameter_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); - // 创建订阅器 + // ======== 订阅者初始化 ======== subscription_grid = this->create_subscription( "grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1)); - msg_subscribe_pl = this->create_subscription( "pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1)); + msg_subscribe_rtk = this->create_subscription( + "rtk_message", 10, std::bind(&fu_node::msg_callback_rtk, this, std::placeholders::_1)); - // 发布控制指令 + // ======== 发布者初始化 ======== pub_mc = this->create_publisher("auto_mc_ctrl", 10); - // 创建定时器，100ms为周期 + // ======== 定时器初始化 ======== timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&fu_node::timer_callback, this)); - // 避障相关参数初始化 - avoid_counter_ = 0; - avoid_state_ = AVOID_NONE; - avoid_direction_ = 0; - - // 初始化车体位置 + // ======== 初始化 ======== + avoiding_obstacle_ = false; + current_waypoint_idx_ = 0; vehicle_min_x_ = -1; vehicle_max_x_ = -1; vehicle_min_y_ = -1; vehicle_max_y_ = -1; - - // 使用统一的时钟源 node_clock_ = this->get_clock(); - // 打印所有参数值（添加到构造函数末尾） + front_stop_zone_ = {0, 0, 0, 0}; + rear_stop_zone_ = {0, 0, 0, 0}; + left_stop_zone_ = {0, 0, 0, 0}; + right_stop_zone_ = {0, 0, 0, 0}; + LOG_INFO( - "\n\n---------- FU节点参数配置 ----------\n 遇障停车功能: %s\n 绕障功能: %s\n 可视化栅格: %s\n " - "车前必停区行数: %d\n 车后必停区行数: %d\n 车左必停区列数: %d\n 车右必停区列数: %d\n 前方扩展检测区域: " - "%d\n 左侧扩展检测区域: %d\n 右侧扩展检测区域: %d\n--------------------------------------------\n", + "\n---------- FU节点参数配置 ----------\n 遇障停车: %s\n 绕障: %s\n 栅格可视化: %s\n" + " 路点容差: %.2f米\n 横向偏移: %d栅格\n 平行距离: %d栅格\n" + " 可绕宽度: %d栅格\n 最小空闲: %d栅格\n--------------------------------------------\n", (enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"), - (enable_visualize_grid_ ? "是" : "否"), FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_, LEFT_STOP_ZONE_COLS_, - RIGHT_STOP_ZONE_COLS_, front_area_extend_, left_area_extend_, right_area_extend_); + (enable_visualize_grid_ ? "是" : "否"), waypoint_tolerance_, lateral_offset_, parallel_distance_, + avoid_able_width_, min_free_space_); } private: + std::unique_ptr state_machine_; rclcpp::Clock::SharedPtr node_clock_; - enum AvoidState - { - AVOID_NONE, // 无绕障 - AVOID_WAITING, // 等待决策 - AVOID_LATERAL_SHIFT, // 阶段1: 横向偏移 - AVOID_PARALLEL_MOVE, // 阶段2: 平行前进 - AVOID_RETURN_TO_PATH // 阶段3: 回归轨迹 - }; + // ======== 配置参数 ======== + bool enable_obstacle_stop_, enable_obstacle_avoid_, enable_visualize_grid_; + int FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_; + int LEFT_STOP_ZONE_COLS_, RIGHT_STOP_ZONE_COLS_; + int front_area_extend_, left_area_extend_, right_area_extend_; + double waypoint_tolerance_; + int lateral_offset_, parallel_distance_, avoid_able_width_, min_free_space_; - // 避障状态管理 - struct AvoidanceContext - { - AvoidState state = AVOID_NONE; - int direction = 0; // -1=左, 1=右 - int counter = 0; - - bool start_time_set = false; - bool last_state_change_time_set = false; - rclcpp::Time start_time; - rclcpp::Time last_state_change_time; - - // 路径记录 - std::vector angle_history; - int original_angle = 0; // 原始角度 - - // 阶段1参数 - int lateral_shift_distance = 0; // 需要横向偏移的距离(栅格数) - int lateral_shift_achieved = 0; // 已完成的横向偏移 - - // 阶段2参数 - double parallel_start_obstacle_x = -1; // 开始平行前进时障碍物的X坐标 - - // 阶段3参数 - bool obstacle_passed = false; // 障碍物是否已通过 - - // 动态参数 - int max_avoid_angle = 25; - double min_lateral_time = 2.0; // 最小横向偏移时间 - double min_parallel_time = 1.5; // 最小平行前进时间 - - void reset() - { - state = AVOID_NONE; - direction = 0; - counter = 0; - angle_history.clear(); - original_angle = 0; - start_time_set = false; - last_state_change_time_set = false; - lateral_shift_distance = 0; - lateral_shift_achieved = 0; - parallel_start_obstacle_x = -1; - obstacle_passed = false; - } - - void addAngleHistory(int angle) - { - angle_history.push_back(angle); - if (angle_history.size() > 20) - { - angle_history.erase(angle_history.begin()); - } - } - - double getElapsedTime(const rclcpp::Time& start, const rclcpp::Time& current) const - { - try - { - auto duration = current - start; - return duration.seconds(); - } - catch (const std::runtime_error& e) - { - return 0.0; - } - } - }; - - AvoidanceContext avoid_ctx_; - - // 障碍物分析结果结构体 - struct ObstacleAnalysis - { - bool has_front_critical = false; // 前方必停区 - bool has_front_area = false; // 前方扩展区域 - bool has_left_critical = false; // 左方必停区 - bool has_right_critical = false; // 右方必停区 - - int obstacle_distance = -1; // 最近障碍物距离 - int obstacle_width = 0; // 障碍物宽度 - int left_edge = -1, right_edge = -1; // 障碍物边缘 - int free_space_left = 0; // 左侧可用空间 - int free_space_right = 0; // 右侧可用空间 - - // 动态安全距离计算 - int calculateSafeDistance(int speed) const - { - // 基于速度计算安全距离：速度越快，需要的距离越远 - return std::max(3, speed / 400 + 2); // 最少3格，速度1500时为5格 - } - - // 评估绕障可行性 - bool canAvoid() const - { - return obstacle_width > 0 && obstacle_width < 10 && (free_space_left >= 3 || free_space_right >= 3); - } - - // 评估是否需要紧急制动 - bool needEmergencyBrake(int speed) const - { - if (!has_front_critical) return false; - int safe_distance = calculateSafeDistance(speed); - return obstacle_distance <= safe_distance; - } - }; - - ObstacleAnalysis obstacle_analysis_; - - // 新增：控制参数 - bool enable_obstacle_stop_; // 遇障停车使能 - bool enable_obstacle_avoid_; // 绕障功能使能 - bool enable_visualize_grid_; // 可视化栅格使能 - - // 必停区范围参数 - int FRONT_STOP_ZONE_ROWS_; // 车前必停区行数 - int REAR_STOP_ZONE_ROWS_; // 车后必停区行数 - int LEFT_STOP_ZONE_COLS_; // 车左必停区列数 - int RIGHT_STOP_ZONE_COLS_; // 车右必停区列数 - - // 分析前方更宽区域的障碍物 - int front_area_extend_; // 前方扩展检测区域 - int left_area_extend_; // 左侧扩展检测区域 - int right_area_extend_; // 右侧扩展检测区域 rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - // 接收PL消息的参数 - double x = 0.0; - double y = 0.0; - int speed = 0; // 从PL消息获取的速度 - int reliability = 0; // RTK信号质量 1行 0不行 + // ======== 规划层消息缓存 ======== + double pl_x_ = 0.0, pl_y_ = 0.0; + int pl_speed_ = 0; - // 发布消息的参数 - int publish_speed = 0; // 转速占空比 0-1500 - int publish_angle = 0; // [-40.0,40.0]，负数表示左转，正数表示右转 + // ======== 栅格检测结果 ======== + bool obstacle_in_front_ = false, obstacle_in_rear_ = false; + bool obstacle_in_left_ = false, obstacle_in_right_ = false; + bool obstacle_in_front_area_ = false; + int obstacle_left_edge_ = -1, obstacle_right_edge_ = -1; + int obstacle_max_x_ = 100; + int free_space_left_ = 0, free_space_right_ = 0; - // 车体矩形边界（栅格坐标） - int vehicle_min_x_; // 车体最小行坐标（顶部） - int vehicle_max_x_; // 车体最大行坐标（底部） - int vehicle_min_y_; // 车体最小列坐标（左侧） - int vehicle_max_y_; // 车体最大列坐标（右侧） + // ======== 自车位置 ======== + int vehicle_min_x_, vehicle_max_x_, vehicle_min_y_, vehicle_max_y_; - // 必停区定义（栅格坐标）- 动态计算 + // ======== 安全区结构体 ======== struct SafetyZone { - int min_x, max_x; // X轴范围（行） - int min_y, max_y; // Y轴范围（列） + int min_x, max_x, min_y, max_y; }; + SafetyZone front_stop_zone_, rear_stop_zone_, left_stop_zone_, right_stop_zone_; - SafetyZone front_stop_zone_; // 前方必停区 - SafetyZone rear_stop_zone_; // 后方必停区 - SafetyZone left_stop_zone_; // 左方必停区 - SafetyZone right_stop_zone_; // 右方必停区 + // ======== 避障相关 ======== + bool avoiding_obstacle_; + std::vector avoidance_waypoints_; + int current_waypoint_idx_; + std::vector> cached_grid_; + bool grid_data_valid_ = false; - AvoidState avoid_state_; // 当前避障状态 - int avoid_counter_; // 避障计数器 - int avoid_direction_; // 避障方向（-1=左，1=右） - int avoid_angle_; // 避障角度 - rclcpp::Time last_avoid_time_; // 上次避障时间 - - // 障碍物检测结果 - bool obstacle_in_front_ = false; // 前方必停区有障碍物 - bool obstacle_in_rear_ = false; // 后方必停区有障碍物 - bool obstacle_in_left_ = false; // 左方必停区有障碍物 - bool obstacle_in_right_ = false; // 右方必停区有障碍物 - bool obstacle_in_front_area_ = false; // 前方区域有障碍物 - int obstacle_left_edge_ = -1; // 障碍物左边缘 - int obstacle_right_edge_ = -1; // 障碍物右边缘 - int obstacle_max_x_ = 100; // 障碍物距离车体最近的X坐标 - int free_space_left_ = 0; // 左侧可用空间 - int free_space_right_ = 0; // 右侧可用空间 - - // 声明订阅者和发布者 + // ======== 发布者和订阅者 ======== + rclcpp::Publisher::SharedPtr pub_mc; rclcpp::Subscription::SharedPtr subscription_grid; rclcpp::Subscription::SharedPtr msg_subscribe_pl; - rclcpp::Subscription::SharedPtr subscription_; - - rclcpp::Publisher::SharedPtr pub_mc; + rclcpp::Subscription::SharedPtr msg_subscribe_rtk; rclcpp::TimerBase::SharedPtr timer_; - // 参数动态更新回调 + // ======== 参数回调 ======== rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector& parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (const auto& param : parameters) { if (param.get_name() == "enable_obstacle_stop") { enable_obstacle_stop_ = param.as_bool(); - LOG_INFO("遇障停车功能 %s", enable_obstacle_stop_ ? "开启" : "关闭"); + LOG_INFO("[参数更新] 遇障停车: %s", enable_obstacle_stop_ ? "开启" : "关闭"); } else if (param.get_name() == "enable_obstacle_avoid") { enable_obstacle_avoid_ = param.as_bool(); - LOG_INFO("绕障功能 %s", enable_obstacle_avoid_ ? "开启" : "关闭"); - // 关闭绕障时重置避障状态 + LOG_INFO("[参数更新] 自主绕障: %s", enable_obstacle_avoid_ ? "开启" : "关闭"); if (!enable_obstacle_avoid_) { - avoid_ctx_.state = AVOID_NONE; - avoid_counter_ = 0; + state_machine_->setState(StateMachine::NORMAL); + avoiding_obstacle_ = false; + avoidance_waypoints_.clear(); } } + else if (param.get_name() == "waypoint_tolerance") + waypoint_tolerance_ = param.as_double(); + else if (param.get_name() == "lateral_offset") + lateral_offset_ = param.as_int(); + else if (param.get_name() == "parallel_distance") + parallel_distance_ = param.as_int(); + else if (param.get_name() == "avoid_able_width") + avoid_able_width_ = param.as_int(); + else if (param.get_name() == "min_free_space") + min_free_space_ = param.as_int(); } return result; } - // 根据目标点x,y计算方向盘角度 - float calculate_the_angle(double x, double y) + // ======== RTK回调 ======== + void msg_callback_rtk(const sweeper_interfaces::msg::Rtk::SharedPtr msg) { - // 处理x为0的情况 - if (x == 0) - { - return 90.0f; - } + last_rtk_time = system_clock::now(); - // 计算目标点的角度 - float angle_f = atan2(y, x) * 180 / M_PI; - - // 计算方向盘角度 - float steering_angle; - if (angle_f <= 90.0f) - { - steering_angle = 90.0f - angle_f; - } - else - { - steering_angle = -(angle_f - 90.0f); - } - - steering_angle = std::clamp(steering_angle, -50.0f, 50.0f); - return steering_angle; + g_rtk.reliability = (msg->p_quality == 4 && msg->h_quality == 4) ? 1 : 0; + g_rtk.lat = msg->lat; + g_rtk.lon = msg->lon; + g_rtk.direction = msg->head; } - // 查找车体在栅格中的矩形边界 + // ======== 角度计算 ======== + float calculate_the_angle(double target_x, double target_y) + { + if (target_x == 0) return 90.0f; + float angle_f = atan2(target_y, target_x) * 180 / M_PI; + float steering_angle = (angle_f <= 90.0f) ? 90.0f - angle_f : -(angle_f - 90.0f); + // 计算阶段就限制角度范围 + return std::clamp(steering_angle, -50.0f, 50.0f); + } + + // ======== 自车位置检测 ======== void findVehicleRectangle(const std::vector>& grid) { - // 重置车体边界 - vehicle_min_x_ = -1; - vehicle_max_x_ = -1; - vehicle_min_y_ = -1; - vehicle_max_y_ = -1; - - // 遍历栅格查找所有车体栅格 + vehicle_min_x_ = vehicle_max_x_ = vehicle_min_y_ = vehicle_max_y_ = -1; for (size_t i = 0; i < grid.size(); ++i) { for (size_t j = 0; j < grid[i].size(); ++j) { if (grid[i][j] == VEHICLE) { - // 更新最小和最大行坐标 - if (vehicle_min_x_ == -1 || static_cast(i) < vehicle_min_x_) vehicle_min_x_ = i; - if (vehicle_max_x_ == -1 || static_cast(i) > vehicle_max_x_) vehicle_max_x_ = i; - - // 更新最小和最大列坐标 - if (vehicle_min_y_ == -1 || static_cast(j) < vehicle_min_y_) vehicle_min_y_ = j; - if (vehicle_max_y_ == -1 || static_cast(j) > vehicle_max_y_) vehicle_max_y_ = j; + if (vehicle_min_x_ == -1 || (int)i < vehicle_min_x_) vehicle_min_x_ = i; + if (vehicle_max_x_ == -1 || (int)i > vehicle_max_x_) vehicle_max_x_ = i; + if (vehicle_min_y_ == -1 || (int)j < vehicle_min_y_) vehicle_min_y_ = j; + if (vehicle_max_y_ == -1 || (int)j > vehicle_max_y_) vehicle_max_y_ = j; } } } - - // 如果找到车体，计算必停区域 if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && vehicle_min_y_ != -1 && vehicle_max_y_ != -1) { calculateSafetyZones(grid.size(), grid[0].size()); } } - // 基于矩形车体计算必停区域 + // ======== 安全区计算 ======== void calculateSafetyZones(int grid_height, int grid_width) { - // 前方必停区：车体前方FRONT_STOP_ZONE_ROWS行，与车体同宽 - front_stop_zone_.min_x = max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_); - front_stop_zone_.max_x = vehicle_min_x_ - 1; // 紧接车体前方 - front_stop_zone_.min_y = vehicle_min_y_; - front_stop_zone_.max_y = vehicle_max_y_; - - // 后方必停区：车体后方REAR_STOP_ZONE_ROWS行，与车体同宽 - rear_stop_zone_.min_x = vehicle_max_x_ + 1; // 紧接车体后方 - rear_stop_zone_.max_x = min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_); - rear_stop_zone_.min_y = vehicle_min_y_; - rear_stop_zone_.max_y = vehicle_max_y_; - - // 左方必停区：车体左侧LEFT_STOP_ZONE_COLS列，与车体同高 - left_stop_zone_.min_x = vehicle_min_x_; - left_stop_zone_.max_x = vehicle_max_x_; - left_stop_zone_.min_y = max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_); - left_stop_zone_.max_y = vehicle_min_y_ - 1; // 紧接车体左侧 - - // 右方必停区：车体右侧RIGHT_STOP_ZONE_COLS列，与车体同高 - right_stop_zone_.min_x = vehicle_min_x_; - right_stop_zone_.max_x = vehicle_max_x_; - right_stop_zone_.min_y = vehicle_max_y_ + 1; // 紧接车体右侧 - right_stop_zone_.max_y = min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_); + front_stop_zone_ = {max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_), vehicle_min_x_ - 1, vehicle_min_y_, + vehicle_max_y_}; + rear_stop_zone_ = {vehicle_max_x_ + 1, min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_), + vehicle_min_y_, vehicle_max_y_}; + left_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_), + vehicle_min_y_ - 1}; + right_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, vehicle_max_y_ + 1, + min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_)}; } + // ======== 栅格回调 ======== void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) { - // 检查消息是否包含维度信息 if (msg->layout.dim.size() != 2) { - LOG_ERROR("Received grid message with invalid dimensions!"); + LOG_ERROR("栅格地图消息维度无效！"); return; } - // 获取栅格尺寸 size_t height = msg->layout.dim[0].size; size_t width = msg->layout.dim[1].size; - // 检查数据长度是否匹配 if (msg->data.size() != height * width) { - LOG_ERROR("Grid data size mismatch! Expected %zu, got %zu", height * width, msg->data.size()); + LOG_ERROR("栅格地图数据长度不匹配！期望%zu，实际%zu", height * width, msg->data.size()); return; } - // 重构二维数组 std::vector> grid(height, std::vector(width)); - - // 填充数据 for (size_t i = 0; i < height; ++i) - { - for (size_t j = 0; j < width; ++j) - { - grid[i][j] = msg->data[i * width + j]; - } - } + for (size_t j = 0; j < width; ++j) grid[i][j] = msg->data[i * width + j]; - // 查找车体矩形边界并计算必停区 findVehicleRectangle(grid); - - // 障碍物检测与分析 analyzeObstacles(grid); - - // 可视化栅格 if (enable_visualize_grid_) visualizeGridInTerminal(grid); - - cacheGridData(grid); // 缓存栅格数据 + cacheGridData(grid); } - std::vector> cached_grid_; - bool grid_data_valid_ = false; - + // ======== 栅格缓存 ======== void cacheGridData(const std::vector>& grid) { cached_grid_ = grid; grid_data_valid_ = true; } - // 分析障碍物分布 + // ======== 障碍物分析 ======== void analyzeObstacles(const std::vector>& grid) { - // 重置检测结果 - obstacle_in_front_ = false; - obstacle_in_rear_ = false; - obstacle_in_left_ = false; - obstacle_in_right_ = false; + obstacle_in_front_ = obstacle_in_rear_ = obstacle_in_left_ = obstacle_in_right_ = false; obstacle_in_front_area_ = false; - obstacle_left_edge_ = -1; - obstacle_right_edge_ = -1; - free_space_left_ = 0; - free_space_right_ = 0; + obstacle_left_edge_ = obstacle_right_edge_ = -1; + free_space_left_ = free_space_right_ = 0; obstacle_max_x_ = 100; + if (vehicle_min_x_ == -1) return; - // 如果未找到车体位置，不进行检测 - if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1 || vehicle_min_y_ == -1 || vehicle_max_y_ == -1) return; - - // 检查前方必停区 + // 检测前方必停区障碍物 bool found_front_obstacle = false; for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x && !found_front_obstacle; --i) - { for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j) - { - if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && - grid[i][j] == OBSTACLE) + if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) { obstacle_in_front_ = true; - obstacle_max_x_ = vehicle_min_x_ - i; found_front_obstacle = true; break; } - } - } - // 检查其他必停区（后方、左方、右方） - checkOtherStopZones(grid); - - // 分析前方扩展区域 - analyzeFrontExtendedArea(grid); - - // 如果没有检测到前方障碍物，设置为-1 - if (obstacle_max_x_ == 100 && !obstacle_in_front_ && !obstacle_in_front_area_) - { - obstacle_max_x_ = -1; - } - } - - // 检查其他必停区 - void checkOtherStopZones(const std::vector>& grid) - { - // 检查后方必停区 - for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x && !obstacle_in_rear_; ++i) - { + // 检测后方必停区障碍物 + for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x; ++i) for (int j = rear_stop_zone_.min_y; j <= rear_stop_zone_.max_y; ++j) - { - if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && - grid[i][j] == OBSTACLE) + if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) { obstacle_in_rear_ = true; break; } - } - } - // 检查左方必停区 - for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x && !obstacle_in_left_; ++i) - { + // 检测左侧必停区障碍物 + for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x; ++i) for (int j = left_stop_zone_.min_y; j <= left_stop_zone_.max_y; ++j) - { - if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && - grid[i][j] == OBSTACLE) + if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) { obstacle_in_left_ = true; break; } - } - } - // 检查右方必停区 - for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x && !obstacle_in_right_; ++i) - { + // 检测右侧必停区障碍物 + for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x; ++i) for (int j = right_stop_zone_.min_y; j <= right_stop_zone_.max_y; ++j) - { - if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && - grid[i][j] == OBSTACLE) + if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) { obstacle_in_right_ = true; break; } - } - } - } - // 分析前方扩展区域 - 优化策略：只检测车辆宽度范围的前方 - void analyzeFrontExtendedArea(const std::vector>& grid) - { - size_t front_area_min_x = static_cast(max(0, vehicle_min_x_ - front_area_extend_)); - size_t front_area_max_x = static_cast(vehicle_min_x_ - 1); - // 按照你的优化：只检测车辆宽度范围，避免不必要的绕障 - size_t front_area_min_y = static_cast(max(0, vehicle_min_y_)); - size_t front_area_max_y = static_cast(min(static_cast(grid[0].size() - 1), vehicle_max_y_)); + // 检测前方扩展区域障碍物 + int front_area_min_x = max(0, vehicle_min_x_ - front_area_extend_); + int front_area_max_x = vehicle_min_x_ - 1; + int front_area_min_y = max(0, vehicle_min_y_ - left_area_extend_); + int front_area_max_y = min((int)grid[0].size() - 1, vehicle_max_y_ + right_area_extend_); + int leftmost_obstacle = -1, rightmost_obstacle = -1; - // 重置边缘检测 - int leftmost_obstacle = -1; - int rightmost_obstacle = -1; - - // 遍历前方核心区域（车辆宽度范围） - for (size_t i = front_area_min_x; i <= front_area_max_x; ++i) + for (size_t i = front_area_min_x; i <= (size_t)front_area_max_x; ++i) { - for (size_t j = front_area_min_y; j <= front_area_max_y; ++j) + for (size_t j = front_area_min_y; j <= (size_t)front_area_max_y; ++j) { if (grid[i][j] == OBSTACLE) { obstacle_in_front_area_ = true; - - // 更新最左和最右障碍物位置 - if (leftmost_obstacle == -1 || static_cast(j) < leftmost_obstacle) - leftmost_obstacle = static_cast(j); - if (rightmost_obstacle == -1 || static_cast(j) > rightmost_obstacle) - rightmost_obstacle = static_cast(j); - - // 更新最近障碍物X坐标 - int distance = vehicle_min_x_ - static_cast(i); - if (distance < obstacle_max_x_) - { - obstacle_max_x_ = distance; - } + if (leftmost_obstacle == -1 || (int)j < leftmost_obstacle) leftmost_obstacle = j; + if (rightmost_obstacle == -1 || (int)j > rightmost_obstacle) rightmost_obstacle = j; + int distance = vehicle_min_x_ - (int)i; + if (distance < obstacle_max_x_) obstacle_max_x_ = distance; } } } - // 重新计算可用空间 - 基于更大的绕障检测范围 + // 计算绕障空间 calculateAvoidanceSpace(grid, leftmost_obstacle, rightmost_obstacle); } - // 计算绕障可用空间 - 在更大范围内分析左右通道 + // ======== 绕障空间计算 ======== void calculateAvoidanceSpace(const std::vector>& grid, int core_left_obstacle, int core_right_obstacle) { - if (obstacle_in_front_area_) + if (!obstacle_in_front_area_) { - obstacle_left_edge_ = core_left_obstacle; - obstacle_right_edge_ = core_right_obstacle; - - // 为了绕障决策，需要检查更宽的左右区域 - size_t avoidance_min_x = static_cast(max(0, vehicle_min_x_ - front_area_extend_)); - size_t avoidance_max_x = static_cast(vehicle_min_x_ - 1); - - // 左侧绕障空间检查：从 车辆左边界 到 障碍物左边界 - int left_check_start = max(0, vehicle_min_y_ - left_area_extend_); - int left_check_end = obstacle_left_edge_ - 1; - free_space_left_ = - calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, left_check_start, left_check_end); - - // 右侧绕障空间检查：从 障碍物右边界 到 车辆右边界 - int right_check_start = obstacle_right_edge_ + 1; - int right_check_end = min(static_cast(grid[0].size() - 1), vehicle_max_y_ + right_area_extend_); - free_space_right_ = - calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, right_check_start, right_check_end); - } - else - { - // 没有前方障碍物时，设置较大的可用空间 左右扩展+车宽 free_space_left_ = left_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1); free_space_right_ = right_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1); - obstacle_left_edge_ = -1; - obstacle_right_edge_ = -1; + obstacle_left_edge_ = obstacle_right_edge_ = -1; + return; } + + obstacle_left_edge_ = core_left_obstacle; + obstacle_right_edge_ = core_right_obstacle; + size_t avoidance_min_x = (size_t)max(0, vehicle_min_x_ - front_area_extend_); + size_t avoidance_max_x = (size_t)vehicle_min_x_ - 1; + + // 计算左侧空闲空间 + int left_check_start = max(0, vehicle_min_y_ - left_area_extend_); + int left_check_end = obstacle_left_edge_ - 1; + free_space_left_ = + calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, left_check_start, left_check_end); + + // 计算右侧空闲空间 + int right_check_start = obstacle_right_edge_ + 1; + int right_check_end = min((int)grid[0].size() - 1, vehicle_max_y_ + right_area_extend_); + free_space_right_ = + calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, right_check_start, right_check_end); } - // 计算指定方向的可用空间 + // ======== 方向空闲空间计算 ======== int calculateSpaceInDirection(const std::vector>& grid, size_t min_x, size_t max_x, int check_start_y, int check_end_y) { if (check_start_y > check_end_y) return 0; - int available_space = 0; - - // 从起始位置开始，逐列检查直到遇到障碍物 for (int j = check_start_y; j <= check_end_y; ++j) { - if (j < 0 || j >= static_cast(grid[0].size())) break; - + if (j < 0 || j >= (int)grid[0].size()) break; bool column_clear = true; - // 检查该列在检测范围内是否有障碍物 for (size_t i = min_x; i <= max_x; ++i) { - if (i >= grid.size()) break; - if (grid[i][j] == OBSTACLE) + if (i >= grid.size() || grid[i][j] == OBSTACLE) { column_clear = false; break; } } - if (column_clear) - { available_space++; - } else - { - break; // 遇到障碍物，停止计算 - } + break; } - return available_space; } + // ======== 栅格可视化 ======== void visualizeGridInTerminal(const std::vector>& grid) { std::stringstream ss; - - ss << " --------------------障碍物矩阵-------------------- " << std::endl; - - // 打印顶部边框 - ss << " " << std::string(grid[0].size(), '-') << std::endl; - - // 打印列号（仅0-9） + ss << " ----------栅格地图可视化---------- " << endl; + ss << " " << string(grid[0].size(), '-') << endl; ss << " "; - for (size_t i = 0; i < grid[0].size(); i++) - { - ss << (i % 10); - } - ss << std::endl; - - // 打印栅格内容（行号 + 栅格） + for (size_t i = 0; i < grid[0].size(); i++) ss << (i % 10); + ss << endl; for (size_t i = 0; i < grid.size(); i++) { - // 行号显示（两位数，右对齐） ss << (i < 10 ? " " : "") << i << "|"; - - // 栅格内容 for (size_t j = 0; j < grid[i].size(); j++) { - // 检查当前位置是否在某个必停区内 - bool in_front_zone = (i >= static_cast(front_stop_zone_.min_x) && - i <= static_cast(front_stop_zone_.max_x) && - j >= static_cast(front_stop_zone_.min_y) && - j <= static_cast(front_stop_zone_.max_y)); + bool in_front = (i >= front_stop_zone_.min_x && i <= front_stop_zone_.max_x && + j >= front_stop_zone_.min_y && j <= front_stop_zone_.max_y); + bool in_left = (i >= left_stop_zone_.min_x && i <= left_stop_zone_.max_x && + j >= left_stop_zone_.min_y && j <= left_stop_zone_.max_y); + bool in_right = (i >= right_stop_zone_.min_x && i <= right_stop_zone_.max_x && + j >= right_stop_zone_.min_y && j <= right_stop_zone_.max_y); - bool in_left_zone = (i >= static_cast(left_stop_zone_.min_x) && - i <= static_cast(left_stop_zone_.max_x) && - j >= static_cast(left_stop_zone_.min_y) && - j <= static_cast(left_stop_zone_.max_y)); - - bool in_right_zone = (i >= static_cast(right_stop_zone_.min_x) && - i <= static_cast(right_stop_zone_.max_x) && - j >= static_cast(right_stop_zone_.min_y) && - j <= static_cast(right_stop_zone_.max_y)); - - // 根据栅格值和必停区状态选择显示符号 if (grid[i][j] == OBSTACLE) ss << "@"; else if (grid[i][j] == VEHICLE) ss << "V"; - else if (in_front_zone) - ss << "F"; // 前方必停区 - else if (in_left_zone) - ss << "L"; // 左方必停区 - else if (in_right_zone) - ss << "R"; // 右方必停区 + else if (in_front) + ss << "F"; + else if (in_left) + ss << "L"; + else if (in_right) + ss << "R"; else ss << "."; } - - ss << "|" << std::endl; + ss << "|" << endl; } - - // 打印底部边框 - ss << " " << std::string(grid[0].size(), '-') << std::endl; - + ss << " " << string(grid[0].size(), '-') << endl; LOG_INFO("%s", ss.str().c_str()); } + // ======== 规划层消息回调 ======== void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg) { - x = msg->x; - y = msg->y; - speed = (int)msg->speed; // 获取PL消息中的速度 - reliability = msg->reliability; - located = msg->enter_range; + pl_x_ = msg->x; + pl_y_ = msg->y; + pl_speed_ = (int)msg->speed; is_start = msg->is_start; - LOG_INFO("目标点位置： x -- %.2f , y -- %.2f", x, y); + LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_); } - void timer_callback() + // ======== 绕障路点规划 ======== + void planAvoidanceWaypoints(int avoid_direction) { - if (speed == 0) + avoidance_waypoints_.clear(); + + const double GRID_RESOLUTION = 0.3; + const int SAFE_GRID_GAP = 1; + const int AVOID_SPEED = 300; + + Waypoint point_lateral_shift; + Waypoint point_parallel_forward; + double actual_lateral_offset_m = 0.0; + int lateral_offset_grid_num = 0; + + const int stage1_forward_grid = state_machine_->obstacle_analysis.obstacle_distance; + const int stage2_forward_grid = parallel_distance_; + const double stage1_forward_m = stage1_forward_grid * GRID_RESOLUTION; + const double stage2_forward_m = stage2_forward_grid * GRID_RESOLUTION; + + // 计算横向偏移量 + if (avoid_direction == -1) { - publish_speed = 0; - publish_angle = 0; - LOG_INFO("PL速度为0，已停车"); + lateral_offset_grid_num = (vehicle_max_y_ - state_machine_->obstacle_analysis.left_edge) + SAFE_GRID_GAP; + actual_lateral_offset_m = -1 * lateral_offset_grid_num * GRID_RESOLUTION; } - else if (is_start) + else if (avoid_direction == 1) { - // 障碍物检测与避障决策 - applyObstacleAvoidance(); + lateral_offset_grid_num = (state_machine_->obstacle_analysis.right_edge - vehicle_min_y_) + SAFE_GRID_GAP; + actual_lateral_offset_m = 1 * lateral_offset_grid_num * GRID_RESOLUTION; + } - // 发布控制指令 - sweeper_interfaces::msg::McCtrl message; - message.sweep = true; - message.brake = 0; // 0开;1关 - message.gear = 2; // 0空挡;1后退;2前进 + // 相对坐标 + double relative_x_1 = actual_lateral_offset_m; + double relative_y_1 = stage1_forward_m; + double relative_x_2 = actual_lateral_offset_m; + double relative_y_2 = stage2_forward_m; - if (reliability == 1) - message.rpm = publish_speed; - else - { - message.rpm = 0; - LOG_ERROR("rtk信号差,停车！！！"); - } + // 转换为绝对GPS坐标 + double abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2; + rtk_relative_to_gps(relative_x_1, relative_y_1, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_1, &abs_lat_1); + rtk_relative_to_gps(relative_x_2, relative_y_2, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_2, &abs_lat_2); - message.angle = publish_angle; // 负数表示左转，正数表示右转 - message.angle_speed = 800; + // 构建路点 + point_lateral_shift = Waypoint(relative_x_1, relative_y_1, AVOID_SPEED, abs_lon_1, abs_lat_1); + point_parallel_forward = Waypoint(relative_x_2, relative_y_2, AVOID_SPEED, abs_lon_2, abs_lat_2); - pub_mc->publish(message); + avoidance_waypoints_.push_back(point_lateral_shift); + avoidance_waypoints_.push_back(point_parallel_forward); - // 改进的状态显示 - std::string state_str; - switch (avoid_ctx_.state) - { - case AVOID_NONE: - state_str = "正常行驶"; - break; - case AVOID_WAITING: - state_str = "等待决策"; - break; - case AVOID_LATERAL_SHIFT: - state_str = "横向偏移"; - break; - case AVOID_PARALLEL_MOVE: - state_str = "平行前进"; - break; - case AVOID_RETURN_TO_PATH: - state_str = "回归轨迹"; - break; - default: - state_str = "未知状态"; - } + current_waypoint_idx_ = 0; + avoiding_obstacle_ = true; + state_machine_->setState(StateMachine::LATERAL_SHIFT); - LOG_INFO("控制: 速度=%d, 角度=%d, 状态=%s, 前方%s障碍物", publish_speed, publish_angle, state_str.c_str(), - (obstacle_in_front_ ? "有" : "无")); + LOG_INFO("[绕障规划] 方向：%s | 路点1 GPS(%.6f,%.6f) | 路点2 GPS(%.6f,%.6f)", + avoid_direction == -1 ? "左" : "右", abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2); + } + + // ======== 相对坐标转GPS绝对坐标 ======== + void rtk_relative_to_gps(double rel_x, double rel_y, double cur_heading, double cur_lon, double cur_lat, + double* dest_lon, double* dest_lat) + { + double Navi_rad = cur_heading * deg_rad; + + // 逆向旋转矩阵：从车体坐标系转回地理坐标系 + float k1 = cos(Navi_rad); + float k2 = sin(Navi_rad); + float k3 = -sin(Navi_rad); + float k4 = cos(Navi_rad); + + // 地理坐标增量 + double delta_x = rel_x * k1 + rel_y * k3; // 东向增量 + double delta_y = rel_x * k2 + rel_y * k4; // 北向增量 + + // 转换为GPS坐标增量 + double delta_lon = delta_x / (deg_rad * R_LONT); + double delta_lat = delta_y / (deg_rad * R_LATI); + + *dest_lon = cur_lon + delta_lon; + *dest_lat = cur_lat + delta_lat; + + LOG_INFO("[相对转GPS] 相对(%.2f,%.2f) -> GPS增量(%.8f,%.8f) -> 目标(%.6f,%.6f)", rel_x, rel_y, delta_lon, + delta_lat, *dest_lon, *dest_lat); + } + + // ======== 路点到达判断 ======== + // 返回值：1=到达 0=未到达 -1=RTK信号丢失(需要停车) + int waypointReached(const Waypoint& target) + { + // 验证RTK信号质量 + auto now = system_clock::now(); + duration time_since_last = now - last_rtk_time; + + if (g_rtk.reliability != 0 && time_since_last <= rtk_timeout) + { + // RTK信号良好：使用绝对坐标判断 + double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat); + + LOG_INFO("[路点判断] RTK信号良好 | 当前GPS(%.6f,%.6f) | 目标GPS(%.6f,%.6f) | 距离: %.2f m | 容差: %.2f m", + g_rtk.lon, g_rtk.lat, target.lon, target.lat, distance, waypoint_tolerance_); + + return distance < waypoint_tolerance_ ? 1 : 0; } else { - // 未启动状态，停车 - publish_speed = 0; - publish_angle = 0; + // RTK信号丢失：无法安全判断 + if (time_since_last > rtk_timeout) + { + LOG_ERROR("[路点判断] RTK超时(%.2fs)，信号丢失，停车退出绕障！", time_since_last.count()); + } + else + { + LOG_ERROR("[路点判断] RTK信号差(可靠性=%d)，停车退出绕障！", g_rtk.reliability); + } + + return -1; } } - // 避障决策与控制 - void applyObstacleAvoidance() + // ======== 路点跟踪执行 ======== + void executeWaypointTracking() { - // 计算基础目标角度 - float target_angle = calculate_the_angle(x, y); - - // 初始化控制参数 - publish_speed = 800; - publish_angle = static_cast(target_angle); - - // 安全检查：未检测到车体位置 - if (vehicle_min_x_ == -1) + if (current_waypoint_idx_ >= (int)avoidance_waypoints_.size()) { - publish_speed = 0; - publish_angle = 0; - avoid_ctx_.reset(); - LOG_WARN("未检测到车体位置，安全停车"); + avoiding_obstacle_ = false; + current_waypoint_idx_ = 0; + avoidance_waypoints_.clear(); + state_machine_->setState(StateMachine::NORMAL); + LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制"); return; } - // 更新障碍物分析 - updateObstacleAnalysis(); + Waypoint& target = avoidance_waypoints_[current_waypoint_idx_]; - // 执行分层避障策略 - if (!executeEmergencyStop()) + // 计算目标方向角： + float target_angle = calculate_the_angle(target.x, target.y); + state_machine_->publish_angle = target_angle; + state_machine_->publish_speed = target.speed; + + LOG_INFO("[路点跟踪] 路点%d/%d | 相对坐标(%.2f,%.2f) | 目标方向: %.2f度", current_waypoint_idx_ + 1, + (int)avoidance_waypoints_.size(), target.x, target.y, target_angle); + + // 检查路点到达情况 + int reached = waypointReached(target); + + if (reached == -1) { - if (!executeTurnProtection()) + // RTK信号丢失，立即停车并退出绕障 + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0.0f; + state_machine_->setState(StateMachine::WAITING); + + avoiding_obstacle_ = false; + current_waypoint_idx_ = 0; + avoidance_waypoints_.clear(); + + LOG_ERROR("[绕障中止] RTK信号丢失，停车并退出绕障！"); + return; + } + else if (reached == 1) + { + // 已到达路点，切换到下一个 + current_waypoint_idx_++; + + if (current_waypoint_idx_ < (int)avoidance_waypoints_.size()) { - executeSmartAvoidance(); + state_machine_->setState(StateMachine::PARALLEL_MOVE); + LOG_INFO("[路点跟踪] 已到达路点%d，切换到路点%d", current_waypoint_idx_, current_waypoint_idx_ + 1); + } + else + { + avoiding_obstacle_ = false; + current_waypoint_idx_ = 0; + avoidance_waypoints_.clear(); + state_machine_->setState(StateMachine::NORMAL); + LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制"); + return; } } + else + { + // 未到达路点，继续前往 + state_machine_->setState(StateMachine::LATERAL_SHIFT); + } - // 记录角度历史 - avoid_ctx_.addAngleHistory(publish_angle); + // 安全检查：前方必停区有障碍物则立即停车 + if (state_machine_->obstacle_analysis.has_front_critical) + { + state_machine_->publish_speed = 0; + LOG_WARN("[绕障安全] 路点跟踪中检测到前方必停区障碍物，立即停车"); + } } + // ======== 可绕障判断 ======== + bool canAvoidObstacle() const + { + if (state_machine_->obstacle_analysis.obstacle_width <= 0 || + state_machine_->obstacle_analysis.obstacle_width > avoid_able_width_) + { + LOG_INFO("[绕障判断] 障碍物宽度%d栅格，超过可绕最大宽度%d栅格，不可绕", + state_machine_->obstacle_analysis.obstacle_width, avoid_able_width_); + return false; + } + bool left_ok = state_machine_->obstacle_analysis.free_space_left >= min_free_space_; + bool right_ok = state_machine_->obstacle_analysis.free_space_right >= min_free_space_; + LOG_INFO("[绕障判断] 左侧空闲%d栅格，右侧空闲%d栅格，最小需求%d栅格 | 左可绕：%s，右可绕：%s", + state_machine_->obstacle_analysis.free_space_left, state_machine_->obstacle_analysis.free_space_right, + min_free_space_, left_ok ? "是" : "否", right_ok ? "是" : "否"); + return left_ok || right_ok; + } + + // ======== 绕障方向选择 ======== + int selectOptimalAvoidDirection() const + { + if (state_machine_->obstacle_analysis.free_space_left > state_machine_->obstacle_analysis.free_space_right) + return -1; + else if (state_machine_->obstacle_analysis.free_space_right > state_machine_->obstacle_analysis.free_space_left) + return 1; + else + return (pl_x_ > 0) ? 1 : -1; + } + + // ======== 更新障碍物分析 ======== void updateObstacleAnalysis() { - obstacle_analysis_ = ObstacleAnalysis{}; // 重置 - - obstacle_analysis_.has_front_critical = obstacle_in_front_; - obstacle_analysis_.has_front_area = obstacle_in_front_area_; - obstacle_analysis_.has_left_critical = obstacle_in_left_; - obstacle_analysis_.has_right_critical = obstacle_in_right_; - - obstacle_analysis_.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; - obstacle_analysis_.left_edge = obstacle_left_edge_; - obstacle_analysis_.right_edge = obstacle_right_edge_; - LOG_INFO("障碍物左侧=%d，障碍物右侧=%d", obstacle_analysis_.left_edge, obstacle_analysis_.right_edge); - obstacle_analysis_.free_space_left = free_space_left_; - obstacle_analysis_.free_space_right = free_space_right_; + state_machine_->obstacle_analysis.has_front_critical = obstacle_in_front_; + state_machine_->obstacle_analysis.has_front_area = obstacle_in_front_area_; + state_machine_->obstacle_analysis.has_left_critical = obstacle_in_left_; + state_machine_->obstacle_analysis.has_right_critical = obstacle_in_right_; + state_machine_->obstacle_analysis.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; + state_machine_->obstacle_analysis.left_edge = obstacle_left_edge_; + state_machine_->obstacle_analysis.right_edge = obstacle_right_edge_; + state_machine_->obstacle_analysis.free_space_left = free_space_left_; + state_machine_->obstacle_analysis.free_space_right = free_space_right_; if (obstacle_left_edge_ != -1 && obstacle_right_edge_ != -1) { - obstacle_analysis_.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1; + state_machine_->obstacle_analysis.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1; } + + LOG_INFO("[障碍物分析] 距离：%d栅格 | 宽度：%d栅格 | 左边缘：%d | 右边缘：%d", + state_machine_->obstacle_analysis.obstacle_distance, state_machine_->obstacle_analysis.obstacle_width, + state_machine_->obstacle_analysis.left_edge, state_machine_->obstacle_analysis.right_edge); } - // 第一优先级：紧急停车策略 + // ======== 紧急停车执行 ======== bool executeEmergencyStop() { if (!enable_obstacle_stop_) return false; - // 前方必停区有障碍物，立即停车 - if (obstacle_analysis_.has_front_critical) + if (state_machine_->obstacle_analysis.has_front_critical) { - publish_speed = 0; - publish_angle = 0; - avoid_ctx_.state = AVOID_WAITING; - avoid_ctx_.counter++; - - // 每2秒打印一次状态 - if (avoid_ctx_.counter % 20 == 0) - { - LOG_WARN("紧急停车：前方必停区有障碍物，距离=%d格", obstacle_analysis_.obstacle_distance); - } + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0.0f; + state_machine_->setState(StateMachine::WAITING); + LOG_WARN("[紧急停车] 前方必停区检测到障碍物，执行紧急停车"); return true; } - - // 动态紧急制动：基于速度和距离 - if (obstacle_analysis_.needEmergencyBrake(publish_speed)) - { - publish_speed = 0; - publish_angle = 0; - avoid_ctx_.state = AVOID_WAITING; - LOG_WARN("动态紧急制动：障碍物距离%d格，速度%d", obstacle_analysis_.obstacle_distance, publish_speed); - return true; - } - return false; } - // 第二优先级：转向保护策略 - bool executeTurnProtection() + // ======== 定时器回调（主逻辑） ======== + void timer_callback() { - if (!enable_obstacle_stop_) return false; + // 更新障碍物分析 + updateObstacleAnalysis(); - // 检查转向方向的安全性 - if (publish_angle < -10 && obstacle_analysis_.has_left_critical) + // 开始清扫任务 + if (is_start) { - publish_speed = 0; - publish_angle = 0; - LOG_WARN("转向保护：左转时左侧有障碍物"); - return true; - } - - if (publish_angle > 10 && obstacle_analysis_.has_right_critical) - { - publish_speed = 0; - publish_angle = 0; - LOG_WARN("转向保护：右转时右侧有障碍物"); - return true; - } - - return false; - } - - // 处理无法绕行的障碍物 - void handleUnpassableObstacle() - { - int safe_distance = obstacle_analysis_.calculateSafeDistance(publish_speed); - - if (obstacle_analysis_.obstacle_distance <= safe_distance) - { - // 太近，必须停车 - publish_speed = 0; - avoid_ctx_.state = AVOID_WAITING; - avoid_ctx_.counter++; - - if (avoid_ctx_.counter % 30 == 0) - { // 每3秒打印一次 - LOG_INFO("障碍物无法绕行(宽度=%d格)，等待通过...", obstacle_analysis_.obstacle_width); - } - } - else - { - // 距离足够，可以减速接近 - publish_speed = std::max(600, publish_speed / 2); - LOG_INFO("障碍物较宽，减速接近中，距离=%d格", obstacle_analysis_.obstacle_distance); - } - } - - // 启动绕障 - void initiateAvoidance() - { - auto current_time = node_clock_->now(); - - // 选择最优绕障方向 - int direction = selectOptimalDirection(); - - // 计算需要的横向偏移距离 - int required_shift = calculateRequiredLateralShift(direction); - - avoid_ctx_.state = AVOID_LATERAL_SHIFT; - avoid_ctx_.direction = direction; - avoid_ctx_.counter = 0; - avoid_ctx_.start_time = current_time; - avoid_ctx_.start_time_set = true; - avoid_ctx_.last_state_change_time = current_time; - avoid_ctx_.last_state_change_time_set = true; - avoid_ctx_.original_angle = static_cast(calculate_the_angle(x, y)); - avoid_ctx_.lateral_shift_distance = required_shift; - avoid_ctx_.lateral_shift_achieved = 0; - avoid_ctx_.parallel_start_obstacle_x = -1; - avoid_ctx_.obstacle_passed = false; - - LOG_INFO("启动绕障: 方向=%s, 需要偏移=%d格, 障碍物宽度=%d, 左侧空间=%d, 右侧空间=%d", - (direction == -1) ? "左" : "右", required_shift, obstacle_analysis_.obstacle_width, - obstacle_analysis_.free_space_left, obstacle_analysis_.free_space_right); - } - - // 方向选择算法 - int selectOptimalDirection() - { - int left_score = 0; - int right_score = 0; - - // 1. 可用空间评分(主要因素) - left_score += obstacle_analysis_.free_space_left * 20; - right_score += obstacle_analysis_.free_space_right * 20; - - // 2. 安全性评分(关键因素) - if (obstacle_analysis_.has_left_critical) left_score -= 1000; - if (obstacle_analysis_.has_right_critical) right_score -= 1000; - - // 3. 障碍物位置分析 - 基于车辆边界而非中心 - if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) - { - // 计算障碍物与车辆的相对位置 - int vehicle_width = vehicle_max_y_ - vehicle_min_y_ + 1; - - // 左侧可用空间 = 车辆左边界到障碍物左边界的距离 - int left_clearance = obstacle_analysis_.left_edge - vehicle_min_y_; - // 右侧可用空间 = 障碍物右边界到车辆右边界的距离 - int right_clearance = vehicle_max_y_ - obstacle_analysis_.right_edge; - - LOG_INFO("位置分析: 左侧间隙=%d, 右侧间隙=%d, 车宽=%d", left_clearance, right_clearance, vehicle_width); - - // 基于实际间隙大小评分 - if (left_clearance > right_clearance + 2) + // PL速度为0，停车 + if (pl_speed_ == 0) { - left_score += 25; // 左侧明显更宽松 - } - else if (right_clearance > left_clearance + 2) - { - right_score += 25; // 右侧明显更宽松 + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0; + LOG_INFO("PL速度为0，已停车"); + publishControlCommand(); + return; } - // 如果障碍物严重偏向一侧,反方向绕行 - if (obstacle_analysis_.right_edge < vehicle_min_y_) + // 紧急停车优先级最高 + if (executeEmergencyStop()) { - // 障碍物完全在车辆左侧,应该右绕 - right_score += 30; - LOG_INFO("障碍物在左侧,倾向右绕"); + publishControlCommand(); + return; } - else if (obstacle_analysis_.left_edge > vehicle_max_y_) + + // 绕障逻辑 + if (enable_obstacle_avoid_) { - // 障碍物完全在车辆右侧,应该左绕 - left_score += 30; - LOG_INFO("障碍物在右侧,倾向左绕"); - } - } - - // 4. 目标方向偏好 - 降低权重,避免过度影响 - float target_angle = calculate_the_angle(x, y); - if (target_angle < -15) // 阈值从10提高到15 - left_score += 15; // 权重从20降低到15 - if (target_angle > 15) right_score += 15; - - // 5. 历史趋势稳定性(避免频繁切换) - 权重提高 - if (!avoid_ctx_.angle_history.empty() && avoid_ctx_.angle_history.size() >= 5) // 从3提高到5 - { - int recent_avg = 0; - int count = std::min(5, static_cast(avoid_ctx_.angle_history.size())); - - for (int i = avoid_ctx_.angle_history.size() - count; i < static_cast(avoid_ctx_.angle_history.size()); - i++) - { - recent_avg += avoid_ctx_.angle_history[i]; - } - recent_avg /= count; - - if (recent_avg < -8) // 阈值从5提高到8 - left_score += 15; // 权重从10提高到15 - if (recent_avg > 8) right_score += 15; - } - - int selected_direction = (left_score > right_score) ? -1 : 1; - - LOG_INFO("方向选择 - 左评分:%d 右评分:%d → %s (左空间:%d 右空间:%d)", left_score, right_score, - (selected_direction == -1) ? "左绕" : "右绕", obstacle_analysis_.free_space_left, - obstacle_analysis_.free_space_right); - - return selected_direction; - } - - // 计算需要的横向偏移距离 - int calculateRequiredLateralShift(int direction) - { - if (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.right_edge == -1) - { - return 3; // 默认偏移3格 - } - - int safety_margin = 2; // 安全余量 - int required_shift = 0; - - if (direction == -1) // 左绕 - { - // 车体右边界需要绕过障碍物右边界 - required_shift = vehicle_max_y_ - obstacle_analysis_.right_edge + safety_margin; - } - else // 右绕 - { - // 车体左边界需要绕过障碍物左边界 - required_shift = obstacle_analysis_.left_edge - vehicle_min_y_ + safety_margin; - } - - // 限制在合理范围内 - required_shift = std::max(2, std::min(5, required_shift)); - - return required_shift; - } - - // 执行主动绕障 - 三阶段状态机 - void executeSmartAvoidance() - { - if (!enable_obstacle_avoid_ || !obstacle_analysis_.has_front_area) - { - // 无障碍物或绕障功能关闭 - if (avoid_ctx_.state != AVOID_NONE) - { - avoid_ctx_.reset(); - LOG_INFO("前方无障碍物，重置绕障状态"); - } - return; - } - - // 评估是否需要绕障 - if (!obstacle_analysis_.canAvoid()) - { - LOG_INFO("障碍物无法绕行，减速靠近"); - handleUnpassableObstacle(); - return; - } - - // 三阶段状态机 - switch (avoid_ctx_.state) - { - case AVOID_NONE: - case AVOID_WAITING: - initiateAvoidance(); - break; - - case AVOID_LATERAL_SHIFT: - executeLateralShift(); - break; - - case AVOID_PARALLEL_MOVE: - executeParallelMove(); - break; - - case AVOID_RETURN_TO_PATH: - executeReturnToPath(); - break; - } - } - - // 阶段1: 横向偏移 - void executeLateralShift() - { - auto current_time = node_clock_->now(); - double shift_time = 0.0; - - if (avoid_ctx_.last_state_change_time_set) - { - shift_time = avoid_ctx_.getElapsedTime(avoid_ctx_.last_state_change_time, current_time); - } - - // 速度控制:横向偏移时限速 - publish_speed = 600; - - // 角度控制:保持较大角度进行横向偏移 - int shift_angle = calculateLateralShiftAngle(); - publish_angle = avoid_ctx_.direction * shift_angle; - - // 安全检查 - if (checkAvoidanceSafety()) - { - return; - } - - // 估算已完成的横向偏移 - avoid_ctx_.lateral_shift_achieved = static_cast(shift_time * 1.5); - - // 完成条件 - bool time_sufficient = (shift_time >= avoid_ctx_.min_lateral_time); - bool distance_sufficient = (avoid_ctx_.lateral_shift_achieved >= avoid_ctx_.lateral_shift_distance); - bool lateral_clearance_achieved = checkLateralClearance(); - - // 验证是否真正远离障碍物 - bool safe_distance_achieved = verifySafeDistance(); - - // 所有条件都满足才进入下一阶段 - bool shift_complete = - time_sufficient && distance_sufficient && lateral_clearance_achieved && safe_distance_achieved; - - if (shift_complete) - { - avoid_ctx_.state = AVOID_PARALLEL_MOVE; - avoid_ctx_.last_state_change_time = current_time; - avoid_ctx_.last_state_change_time_set = true; - avoid_ctx_.parallel_start_obstacle_x = obstacle_max_x_; - - LOG_INFO("阶段1完成: 横向偏移完成(%.1fs, %d格), 进入阶段2: 平行前进", shift_time, - avoid_ctx_.lateral_shift_achieved); - } - else - { - if (avoid_ctx_.counter % 10 == 0) - { - LOG_INFO("阶段1进行中: 时间%.1fs/%s, 距离%d/%d格, 间隙%s, 安全距离%s", shift_time, - time_sufficient ? "✓" : "✗", avoid_ctx_.lateral_shift_achieved, - avoid_ctx_.lateral_shift_distance, lateral_clearance_achieved ? "✓" : "✗", - safe_distance_achieved ? "✓" : "✗"); - } - } - - avoid_ctx_.counter++; - } - - // 验证安全距离 - bool verifySafeDistance() - { - // 检查当前是否已经没有前方障碍物警告 - if (obstacle_analysis_.has_front_critical || obstacle_analysis_.has_front_area) - { - LOG_DEBUG("前方仍有障碍区域"); - return false; - } - - // 检查绕障方向的侧方是否有障碍物 - if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) - { - LOG_DEBUG("左绕时左侧仍有障碍"); - return false; - } - - if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) - { - LOG_DEBUG("右绕时右侧仍有障碍"); - return false; - } - - // 检查障碍物是否已经移到侧后方 - if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) - { - if (avoid_ctx_.direction == -1) // 左绕,障碍物应该在右侧 - { - if (obstacle_analysis_.left_edge <= vehicle_max_y_) + if (avoiding_obstacle_) { - LOG_DEBUG("左绕时障碍物左边界%d仍在车右边界%d范围内", obstacle_analysis_.left_edge, vehicle_max_y_); - return false; + // 正在绕障，执行路点跟踪 + executeWaypointTracking(); + } + else if (state_machine_->obstacle_analysis.has_front_area && canAvoidObstacle()) + { + // 需要开始绕障 + int avoid_dir = selectOptimalAvoidDirection(); + planAvoidanceWaypoints(avoid_dir); + LOG_INFO("[启动绕障] 开始%s侧绕障", avoid_dir == -1 ? "左" : "右"); + } + else if (state_machine_->getCurrentState() != StateMachine::NORMAL) + { + // 无需绕障，恢复正常状态 + state_machine_->setState(StateMachine::NORMAL); } } - else // 右绕,障碍物应该在左侧 - { - if (obstacle_analysis_.right_edge >= vehicle_min_y_) - { - LOG_DEBUG("右绕时障碍物右边界%d仍在车左边界%d范围内", obstacle_analysis_.right_edge, - vehicle_min_y_); - return false; - } - } - } - return true; + // 正常行驶逻辑 + if (state_machine_->getCurrentState() == StateMachine::NORMAL) + { + state_machine_->publish_speed = 800; + state_machine_->publish_angle = calculate_the_angle(pl_x_, pl_y_); + } + + // 发布控制指令 + publishControlCommand(); + } } - // 计算横向偏移角度 - int calculateLateralShiftAngle() + // ======== 发布控制指令 ======== + void publishControlCommand() { - // 基于可用空间和需求偏移距离计算角度 - int available_space = - (avoid_ctx_.direction == -1) ? obstacle_analysis_.free_space_left : obstacle_analysis_.free_space_right; + auto message = sweeper_interfaces::msg::McCtrl(); + message.sweep = true; + message.brake = 0; + message.gear = 2; + message.angle_speed = 800; - int base_angle = 20; // 基础角度 + // 转速限制 + message.rpm = std::clamp(state_machine_->publish_speed, 0, 800); + message.angle = std::clamp(state_machine_->publish_angle, -50.0f, 50.0f); - if (available_space >= 4) - { - base_angle = 25; // 空间充足，可以大角度 - } - else if (available_space >= 3) - { - base_angle = 20; - } - else - { - base_angle = 15; // 空间紧张，小角度谨慎 - } + pub_mc->publish(message); - // 距离修正：障碍物越近，角度越小 - if (obstacle_analysis_.obstacle_distance > 0 && obstacle_analysis_.obstacle_distance < 3) - { - base_angle = static_cast(base_angle * 0.7); - } - - return std::max(10, std::min(avoid_ctx_.max_avoid_angle, base_angle)); - } - - // 检查横向间隙是否足够 - bool checkLateralClearance() - { - if (!grid_data_valid_ || cached_grid_.empty()) return false; - - // 最小偏移阈值 - const int MIN_SHIFT_THRESHOLD = 2; // 2格 - if (avoid_ctx_.lateral_shift_achieved < MIN_SHIFT_THRESHOLD) - { - LOG_DEBUG("横移距离不足: %d < %d", avoid_ctx_.lateral_shift_achieved, MIN_SHIFT_THRESHOLD); - return false; - } - - // 安全余量 - const int SAFE_MARGIN = 3; // 3格 - // ---------------------------- - // 1) 基于障碍物边界判断横向绕出 - // ---------------------------- - if (avoid_ctx_.direction == -1) // 左绕 - { - int required_position = obstacle_analysis_.left_edge - SAFE_MARGIN; - if (vehicle_max_y_ > required_position) // 右边界 - { - LOG_DEBUG("左绕未完成: 车右边界%d > 目标位置%d", vehicle_max_y_, required_position); - return false; - } - } - else // 右绕 - { - int required_position = obstacle_analysis_.right_edge + SAFE_MARGIN; - if (vehicle_min_y_ < required_position) - { - LOG_DEBUG("右绕未完成: 车左边界%d < 目标位置%d", vehicle_min_y_, required_position); - return false; - } - } - - // ---------------------------- - // 2) 检查前方更深的区域是否清空 - // ---------------------------- - const int CHECK_DEPTH = 10; // 10行 - for (int depth = 1; depth <= CHECK_DEPTH; depth++) - { - int check_row = vehicle_min_x_ - depth; - if (check_row < 0) break; - - // 检查整个车宽范围 - for (int col = vehicle_min_y_; col <= vehicle_max_y_; col++) - { - if (col < 0 || col >= static_cast(cached_grid_[0].size())) continue; - - if (cached_grid_[check_row][col] == OBSTACLE) - { - LOG_DEBUG("前方第%d行仍有障碍,位置[%d,%d]", depth, check_row, col); - return false; - } - } - } - - // ---------------------------- - // 3) 【新增】检查侧方是否真正清空 - // ---------------------------- - if (avoid_ctx_.direction == -1) // 左绕,检查右侧 - { - for (int depth = 1; depth <= 5; depth++) - { - int check_row = vehicle_min_x_ - depth; - if (check_row < 0) break; - - // 检查车辆右侧若干列 - for (int offset = 1; offset <= 3; offset++) - { - int check_col = vehicle_max_y_ + offset; - if (check_col >= static_cast(cached_grid_[0].size())) break; - - if (cached_grid_[check_row][check_col] == OBSTACLE) - { - LOG_DEBUG("右侧仍有障碍[%d,%d],左绕未完全清空", check_row, check_col); - return false; - } - } - } - } - else // 右绕,检查左侧 - { - for (int depth = 1; depth <= 5; depth++) - { - int check_row = vehicle_min_x_ - depth; - if (check_row < 0) break; - - // 检查车辆左侧若干列 - for (int offset = 1; offset <= 3; offset++) - { - int check_col = vehicle_min_y_ - offset; - if (check_col < 0) break; - - if (cached_grid_[check_row][check_col] == OBSTACLE) - { - LOG_DEBUG("左侧仍有障碍[%d,%d],右绕未完全清空", check_row, check_col); - return false; - } - } - } - } - - LOG_INFO("横向间隙检查通过: 方向=%s, 已移动=%d格", (avoid_ctx_.direction == -1) ? "左" : "右", - avoid_ctx_.lateral_shift_achieved); - - return true; - } - - // 阶段2: 平行前进 - void executeParallelMove() - { - auto current_time = node_clock_->now(); - double parallel_time = 0.0; - - if (avoid_ctx_.last_state_change_time_set) - { - parallel_time = avoid_ctx_.getElapsedTime(avoid_ctx_.last_state_change_time, current_time); - } - - // 速度控制：平行前进时可以适当提速 - publish_speed = 700; - - // 角度控制：回正，保持直行 - publish_angle = 0; - - // 安全检查 - if (checkParallelMoveSafety()) - { - return; // 检测到危险 - } - - // 判断障碍物是否已经与车头持平或更后 - bool obstacle_aligned_or_behind = checkObstaclePosition(); - - // 最小平行前进时间保护 - bool min_time_met = (parallel_time >= avoid_ctx_.min_parallel_time); - - if (obstacle_aligned_or_behind && min_time_met) - { - avoid_ctx_.state = AVOID_RETURN_TO_PATH; - avoid_ctx_.last_state_change_time = current_time; - avoid_ctx_.last_state_change_time_set = true; - avoid_ctx_.obstacle_passed = true; - - LOG_INFO("阶段2完成: 障碍物已在车身后方(%.1fs), 进入阶段3: 回归轨迹", parallel_time); - } - else - { - if (avoid_ctx_.counter % 10 == 0) - { - LOG_INFO("阶段2进行中: 平行前进 %.1fs, 障碍物位置检查=%s", parallel_time, - obstacle_aligned_or_behind ? "已通过" : "未通过"); - } - } - - avoid_ctx_.counter++; - } - - // 检查障碍物位置（是否与车头持平或在车身后方） - bool checkObstaclePosition() - { - // 方法1: 检查前方核心区域是否清空 - bool front_core_clear = !obstacle_analysis_.has_front_area && !obstacle_analysis_.has_front_critical; - - // 方法2: 检查障碍物是否不再在绕障侧的检测范围内 - bool lateral_clear = false; - if (avoid_ctx_.direction == -1) // 左绕 - { - // 检查右侧区域是否清空（障碍物应该在左后方） - lateral_clear = !obstacle_analysis_.has_right_critical && - (obstacle_analysis_.right_edge == -1 || obstacle_analysis_.right_edge < vehicle_min_y_); - } - else // 右绕 - { - // 检查左侧区域是否清空（障碍物应该在右后方） - lateral_clear = !obstacle_analysis_.has_left_critical && - (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.left_edge > vehicle_max_y_); - } - - // 综合判断 - bool obstacle_passed = front_core_clear && lateral_clear; - - if (avoid_ctx_.counter % 10 == 0) - { - LOG_DEBUG("障碍物位置检查: 前方清空=%s, 侧方清空=%s => %s", front_core_clear ? "是" : "否", - lateral_clear ? "是" : "否", obstacle_passed ? "已通过" : "未通过"); - } - - return obstacle_passed; - } - - // 平行前进安全检查 - bool checkParallelMoveSafety() - { - // 检查前方是否出现新障碍物 - if (obstacle_analysis_.has_front_critical) - { - publish_speed = 0; - avoid_ctx_.state = AVOID_WAITING; - LOG_ERROR("平行前进时前方出现障碍物！"); - return true; - } - - // 检查绕障侧是否有障碍物（防止剐蹭） - if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) - { - publish_speed = 0; - avoid_ctx_.state = AVOID_WAITING; - LOG_ERROR("平行前进时左侧出现障碍物！"); - return true; - } - - if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) - { - publish_speed = 0; - avoid_ctx_.state = AVOID_WAITING; - LOG_ERROR("平行前进时右侧出现障碍物！"); - return true; - } - - return false; - } - - // 阶段3: 回归原轨迹 - void executeReturnToPath() - { - auto current_time = node_clock_->now(); - double return_time = 0.0; - - if (avoid_ctx_.last_state_change_time_set) - { - return_time = avoid_ctx_.getElapsedTime(avoid_ctx_.last_state_change_time, current_time); - } - - // 计算目标角度 - float target_angle = calculate_the_angle(x, y); - int target_angle_int = static_cast(target_angle); - - // 1. 转向保护 - 有障碍就停 - if (checkReturnSafety(target_angle_int)) - { - return; - } - - // 2. 缓慢回正 - int angle_diff = target_angle_int - publish_angle; - const int ANGLE_TOLERANCE = 3; // ±3度容差 - const int RETURN_RATE = 1; // 固定回正速率 - - if (std::abs(angle_diff) <= ANGLE_TOLERANCE) - { - // 回正完成 - publish_angle = target_angle_int; - publish_speed = 800; - avoid_ctx_.reset(); - - LOG_INFO("✓ 回正完成(%.1fs), 绕障结束! 目标角度=%d°", return_time, target_angle_int); - } - else - { - // 缓慢调整角度 - if (angle_diff > 0) - { - publish_angle += std::min(RETURN_RATE, angle_diff); - } - else - { - publish_angle -= std::min(RETURN_RATE, -angle_diff); - } - - publish_speed = 700; // 回正时保持低速 - - if (avoid_ctx_.counter % 10 == 0) - { - LOG_INFO("回正中: 当前角度=%d°, 目标角度=%d°, 差值=%d°", publish_angle, target_angle_int, angle_diff); - } - } - - avoid_ctx_.counter++; - } - - // 回归安全检查 - bool checkReturnSafety(int target_angle) - { - // 检查目标转向方向是否有障碍 - if (target_angle < -10 && obstacle_analysis_.has_left_critical) - { - publish_speed = 0; - publish_angle = 0; - LOG_WARN("回正暂停: 需要左转但左侧有障碍"); - return true; - } - - if (target_angle > 10 && obstacle_analysis_.has_right_critical) - { - publish_speed = 0; - publish_angle = 0; - LOG_WARN("回正暂停: 需要右转但右侧有障碍"); - return true; - } - - // 检查前方是否有新障碍 - if (obstacle_analysis_.has_front_critical) - { - publish_speed = 0; - publish_angle = 0; - LOG_WARN("回正暂停: 前方出现障碍"); - avoid_ctx_.state = AVOID_WAITING; - return true; - } - - return false; - } - - // 绕障安全检查 - bool checkAvoidanceSafety() - { - // 检查绕障方向是否出现新障碍物 - if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) - { - publish_speed = 0; - avoid_ctx_.state = AVOID_WAITING; - LOG_ERROR("左绕过程中左侧出现障碍物！"); - return true; - } - - if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) - { - publish_speed = 0; - avoid_ctx_.state = AVOID_WAITING; - LOG_ERROR("右绕过程中右侧出现障碍物！"); - return true; - } - - return false; + LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f°", state_machine_->getStateString().c_str(), + message.rpm, message.angle); } }; +// ======== 主函数 ======== int main(int argc, char** argv) { - // 初始化日志系统 logger::Logger::Init("fu", "./nodes_log"); rclcpp::init(argc, argv); auto node = std::make_shared("fu_node"); rclcpp::spin(node); rclcpp::shutdown(); - // 关闭日志系统 + logger::Logger::Shutdown(); + return 0; } \ No newline at end of file