From e1ece6e7734cda0e8e2fea40874a5958b68947ec Mon Sep 17 00:00:00 2001 From: Alvin-lyq <2601685812@qq.com> Date: Fri, 31 Oct 2025 15:10:10 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=89=E6=AE=B5=E5=BC=8F=E7=BB=95=E9=9A=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/fu/src/fu_node.cpp | 1639 ++++++++++++++++++++-------------------- 1 file changed, 838 insertions(+), 801 deletions(-) diff --git a/src/fu/src/fu_node.cpp b/src/fu/src/fu_node.cpp index ae8b267..8d0437d 100644 --- a/src/fu/src/fu_node.cpp +++ b/src/fu/src/fu_node.cpp @@ -90,6 +90,9 @@ public: vehicle_min_y_ = -1; vehicle_max_y_ = -1; + // 使用统一的时钟源 + node_clock_ = this->get_clock(); + // 打印所有参数值（添加到构造函数末尾） RCLCPP_INFO_STREAM(this->get_logger(), "\n\n---------- FU节点参数配置 ----------" @@ -107,14 +110,15 @@ public: } private: - // 避障状态 + rclcpp::Clock::SharedPtr node_clock_; + enum AvoidState { - AVOID_NONE, // 无避障 - AVOID_WAITING, // 等待决策 - AVOID_LEFT, // 向左绕障 - AVOID_RIGHT, // 向右绕障 - AVOID_RETURNING // 返回原路径 + AVOID_NONE, // 无绕障 + AVOID_WAITING, // 等待决策 + AVOID_LATERAL_SHIFT, // 阶段1: 横向偏移 + AVOID_PARALLEL_MOVE, // 阶段2: 平行前进 + AVOID_RETURN_TO_PATH // 阶段3: 回归轨迹 }; // 避障状态管理 @@ -123,17 +127,30 @@ private: 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; // 开始绕障时的原始角度 + 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_avoid_time = 1.5; // 最小绕障时间 - double max_avoid_time = 6.0; // 最大绕障时间 + int max_avoid_angle = 25; + double min_lateral_time = 2.0; // 最小横向偏移时间 + double min_parallel_time = 1.5; // 最小平行前进时间 void reset() { @@ -142,16 +159,35 @@ private: 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) - { // 只保留最近20个角度 + { angle_history.erase(angle_history.begin()); } } + + double getElapsedTime(const rclcpp::Time &start, const rclcpp::Time ¤t) const + { + try + { + auto duration = current - start; + return duration.seconds(); + } + catch (const std::runtime_error &e) + { + return 0.0; + } + } }; AvoidanceContext avoid_ctx_; @@ -220,7 +256,7 @@ private: int reliability = 0; // RTK信号质量 1行 0不行 // 发布消息的参数 - int publish_speed = 0; // 转速 0-1000 + int publish_speed = 0; // 转速占空比 0-100 int publish_angle = 0; // [-66.0,66.0]，负数表示左转，正数表示右转 // 车体矩形边界（栅格坐标） @@ -465,7 +501,7 @@ private: vehicle_min_y_ == -1 || vehicle_max_y_ == -1) return; - // 检查前方必停区（优化：按X从近到远检索，找到最近障碍物后立即退出） + // 检查前方必停区 bool found_front_obstacle = false; for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x && !found_front_obstacle; --i) { @@ -475,16 +511,32 @@ private: static_cast(j) < grid[0].size() && grid[i][j] == OBSTACLE) { - obstacle_in_front_ = true; // 标记存在障碍物 + obstacle_in_front_ = true; obstacle_max_x_ = vehicle_min_x_ - i; found_front_obstacle = true; - break; // 退出内层循环 + 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; ++i) + 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) { @@ -495,12 +547,10 @@ private: break; } } - if (obstacle_in_rear_) - break; } // 检查左方必停区 - for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x; ++i) + 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) { @@ -511,12 +561,10 @@ private: break; } } - if (obstacle_in_left_) - break; } // 检查右方必停区 - for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x; ++i) + 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) { @@ -527,19 +575,23 @@ private: break; } } - if (obstacle_in_right_) - 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_ - left_area_extend_)); - size_t front_area_max_y = static_cast(min(static_cast(grid[0].size() - 1), - vehicle_max_y_ + right_area_extend_)); - // RCLCPP_INFO_STREAM(this->get_logger(), - // "绕障检测区域： 前后=" << front_area_max_x - front_area_min_x - // << ", 左右=" << front_area_max_y - front_area_min_y); + // 按照你的优化：只检测车辆宽度范围，避免不必要的绕障 + 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) @@ -548,12 +600,11 @@ private: { obstacle_in_front_area_ = true; - // 更新障碍物边缘 - if (obstacle_left_edge_ == -1 || static_cast(j) < obstacle_left_edge_) - obstacle_left_edge_ = j; - - if (obstacle_right_edge_ == -1 || static_cast(j) > obstacle_right_edge_) - obstacle_right_edge_ = j; + // 更新最左和最右障碍物位置 + 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); @@ -565,18 +616,83 @@ private: } } - // 计算左右可用空间 + // 重新计算可用空间 - 基于更大的绕障检测范围 + 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_) { - free_space_left_ = obstacle_left_edge_ - front_area_min_y; - free_space_right_ = front_area_max_y - obstacle_right_edge_; + 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; // 遇到障碍物，停止计算 + } } - // 如果没有检测到前方障碍物，设置为-1表示无前方障碍物 - if (obstacle_max_x_ == 100 && !obstacle_in_front_ && !obstacle_in_front_area_) - { - obstacle_max_x_ = -1; // -1表示无前方障碍物 - } + return available_space; } void visualizeGridInTerminal(const std::vector> &grid) @@ -648,11 +764,9 @@ private: x = msg->x; y = msg->y; speed = (int)msg->speed; // 获取PL消息中的速度 - std::cout << "pl speed " << speed << std::endl; reliability = msg->reliability; located = msg->enter_range; is_start = msg->is_start; - std::cout << "pl is_start " << is_start << std::endl; } void msg_callback_DetectLine(const sweeper_interfaces::msg::DetectLine::SharedPtr msg) @@ -676,8 +790,14 @@ private: // 发布控制指令 sweeper_interfaces::msg::McCtrl message; - message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫 - message.gear = 2; // 前进挡 + // bool sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫 + bool sweep = true; // 任务开始就开启清扫 + message.edge_brush_lift = sweep; + message.sweep_ctrl = sweep; + message.spray = false; + message.mud_flap = sweep; + message.dust_shake = sweep; + message.gear = 0; message.brake = (publish_speed == 0) ? 1 : 0; if (reliability == 1) @@ -687,21 +807,40 @@ private: message.rpm = 0; RCLCPP_ERROR(this->get_logger(), "rtk信号差,停车！！！"); } + message.angle = publish_angle; // 负数表示左转，正数表示右转 message.angle_speed = 800; pub_mc->publish(message); - RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed << ", 角度=" << publish_angle << ", 状态=" << [&]() - { - switch(avoid_state_){ - case AVOID_NONE: return "无避障"; - case AVOID_WAITING: return "等待决策"; - case AVOID_LEFT: return "向左绕障"; - case AVOID_RIGHT: return "向右绕障"; - case AVOID_RETURNING: return "返回原路径"; - default: return "未知状态"; - } }() << ", 前方" << (obstacle_in_front_ ? "有" : "无") << "障碍物"); + // 改进的状态显示 + 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 = "未知状态"; + } + + RCLCPP_INFO_STREAM(this->get_logger(), + "控制: 速度=" << publish_speed + << ", 角度=" << publish_angle + << ", 状态=" << state_str + << ", 前方" << (obstacle_in_front_ ? "有" : "无") << "障碍物"); } else { @@ -718,7 +857,7 @@ private: float target_angle = calculate_the_angle(x, y); // 初始化控制参数 - publish_speed = std::max(500, std::min(speed, 1000)); // 限制速度范围 + publish_speed = 30; publish_angle = static_cast(target_angle); // 安全检查：未检测到车体位置 @@ -759,6 +898,9 @@ private: 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_; + RCLCPP_INFO(this->get_logger(), "障碍物左侧=%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_; @@ -812,7 +954,7 @@ private: return false; // 检查转向方向的安全性 - if (publish_angle < -20 && obstacle_analysis_.has_left_critical) + if (publish_angle < -10 && obstacle_analysis_.has_left_critical) { publish_speed = 0; publish_angle = 0; @@ -820,7 +962,7 @@ private: return true; } - if (publish_angle > 20 && obstacle_analysis_.has_right_critical) + if (publish_angle > 10 && obstacle_analysis_.has_right_critical) { publish_speed = 0; publish_angle = 0; @@ -831,46 +973,6 @@ private: return false; } - // 第三优先级：智能绕障策略 - void executeSmartAvoidance() - { - if (!enable_obstacle_avoid_ || !obstacle_analysis_.has_front_area) - { - // 无障碍物或绕障功能关闭，执行路径回正 - if (avoid_ctx_.state != AVOID_NONE) - { - executePathReturn(); - } - return; - } - - // 评估是否需要绕障 - if (!obstacle_analysis_.canAvoid()) - { - // 无法绕障，等待或减速 - handleUnpassableObstacle(); - return; - } - - // 状态机处理 - switch (avoid_ctx_.state) - { - case AVOID_NONE: - case AVOID_WAITING: - initiateAvoidance(); - break; - - case AVOID_LEFT: - case AVOID_RIGHT: - executeActiveAvoidance(); - break; - - case AVOID_RETURNING: - executePathReturn(); - break; - } - } - // 处理无法绕行的障碍物 void handleUnpassableObstacle() { @@ -892,7 +994,7 @@ private: else { // 距离足够，可以减速接近 - publish_speed = std::max(500, publish_speed / 2); + publish_speed = std::max(10, publish_speed / 2); RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格", obstacle_analysis_.obstacle_distance); } @@ -901,145 +1003,699 @@ private: // 启动绕障 void initiateAvoidance() { - auto current_time = this->get_clock()->now(); + auto current_time = node_clock_->now(); // 选择最优绕障方向 int direction = selectOptimalDirection(); - avoid_ctx_.state = (direction == -1) ? AVOID_LEFT : AVOID_RIGHT; + // 计算需要的横向偏移距离 + 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; - RCLCPP_INFO(this->get_logger(), "启动绕障：方向=%s，障碍物宽度=%d，左侧空间=%d，右侧空间=%d", + RCLCPP_INFO(this->get_logger(), + "启动绕障: 方向=%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 * 10; - right_score += obstacle_analysis_.free_space_right * 10; + // 1. 可用空间评分(主要因素) + left_score += obstacle_analysis_.free_space_left * 20; + right_score += obstacle_analysis_.free_space_right * 20; - // 2. 必停区安全性评分 + // 2. 安全性评分(关键因素) if (obstacle_analysis_.has_left_critical) - left_score -= 100; + left_score -= 1000; if (obstacle_analysis_.has_right_critical) - right_score -= 100; + right_score -= 1000; - // 3. 目标方向偏好 - float target_angle = calculate_the_angle(x, y); - if (target_angle < -5) - left_score += 15; // 目标偏左 - if (target_angle > 5) - right_score += 15; // 目标偏右 - - // 4. 历史方向稳定性 - 避免频繁切换 - if (!avoid_ctx_.angle_history.empty()) - { - int recent_trend = 0; - int history_size = std::min(5, static_cast(avoid_ctx_.angle_history.size())); - - for (int i = avoid_ctx_.angle_history.size() - history_size; - i < static_cast(avoid_ctx_.angle_history.size()); i++) - { - recent_trend += avoid_ctx_.angle_history[i]; - } - recent_trend /= history_size; - - if (recent_trend < -10) - left_score += 8; // 最近偏左 - if (recent_trend > 10) - right_score += 8; // 最近偏右 - } - - // 5. 障碍物位置偏向性 + // 3. 障碍物位置分析 - 基于车辆边界而非中心 if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) { - int vehicle_center = (vehicle_min_y_ + vehicle_max_y_) / 2; - int obstacle_center = (obstacle_analysis_.left_edge + obstacle_analysis_.right_edge) / 2; + // 计算障碍物与车辆的相对位置 + int vehicle_width = vehicle_max_y_ - vehicle_min_y_ + 1; - if (obstacle_center < vehicle_center) + // 左侧可用空间 = 车辆左边界到障碍物左边界的距离 + int left_clearance = obstacle_analysis_.left_edge - vehicle_min_y_; + // 右侧可用空间 = 障碍物右边界到车辆右边界的距离 + int right_clearance = vehicle_max_y_ - obstacle_analysis_.right_edge; + + RCLCPP_INFO(this->get_logger(), + "位置分析: 左侧间隙=%d, 右侧间隙=%d, 车宽=%d", + left_clearance, right_clearance, vehicle_width); + + // 基于实际间隙大小评分 + if (left_clearance > right_clearance + 2) { - right_score += 5; // 障碍物偏左，倾向于右绕 + left_score += 25; // 左侧明显更宽松 } - else + else if (right_clearance > left_clearance + 2) { - left_score += 5; // 障碍物偏右，倾向于左绕 + right_score += 25; // 右侧明显更宽松 } + + // 如果障碍物严重偏向一侧,反方向绕行 + if (obstacle_analysis_.right_edge < vehicle_min_y_) + { + // 障碍物完全在车辆左侧,应该右绕 + right_score += 30; + RCLCPP_INFO(this->get_logger(), "障碍物在左侧,倾向右绕"); + } + else if (obstacle_analysis_.left_edge > vehicle_max_y_) + { + // 障碍物完全在车辆右侧,应该左绕 + left_score += 30; + RCLCPP_INFO(this->get_logger(), "障碍物在右侧,倾向左绕"); + } + } + + // 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; - RCLCPP_INFO(this->get_logger(), "方向选择评分：左=%d 右=%d，选择%s", - left_score, right_score, (selected_direction == -1) ? "左绕" : "右绕"); + RCLCPP_INFO(this->get_logger(), + "方向选择 - 左评分:%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; } - // 执行主动绕障 - void executeActiveAvoidance() + // 计算需要的横向偏移距离 + int calculateRequiredLateralShift(int direction) { - auto current_time = this->get_clock()->now(); - double avoid_time = (current_time - avoid_ctx_.start_time).seconds(); - - // 动态速度调整 - publish_speed = calculateAvoidanceSpeed(avoid_time); - - // 自适应角度控制 - publish_angle = calculateAvoidanceAngle(avoid_time); - - // 绕障过程中的安全检查 - if (checkAvoidanceSafety()) + if (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.right_edge == -1) { - return; // 检查到危险，已经处理 + return 3; // 默认偏移3格 } - // 判断是否应该结束绕障 - if (shouldEndAvoidance(avoid_time)) + int safety_margin = 2; // 安全余量 + int required_shift = 0; + + if (direction == -1) // 左绕 { - avoid_ctx_.state = AVOID_RETURNING; - avoid_ctx_.last_state_change_time = current_time; - RCLCPP_INFO(this->get_logger(), "绕障完成，开始回正路径"); + // 车体右边界需要绕过障碍物右边界 + 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(); + RCLCPP_INFO(this->get_logger(), "前方无障碍物，重置绕障状态"); + } + return; + } + + // 评估是否需要绕障 + if (!obstacle_analysis_.canAvoid()) + { + RCLCPP_INFO(this->get_logger(), "障碍物无法绕行，减速靠近"); + 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; } } - // 计算绕障速度 - int calculateAvoidanceSpeed(double avoid_time) + // 阶段1: 横向偏移 + void executeLateralShift() { - // 绕障初期降低速度 - if (avoid_time < 1.0) + auto current_time = node_clock_->now(); + double shift_time = 0.0; + + if (avoid_ctx_.last_state_change_time_set) { - return 12; // 起始阶段低速 + shift_time = avoid_ctx_.getElapsedTime(avoid_ctx_.last_state_change_time, current_time); } - else if (avoid_time < 3.0) + + // 速度控制:横向偏移时限速 + publish_speed = 15; + + // 角度控制:保持较大角度进行横向偏移 + int shift_angle = calculateLateralShiftAngle(); + publish_angle = avoid_ctx_.direction * shift_angle; + + // 安全检查 + if (checkAvoidanceSafety()) { - return 18; // 中期稍微提速 + 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_; + + RCLCPP_INFO(this->get_logger(), + "阶段1完成: 横向偏移完成(%.1fs, %d格), 进入阶段2: 平行前进", + shift_time, avoid_ctx_.lateral_shift_achieved); } else { - return 15; // 后期保持稳定 + if (avoid_ctx_.counter % 10 == 0) + { + RCLCPP_INFO(this->get_logger(), + "阶段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++; } - // 计算绕障角度 - 更平滑的角度变化 - int calculateAvoidanceAngle(double avoid_time) + // 验证安全距离 + bool verifySafeDistance() { - // 使用平滑的角度增长曲线 - double progress = std::min(1.0, avoid_time / 2.0); // 2秒内达到最大角度 - double smooth_progress = 0.5 * (1 - cos(M_PI * progress)); // 正弦曲线，更平滑 + // 检查当前是否已经没有前方障碍物警告 + if (obstacle_analysis_.has_front_critical || obstacle_analysis_.has_front_area) + { + RCLCPP_DEBUG(this->get_logger(), "前方仍有障碍区域"); + return false; + } - int target_angle = static_cast(avoid_ctx_.max_avoid_angle * smooth_progress); - return avoid_ctx_.direction * target_angle; + // 检查绕障方向的侧方是否有障碍物 + if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) + { + RCLCPP_DEBUG(this->get_logger(), "左绕时左侧仍有障碍"); + return false; + } + + if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) + { + RCLCPP_DEBUG(this->get_logger(), "右绕时右侧仍有障碍"); + 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_) + { + RCLCPP_DEBUG(this->get_logger(), + "左绕时障碍物左边界%d仍在车右边界%d范围内", + obstacle_analysis_.left_edge, vehicle_max_y_); + return false; + } + } + else // 右绕,障碍物应该在左侧 + { + if (obstacle_analysis_.right_edge >= vehicle_min_y_) + { + RCLCPP_DEBUG(this->get_logger(), + "右绕时障碍物右边界%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) + { + RCLCPP_DEBUG(this->get_logger(), + "横移距离不足: %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) // 右边界 + { + RCLCPP_DEBUG(this->get_logger(), + "左绕未完成: 车右边界%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) + { + RCLCPP_DEBUG(this->get_logger(), + "右绕未完成: 车左边界%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) + { + RCLCPP_DEBUG(this->get_logger(), + "前方第%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) + { + RCLCPP_DEBUG(this->get_logger(), + "右侧仍有障碍[%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) + { + RCLCPP_DEBUG(this->get_logger(), + "左侧仍有障碍[%d,%d],右绕未完全清空", + check_row, check_col); + return false; + } + } + } + } + + RCLCPP_INFO(this->get_logger(), + "横向间隙检查通过: 方向=%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 = 20; + + // 角度控制：回正，保持直行 + 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; + + RCLCPP_INFO(this->get_logger(), + "阶段2完成: 障碍物已在车身后方(%.1fs), 进入阶段3: 回归轨迹", + parallel_time); + } + else + { + if (avoid_ctx_.counter % 10 == 0) + { + RCLCPP_INFO(this->get_logger(), + "阶段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) + { + RCLCPP_DEBUG(this->get_logger(), + "障碍物位置检查: 前方清空=%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; + RCLCPP_ERROR(this->get_logger(), "平行前进时前方出现障碍物！"); + return true; + } + + // 检查绕障侧是否有障碍物（防止剐蹭） + if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + RCLCPP_ERROR(this->get_logger(), "平行前进时左侧出现障碍物！"); + return true; + } + + if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + avoid_ctx_.state = AVOID_WAITING; + RCLCPP_ERROR(this->get_logger(), "平行前进时右侧出现障碍物！"); + 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 = speed; + avoid_ctx_.reset(); + + RCLCPP_INFO(this->get_logger(), + "✓ 回正完成(%.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 = 20; // 回正时保持低速 + + if (avoid_ctx_.counter % 10 == 0) + { + RCLCPP_INFO(this->get_logger(), + "回正中: 当前角度=%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; + RCLCPP_WARN(this->get_logger(), "回正暂停: 需要左转但左侧有障碍"); + return true; + } + + if (target_angle > 10 && obstacle_analysis_.has_right_critical) + { + publish_speed = 0; + publish_angle = 0; + RCLCPP_WARN(this->get_logger(), "回正暂停: 需要右转但右侧有障碍"); + return true; + } + + // 检查前方是否有新障碍 + if (obstacle_analysis_.has_front_critical) + { + publish_speed = 0; + publish_angle = 0; + RCLCPP_WARN(this->get_logger(), "回正暂停: 前方出现障碍"); + avoid_ctx_.state = AVOID_WAITING; + return true; + } + + return false; } // 绕障安全检查 @@ -1064,625 +1720,6 @@ private: return false; } - - // 判断是否应该结束绕障 - bool shouldEndAvoidance(double avoid_time) - { - // 基础时间检查 - if (avoid_time < avoid_ctx_.min_avoid_time) - { - return false; // 未达到最小绕障时间 - } - - // 强制超时保护 - if (avoid_time >= avoid_ctx_.max_avoid_time) - { - RCLCPP_WARN(this->get_logger(), "绕障时间过长(%.1fs)，强制结束", avoid_time); - return true; - } - - // 综合评估绕障完成条件 - AvoidanceCompletionAnalysis completion = analyzeAvoidanceCompletion(avoid_time); - - // 记录评估结果用于调试 - if (avoid_ctx_.counter % 10 == 0) - { // 每1秒打印一次 - RCLCPP_INFO(this->get_logger(), - "绕障评估 - 前方清空:%s, 侧向安全:%s, 角度合理:%s, 位置偏移:%s, 综合评分:%.1f", - completion.front_clear ? "✓" : "✗", - completion.lateral_safe ? "✓" : "✗", - completion.angle_reasonable ? "✓" : "✗", - completion.position_good ? "✓" : "✗", - completion.overall_score); - } - - // 决策逻辑：综合评分达到阈值或满足关键条件 - if (completion.overall_score >= 0.75) - { - RCLCPP_INFO(this->get_logger(), "绕障完成：综合评分%.2f达标", completion.overall_score); - return true; - } - - // 紧急情况：即使评分不够也要结束绕障 - if (completion.emergency_end) - { - RCLCPP_WARN(this->get_logger(), "紧急结束绕障：%s", completion.emergency_reason.c_str()); - return true; - } - - return false; - } - - // 绕障完成度分析结构 - struct AvoidanceCompletionAnalysis - { - bool front_clear = false; // 前方通道清晰 - bool lateral_safe = false; // 侧向安全 - bool angle_reasonable = false; // 当前角度合理 - bool position_good = false; // 位置偏移适中 - bool path_aligned = false; // 路径对齐良好 - bool stability_good = false; // 行驶稳定 - - bool emergency_end = false; // 紧急结束标志 - std::string emergency_reason; // 紧急结束原因 - - double overall_score = 0.0; // 综合评分 [0,1] - - // 各项权重 - static constexpr double WEIGHT_FRONT = 0.3; // 前方清空权重 - static constexpr double WEIGHT_LATERAL = 0.25; // 侧向安全权重 - static constexpr double WEIGHT_ANGLE = 0.2; // 角度合理权重 - static constexpr double WEIGHT_POSITION = 0.15; // 位置权重 - static constexpr double WEIGHT_PATH = 0.1; // 路径对齐权重 - }; - - // 绕障完成度综合分析 - AvoidanceCompletionAnalysis analyzeAvoidanceCompletion(double avoid_time) - { - AvoidanceCompletionAnalysis analysis; - - // 1. 前方通道清晰度分析 - analysis.front_clear = analyzeFrontClearance(); - - // 2. 侧向安全性分析 - analysis.lateral_safe = analyzeLateralSafety(); - - // 3. 当前角度合理性分析 - analysis.angle_reasonable = analyzeAngleReasonability(); - - // 4. 车辆位置偏移分析 - analysis.position_good = analyzePositionOffset(); - - // 5. 路径对齐度分析 - analysis.path_aligned = analyzePathAlignment(); - - // 6. 行驶稳定性分析 - analysis.stability_good = analyzeStability(); - - // 7. 紧急情况检查 - checkEmergencyConditions(analysis, avoid_time); - - // 8. 计算综合评分 - analysis.overall_score = calculateOverallScore(analysis); - - return analysis; - } - - // 方向性通道清晰度分析结构 - struct DirectionalClearance - { - bool can_return_safely = false; // 是否可以安全回正 - int clear_width = 0; // 清晰通道宽度 - int clear_depth = 0; // 清晰通道深度 - std::string analysis_detail; // 分析详情 - }; - - // 前方通道清晰度分析 - bool analyzeFrontClearance() - { - // 方案1：全局前方区域完全清空（最理想情况） - if (!obstacle_analysis_.has_front_area && obstacle_analysis_.obstacle_distance > 5) - { - return true; - } - - // 方案2：智能方向性清晰度检测 - // 如果全局前方有障碍物，但回正方向的通道清晰，也应该允许结束绕障 - DirectionalClearance clearance = analyzeDirectionalClearance(); - - if (clearance.can_return_safely) - { - RCLCPP_INFO(this->get_logger(), - "方向性通道分析：回正方向清晰，允许结束绕障 (清晰宽度:%d格)", - clearance.clear_width); - return true; - } - - return false; - } - - // 分析方向性通道清晰度 - DirectionalClearance analyzeDirectionalClearance() - { - DirectionalClearance result; - - if (vehicle_min_y_ == -1 || vehicle_max_y_ == -1) - { - result.analysis_detail = "无法确定车辆位置"; - return result; - } - - // 计算目标回正角度 - float target_angle = calculate_the_angle(x, y); - int target_angle_int = static_cast(target_angle); - - // 根据目标角度确定主要行驶方向 - // 目标角度：负数=左转，正数=右转，接近0=直行 - if (std::abs(target_angle_int) <= 10) - { - // 直行为主，检查车辆前方中央区域 - result = analyzeForwardClearance(); - } - else if (target_angle_int < -10) - { - // 需要左转，重点检查左前方区域 - result = analyzeLeftwardClearance(); - } - else - { - // 需要右转，重点检查右前方区域 - result = analyzeRightwardClearance(); - } - - // 补充安全检查：即使方向性通道清晰，也要确保有最小安全距离 - if (result.can_return_safely && result.clear_depth < 3) - { - result.can_return_safely = false; - result.analysis_detail += " (但安全距离不足)"; - } - - return result; - } - - // 分析正前方通道（直行场景） - DirectionalClearance analyzeForwardClearance() - { - DirectionalClearance result; - result.analysis_detail = "直行通道分析"; - - // 检查车辆正前方的通道 - int vehicle_center_y = (vehicle_min_y_ + vehicle_max_y_) / 2; - int vehicle_width = vehicle_max_y_ - vehicle_min_y_ + 1; - - // 定义正前方检测区域：车辆宽度 + 2格安全余量 - int check_left = vehicle_center_y - vehicle_width / 2 - 1; - int check_right = vehicle_center_y + vehicle_width / 2 + 1; - int check_depth = std::min(8, front_area_extend_ + 2); // 检查深度 - - result.clear_width = check_right - check_left + 1; - result.clear_depth = checkClearDepthInRangeAccurate(check_left, check_right, check_depth); - - // 如果正前方通道深度足够，认为可以安全直行 - result.can_return_safely = (result.clear_depth >= 4); - - return result; - } - - // 分析左前方通道（左转场景） - DirectionalClearance analyzeLeftwardClearance() - { - DirectionalClearance result; - result.analysis_detail = "左转通道分析"; - - // 重点检查左前方区域是否清晰 - int vehicle_left = vehicle_min_y_; - int check_left = std::max(0, vehicle_left - 2); // 车辆左侧4格范围 - int check_right = vehicle_left + 2; // 延伸到车辆中部 - int check_depth = std::min(6, front_area_extend_); - - result.clear_width = check_right - check_left + 1; - result.clear_depth = checkClearDepthInRangeAccurate(check_left, check_right, check_depth); - - // 左转场景：左前方清晰深度要求可以稍低一些 - result.can_return_safely = (result.clear_depth >= 3) && - !obstacle_analysis_.has_left_critical; - - return result; - } - - // 分析右前方通道（右转场景） - DirectionalClearance analyzeRightwardClearance() - { - DirectionalClearance result; - result.analysis_detail = "右转通道分析"; - - // 重点检查右前方区域是否清晰 - int vehicle_right = vehicle_max_y_; - int check_left = vehicle_right - 2; // 从车辆中部开始 - int check_right = vehicle_right + 2; // 车辆右侧4格范围 - int check_depth = std::min(6, front_area_extend_); - - result.clear_width = check_right - check_left + 1; - result.clear_depth = checkClearDepthInRangeAccurate(check_left, check_right, check_depth); - - // 右转场景：右前方清晰深度要求可以稍低一些 - result.can_return_safely = (result.clear_depth >= 3) && - !obstacle_analysis_.has_right_critical; - - return result; - } - - // 检查指定范围内的清晰深度 - int checkClearDepthInRange(int left_bound, int right_bound, int max_depth) - { - // 简化实现：基于现有的障碍物分析数据进行推断 - if (obstacle_analysis_.obstacle_distance <= 0) - { - return max_depth; // 无障碍物，认为完全清晰 - } - - // 如果有障碍物，检查障碍物是否在我们关心的范围内 - bool obstacle_in_range = false; - if (obstacle_analysis_.left_edge != -1 && obstacle_analysis_.right_edge != -1) - { - // 检查障碍物边缘是否与检测范围重叠 - obstacle_in_range = !(obstacle_analysis_.right_edge < left_bound || - obstacle_analysis_.left_edge > right_bound); - } - - if (!obstacle_in_range) - { - return max_depth; // 障碍物不在检测范围内，认为该方向清晰 - } - - // 障碍物在范围内，返回到障碍物的距离 - return std::max(0, obstacle_analysis_.obstacle_distance - 1); - } - - // 精确的范围清晰度检查（使用缓存的栅格数据） - int checkClearDepthInRangeAccurate(int left_bound, int right_bound, int max_depth) - { - if (!grid_data_valid_ || cached_grid_.empty()) - { - return checkClearDepthInRange(left_bound, right_bound, max_depth); // 回退到简化版本 - } - - // 从车辆前方开始向前检查 - for (int depth = 1; depth <= max_depth; depth++) - { - int check_row = vehicle_min_x_ - depth; - if (check_row < 0) - break; - - // 检查该行的指定列范围 - for (int col = left_bound; col <= right_bound; col++) - { - if (col < 0 || col >= static_cast(cached_grid_[0].size())) - continue; - - if (cached_grid_[check_row][col] == OBSTACLE) - { - return depth - 1; // 遇到障碍物，返回清晰深度 - } - } - } - - return max_depth; // 整个范围都清晰 - } - - // 侧向安全性分析 - bool analyzeLateralSafety() - { - // 基础安全：绕障方向无障碍物 - bool basic_safe = true; - if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) - { - basic_safe = false; - } - if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) - { - basic_safe = false; - } - - if (!basic_safe) - return false; - - // 进阶安全：有足够的侧向安全余量 - int required_clearance = 2; // 需要至少2格的安全间隙 - Claude ： 4格 - bool lateral_clearance_ok = false; - - if (avoid_ctx_.direction == -1) - { - lateral_clearance_ok = obstacle_analysis_.free_space_left >= required_clearance; - } - else - { - lateral_clearance_ok = obstacle_analysis_.free_space_right >= required_clearance; - } - - return lateral_clearance_ok; - } - - // 角度合理性分析 - bool analyzeAngleReasonability() - { - // 当前角度不应该过大 - int current_angle = publish_angle; - if (std::abs(current_angle) > avoid_ctx_.max_avoid_angle + 5) - { - return false; // 角度过大 - } - - // 计算理想回正角度 - float target_angle = calculate_the_angle(x, y); - int angle_diff = std::abs(current_angle - static_cast(target_angle)); - - // 角度差应该在合理范围内 - bool angle_diff_ok = angle_diff <= 25; // 允许25度的偏差 - - // 角度变化应该稳定（不剧烈摆动） - bool angle_stable = true; - if (avoid_ctx_.angle_history.size() >= 3) - { - int recent_changes = 0; - for (size_t i = avoid_ctx_.angle_history.size() - 3; - i < avoid_ctx_.angle_history.size() - 1; i++) - { - int change = std::abs(avoid_ctx_.angle_history[i + 1] - avoid_ctx_.angle_history[i]); - if (change > 10) - recent_changes++; - } - angle_stable = (recent_changes <= 1); // 最多允许1次大幅变化 - } - - return angle_diff_ok && angle_stable; - } - - // 位置偏移分析 - bool analyzePositionOffset() - { - // 分析车辆是否已经偏移到足够安全的位置 - // 这需要基于车辆在栅格中的位置来判断 - - if (vehicle_min_y_ == -1 || vehicle_max_y_ == -1) - { - return false; // 无法确定车辆位置 - } - - // 计算车辆中心位置 - int vehicle_center_y = (vehicle_min_y_ + vehicle_max_y_) / 2; - - // 如果有障碍物，检查车辆是否已经偏移到安全位置 - if (obstacle_analysis_.left_edge != -1 && obstacle_analysis_.right_edge != -1) - { - int obstacle_center = (obstacle_analysis_.left_edge + obstacle_analysis_.right_edge) / 2; - int lateral_offset = std::abs(vehicle_center_y - obstacle_center); - - // 侧向偏移应该足够大 - bool offset_sufficient = lateral_offset >= 5; // 至少5格的偏移 - - // 车辆应该在障碍物的安全一侧 - bool on_safe_side = false; - if (avoid_ctx_.direction == -1) - { - on_safe_side = vehicle_center_y < obstacle_analysis_.left_edge - 2; // 在障碍物左侧 - } - else - { - on_safe_side = vehicle_center_y > obstacle_analysis_.right_edge + 2; // 在障碍物右侧 - } - - return offset_sufficient && on_safe_side; - } - - return true; // 无障碍物时认为位置良好 - } - - // 路径对齐度分析 - bool analyzePathAlignment() - { - float target_angle = calculate_the_angle(x, y); - int current_angle = publish_angle; - - // 检查当前角度是否朝向目标方向 - bool heading_towards_target = std::abs(current_angle - target_angle) <= 30; - - // 检查是否过度绕行（偏离目标太远） - bool not_over_avoiding = std::abs(current_angle) <= 35; - - return heading_towards_target && not_over_avoiding; - } - - // 行驶稳定性分析 - bool analyzeStability() - { - // 检查最近的角度变化是否稳定 - if (avoid_ctx_.angle_history.size() < 5) - { - return true; // 历史数据不足，假设稳定 - } - - // 计算最近5次角度的标准差 - double sum = 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++) - { - sum += avoid_ctx_.angle_history[i]; - } - double mean = sum / count; - - double variance = 0; - for (int i = avoid_ctx_.angle_history.size() - count; - i < static_cast(avoid_ctx_.angle_history.size()); i++) - { - variance += (avoid_ctx_.angle_history[i] - mean) * (avoid_ctx_.angle_history[i] - mean); - } - variance /= count; - double std_dev = sqrt(variance); - - // 标准差小于10度认为稳定 - return std_dev <= 10.0; - } - - // 紧急情况检查 - void checkEmergencyConditions(AvoidanceCompletionAnalysis &analysis, double avoid_time) - { - // 紧急情况1：绕障方向出现新障碍物 - if ((avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) || - (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical)) - { - analysis.emergency_end = true; - analysis.emergency_reason = "绕障方向出现障碍物"; - return; - } - - // 紧急情况2：前方突然出现更危险的障碍物 - if (obstacle_analysis_.has_front_critical && - obstacle_analysis_.obstacle_distance <= 2) - { - analysis.emergency_end = true; - analysis.emergency_reason = "前方出现危险障碍物"; - return; - } - - // 紧急情况3：车辆位置异常（可能传感器故障） - if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1) - { - analysis.emergency_end = true; - analysis.emergency_reason = "车辆位置检测异常"; - return; - } - - // 紧急情况4：角度异常（可能失控） - if (std::abs(publish_angle) > 50) - { - analysis.emergency_end = true; - analysis.emergency_reason = "转向角度异常过大"; - return; - } - - // 紧急情况5：绕障过程中路径目标丢失 - if (x == 0 && y == 0 && avoid_time > 2.0) - { - analysis.emergency_end = true; - analysis.emergency_reason = "路径目标信号丢失"; - return; - } - } - - // 计算综合评分 - double calculateOverallScore(const AvoidanceCompletionAnalysis &analysis) - { - double score = 0.0; - - if (analysis.front_clear) - score += AvoidanceCompletionAnalysis::WEIGHT_FRONT; - if (analysis.lateral_safe) - score += AvoidanceCompletionAnalysis::WEIGHT_LATERAL; - if (analysis.angle_reasonable) - score += AvoidanceCompletionAnalysis::WEIGHT_ANGLE; - if (analysis.position_good) - score += AvoidanceCompletionAnalysis::WEIGHT_POSITION; - if (analysis.path_aligned) - score += AvoidanceCompletionAnalysis::WEIGHT_PATH; - - // 稳定性作为加分项 - if (analysis.stability_good) - score += 0.05; - - return std::min(1.0, score); // 确保不超过1.0 - } - - // 改进的路径回正策略 - void executePathReturn() - { - auto current_time = this->get_clock()->now(); - double return_time = (current_time - avoid_ctx_.last_state_change_time).seconds(); - - // 恢复正常速度 - publish_speed = std::min(1000, speed); - - // 计算目标角度 - float target_angle = calculate_the_angle(x, y); - int target_angle_int = static_cast(target_angle); - - // 转向过程中的安全检查 - if (checkTurnSafety(target_angle_int)) - { - return; // 检查到危险，已处理 - } - - // 平滑回正到目标角度 - int angle_diff = target_angle_int - publish_angle; - int return_rate = calculateReturnRate(return_time, angle_diff); - - if (std::abs(angle_diff) <= return_rate) - { - // 已接近目标角度，完成回正 - publish_angle = target_angle_int; - avoid_ctx_.reset(); - publish_speed = speed; // 恢复原始速度 - RCLCPP_INFO(this->get_logger(), "路径回正完成，恢复正常行驶"); - } - else - { - // 逐步调整到目标角度 - if (angle_diff > 0) - { - publish_angle += return_rate; - } - else - { - publish_angle -= return_rate; - } - } - } - - // 计算回正速率 - int calculateReturnRate(double return_time, int angle_diff) - { - // 基础回正速率 - int base_rate = 3; - - // 角度差越大，回正越快 - if (std::abs(angle_diff) > 30) - { - base_rate = 6; - } - else if (std::abs(angle_diff) > 15) - { - base_rate = 4; - } - - // 时间越长，回正越快 - if (return_time > 3.0) - { - base_rate += 2; - } - - return base_rate; - } - - // 转向安全检查 - bool checkTurnSafety(int target_angle) - { - if (target_angle < -20 && obstacle_analysis_.has_left_critical) - { - publish_speed = 0; - RCLCPP_WARN(this->get_logger(), "回正过程中检测到左侧障碍物"); - return true; - } - - if (target_angle > 20 && obstacle_analysis_.has_right_critical) - { - publish_speed = 0; - RCLCPP_WARN(this->get_logger(), "回正过程中检测到右侧障碍物"); - return true; - } - - return false; - } }; int main(int argc, char **argv)