【一看就会】Autoware.universe的“规划”部分源码梳理【三十五】（motion_velocity_planner第四部分）
文章目录
- 前言
-
- 四、autoware_motion_velocity_out_of_lane_module
-
- 功能概述
- 处理流程图
- 输入输出
- 关键算法实现
- 主要参数配置
-
- 安全参数
- 速度参数
- 检测参数
- 工作流程
- 各文件主要功能
-
- 核心功能文件：
- 工具类文件：
- 源码注释
-
- calculate_slowdown_points.cpp
- filter_predicted_objects.cpp
- footprint.cpp
- debug.cpp
- 总结
前言
书接上文，本篇开始讲解运动速度规划中的第三个模块——autoware_motion_velocity_out_of_lane_module。
四、autoware_motion_velocity_out_of_lane_module
处理车辆偏离车道的情况，计算减速点，确保车辆在安全区域内行驶。
功能概述
负责检测和处理车辆偏离车道的情况
计算减速点和停止点，确保车辆安全行驶
处理车辆与其他车道上障碍物的交互
处理流程图
graph TD
A[输入数据] --> B[车道分析]
B --> C[障碍物处理]
C --> D[轨迹规划]
D --> E[输出结果]
subgraph 车道分析
B1[当前车道检测]
B2[相邻车道分析]
B3[车道边界计算]
end
subgraph 障碍物处理
C1[障碍物过滤]
C2[碰撞风险评估]
C3[减速区间计算]
end
subgraph 轨迹规划
D1[速度规划]
D2[轨迹平滑]
D3[安全检查]
end
输入输出
graph LR
A[输入] --> B[处理] --> C[输出]
subgraph 输入数据
A1[轨迹点]
A2[车道信息]
A3[障碍物信息]
end
subgraph 输出数据
C1[修改后的轨迹]
C2[减速点信息]
C3[调试可视化]
end
关键算法实现
graph TD
A[主要算法] --> B[车道分析算法]
A --> C[碰撞检测算法]
A --> D[减速点计算]
B --> B1[当前车道识别]
B --> B2[相邻车道分析]
C --> C1[空间碰撞检测]
C --> C2[时间碰撞预测]
D --> D1[安全距离计算]
D --> D2[速度曲线生成]
主要参数配置
安全参数
safety_distance: 1.0 # 安全距离
stop_margin: 2.0 # 停止裕度
slowdown_margin: 5.0 # 减速裕度
速度参数
max_slowdown_velocity: 2.0 # 最大减速速度
min_slowdown_velocity: 0.5 # 最小减速速度
检测参数
detection_distance: 30.0 # 检测距离
工作流程
stateDiagram-v2
[*] --> 初始化
初始化 --> 数据接收
数据接收 --> 车道分析
车道分析 --> 障碍物处理
障碍物处理 --> 减速点计算
减速点计算 --> 轨迹生成
轨迹生成 --> 结果输出
结果输出 --> 数据接收
各文件主要功能
核心功能文件：
out_of_lane_module.cpp/hpp: 模块的主要实现，负责整体流程控制
out_of_lane_collisions.cpp/hpp: 处理车道外碰撞检测
lanelets_selection.cpp/hpp: 车道选择和分析
工具类文件：
calculate_slowdown_points.cpp/hpp: 计算减速点位置
filter_predicted_objects.cpp/hpp: 过滤预测的障碍物
footprint.cpp/hpp: 计算车辆占用空间
debug.cpp/hpp: 调试和可视化功能
源码注释
calculate_slowdown_points.cpp
#include "calculate_slowdown_points.hpp"
#include "footprint.hpp"
#include "types.hpp"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
namespace autoware::motion_velocity_planner::out_of_lane
{
/**
* @brief 计算最后一个避免碰撞的姿态。
*
* @param trajectory 自车轨迹点列表。
* @param footprint 对象足迹（多边形）。
* @param polygons_to_avoid 需要避免的多边形集合。
* @param min_arc_length 最小弧长。
* @param max_arc_length 最大弧长。
* @param precision 精度。
* @return std::optional 返回最后一个避免碰撞的姿态，如果没有找到则返回空。
*/
std::optional calculate_last_avoiding_pose(
const std::vector & trajectory,
const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid,
const double min_arc_length, const double max_arc_length, const double precision)
{
geometry_msgs::msg::Pose interpolated_pose{};
bool is_avoiding_pose = false;
auto from = min_arc_length;
auto to = max_arc_length;
while (to - from > precision) {
auto l = from + 0.5 * (to - from);
interpolated_pose = motion_utils::calcInterpolatedPose(trajectory, l);
const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose);
is_avoiding_pose =
std::all_of(polygons_to_avoid.begin(), polygons_to_avoid.end(), [&](const auto & polygon) {
return boost::geometry::disjoint(interpolated_footprint, polygon);
});
if (is_avoiding_pose) {
from = l;
} else {
to = l;
}
}
if (is_avoiding_pose) {
return interpolated_pose;
}
return std::nullopt;
}
/**
* @brief 计算碰撞前的安全姿态。
*
* @param ego_data 自车数据。
* @param point_to_avoid 需要避免的点信息。
* @param footprint 对象足迹（多边形）。
* @param precision 精度。
* @return std::optional 返回碰撞前的安全姿态，如果没有找到则返回空。
*/
std::optional calculate_pose_ahead_of_collision(
const EgoData & ego_data, const OutOfLanePoint & point_to_avoid,
const universe_utils::Polygon2d & footprint, const double precision)
{
const auto first_avoid_arc_length = motion_utils::calcSignedArcLength(
ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index);
for (auto l = first_avoid_arc_length - precision; l >= ego_data.min_stop_arc_length;
l -= precision) {
const auto interpolated_pose =
motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l);
const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose);
if (!boost::geometry::disjoint(interpolated_footprint, point_to_avoid.out_overlaps)) {
return interpolated_pose;
}
}
return std::nullopt;
}
/**
* @brief 计算减速点。
*
* @param ego_data 自车数据。
* @param out_of_lane_data 车道外数据。
* @param params 规划参数。
* @return std::optional 返回减速点姿态，如果没有找到则返回空。
*/
std::optional calculate_slowdown_point(
const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params)
{
// 查找需要避免的第一个点
const auto point_to_avoid_it = std::find_if(
out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(),
[&](const auto & p) { return p.to_avoid; });
if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) {
return std::nullopt;
}
// 创建不同类型的足迹
const auto raw_footprint = make_base_footprint(params, true); // 忽略额外的足迹偏移
const auto base_footprint = make_base_footprint(params);
params.extra_front_offset += params.lon_dist_buffer;
params.extra_right_offset += params.lat_dist_buffer;
params.extra_left_offset += params.lat_dist_buffer;
const auto expanded_footprint = make_base_footprint(params); // 增加距离缓冲区
// 获取需要避免的多边形集合
lanelet::BasicPolygons2d polygons_to_avoid;
for (const auto & ll : point_to_avoid_it->overlapped_lanelets) {
polygons_to_avoid.push_back(ll.polygon2d().basicPolygon());
}
// 获取第一个车道外点的索引和弧长
const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index;
const auto first_outside_arc_length =
motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx);
std::optional slowdown_point;
// 尝试使用扩展足迹（带距离缓冲区和额外足迹偏移）计算最后一个避免碰撞的姿态
for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) {
slowdown_point = calculate_last_avoiding_pose(
ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length,
first_outside_arc_length, params.precision);
if (slowdown_point) {
break;
}
}
// 如果没有找到减速点，则尝试在碰撞前停止
if (!slowdown_point) {
slowdown_point = calculate_pose_ahead_of_collision(
ego_data, *point_to_avoid_it, expanded_footprint, params.precision);
}
return slowdown_point;
}
/**
* @brief 计算最小停车和减速距离。
*
* @param ego_data 自车数据。
* @param planner_data 规划数据。
* @param previous_slowdown_pose 上一个减速点姿态（可选）。
*/
void calculate_min_stop_and_slowdown_distances(
out_of_lane::EgoData & ego_data, const PlannerData & planner_data,
const std::optional & previous_slowdown_pose)
{
// 计算最小停车距离
ego_data.min_stop_distance = planner_data.calculate_min_deceleration_distance(0.0).value_or(0.0);
if (previous_slowdown_pose) {
// 确保不因最小距离限制而移除上一个减速点
const auto previous_slowdown_pose_arc_length = motion_utils::calcSignedArcLength(
ego_data.trajectory_points, 0UL, previous_slowdown_pose->position);
ego_data.min_stop_distance =
std::min(previous_slowdown_pose_arc_length, ego_data.min_stop_distance);
}
// 计算最小停车弧长
ego_data.min_stop_arc_length =
ego_data.longitudinal_offset_to_first_trajectory_index + ego_data.min_stop_distance;
}
} // namespace autoware::motion_velocity_planner::out_of_lane
主要功能说明：
calculate_last_avoiding_pose：通过二分法搜索自车轨迹上的最后一个避免碰撞的姿态。遍历指定范围内的弧长，检查每个插值姿态的足迹是否与需要避免的多边形集合相交，返回最后一个不相交的姿态。
calculate_pose_ahead_of_collision：从碰撞点向前搜索，找到一个安全的姿态。确保该姿态的足迹不会与需要避免的区域重叠。
calculate_slowdown_point：根据自车数据和车道外数据，计算减速点。首先查找需要避免的第一个点，然后尝试使用不同类型的足迹（扩展、基础、原始）计算最后一个避免碰撞的姿态。如果失败，则尝试在碰撞前停止。
calculate_min_stop_and_slowdown_distances：计算最小停车和减速距离。根据规划数据计算最小停车距离，并确保不因最小距离限制而移除上一个减速点。最后计算最小停车弧长。
filter_predicted_objects.cpp
#include "filter_predicted_objects.hpp"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
namespace autoware::motion_velocity_planner::out_of_lane
{
/**
* @brief 在停止线处截断预测路径。
*
* @param predicted_path 预测路径。
* @param stop_line 停止线（线段）。
* @param object_front_overhang 对象前部超出量。
*/
void cut_predicted_path_beyond_line(
autoware_perception_msgs::msg::PredictedPath & predicted_path,
const universe_utils::LineString2d & stop_line, const double object_front_overhang)
{
if (predicted_path.path.empty() || stop_line.size() < 2) return;
auto stop_line_idx = 0UL;
bool found = false;
lanelet::BasicSegment2d path_segment;
path_segment.first.x() = predicted_path.path.front().position.x;
path_segment.first.y() = predicted_path.path.front().position.y;
// 查找与停止线相交的路径段
for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) {
path_segment.second.x() = predicted_path.path[stop_line_idx].position.x;
path_segment.second.y() = predicted_path.path[stop_line_idx].position.y;
if (boost::geometry::intersects(stop_line, path_segment)) {
found = true;
break;
}
path_segment.first = path_segment.second;
}
if (found) {
auto cut_idx = stop_line_idx;
double arc_length = 0;
// 确保保留足够的对象前部超出量
while (cut_idx > 0 && arc_length < object_front_overhang) {
arc_length += universe_utils::calcDistance2d(
predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
--cut_idx;
}
predicted_path.path.resize(cut_idx);
}
}
/**
* @brief 查找下一个停止线。
*
* @param path 预测路径。
* @param ego_data 自车数据。
* @return std::optional 返回找到的最早停止线，如果没有找到则返回空。
*/
std::optional find_next_stop_line(
const autoware_perception_msgs::msg::PredictedPath & path, const EgoData & ego_data)
{
universe_utils::LineString2d query_path;
for (const auto & p : path.path) query_path.emplace_back(p.position.x, p.position.y);
std::vector query_results;
ego_data.stop_lines_rtree.query(
boost::geometry::index::intersects(query_path), std::back_inserter(query_results));
auto earliest_intersecting_index = query_path.size();
std::optional earliest_stop_line;
universe_utils::Segment2d path_segment;
// 查找与路径相交的最早停止线
for (const auto & [_, stop_line] : query_results) {
for (auto index = 0UL; index + 1 < query_path.size(); ++index) {
path_segment.first = query_path[index];
path_segment.second = query_path[index + 1];
if (boost::geometry::intersects(path_segment, stop_line.stop_line)) {
bool within_stop_line_lanelet =
std::any_of(stop_line.lanelets.begin(), stop_line.lanelets.end(), [&](const auto & ll) {
return boost::geometry::within(path_segment.first, ll.polygon2d().basicPolygon());
});
if (within_stop_line_lanelet) {
earliest_intersecting_index = std::min(index, earliest_intersecting_index);
earliest_stop_line = stop_line.stop_line;
}
}
}
}
return earliest_stop_line;
}
/**
* @brief 在红灯处截断预测路径。
*
* @param predicted_path 预测路径。
* @param ego_data 自车数据。
* @param object_front_overhang 对象前部超出量。
*/
void cut_predicted_path_beyond_red_lights(
autoware_perception_msgs::msg::PredictedPath & predicted_path, const EgoData & ego_data,
const double object_front_overhang)
{
const auto stop_line = find_next_stop_line(predicted_path, ego_data);
if (stop_line) {
cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
}
}
/**
* @brief 过滤预测对象。
*
* @param planner_data 规划数据。
* @param ego_data 自车数据。
* @param params 规划参数。
* @return autoware_perception_msgs::msg::PredictedObjects 返回过滤后的预测对象列表。
*/
autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params)
{
autoware_perception_msgs::msg::PredictedObjects filtered_objects;
filtered_objects.header = planner_data.predicted_objects.header;
for (const auto & object : planner_data.predicted_objects.objects) {
// 跳过行人
const auto is_pedestrian =
std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
}) != object.classification.end();
if (is_pedestrian) continue;
// 跳过来自后方的对象
const auto is_coming_from_behind =
motion_utils::calcSignedArcLength(
ego_data.trajectory_points, 0UL,
object.kinematics.initial_pose_with_covariance.pose.position) < 0.0;
if (params.objects_ignore_behind_ego && is_coming_from_behind) {
continue;
}
auto filtered_object = object;
// 移除无效的预测路径
const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path);
if (no_overlap_path.size() <= 1) return true;
const auto lat_offset_to_current_ego =
std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position));
const auto is_crossing_ego =
lat_offset_to_current_ego <=
object.shape.dimensions.y / 2.0 + std::max(
params.left_offset + params.extra_left_offset,
params.right_offset + params.extra_right_offset);
return is_low_confidence || is_crossing_ego;
};
auto & predicted_paths = filtered_object.kinematics.predicted_paths;
const auto new_end =
std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
predicted_paths.erase(new_end, predicted_paths.end());
// 在红灯处截断预测路径
if (params.objects_cut_predicted_paths_beyond_red_lights)
for (auto & predicted_path : predicted_paths)
cut_predicted_path_beyond_red_lights(
predicted_path, ego_data, filtered_object.shape.dimensions.x);
// 移除空的预测路径
predicted_paths.erase(
std::remove_if(
predicted_paths.begin(), predicted_paths.end(),
[](const auto & p) { return p.path.empty(); }),
predicted_paths.end());
// 如果有有效的预测路径，则保留该对象
if (!filtered_object.kinematics.predicted_paths.empty())
filtered_objects.objects.push_back(filtered_object);
}
return filtered_objects;
}
} // namespace autoware::motion_velocity_planner::out_of_lane
主要功能说明：
cut_predicted_path_beyond_line：在指定的停止线处截断预测路径。遍历路径段，查找与停止线相交的点，并根据对象前部超出量调整截断位置。
find_next_stop_line：查找预测路径中最早遇到的停止线。通过查询 RTree 索引，找到与路径相交的最早停止线，并确保该停止线位于车道内。
cut_predicted_path_beyond_red_lights：在红灯处截断预测路径。调用 find_next_stop_line 查找下一个停止线，并使用 cut_predicted_path_beyond_line 截断路径。
filter_predicted_objects：过滤预测对象。主要步骤包括：
跳过行人。
跳过来自后方的对象（如果配置允许）。
移除低置信度或与自车轨迹交叉的预测路径。
在红灯处截断预测路径（如果配置允许）。
移除空的预测路径。
保留有有效预测路径的对象。
footprint.cpp
#include "footprint.hpp"
#include
#include
#include
#include
#include
#include
namespace autoware::motion_velocity_planner::out_of_lane
{
/**
* @brief 创建基础足迹。
*
* @param p 规划参数。
* @param ignore_offset 是否忽略额外偏移量。
* @return universe_utils::Polygon2d 返回创建的基础足迹多边形。
*/
universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool ignore_offset)
{
universe_utils::Polygon2d base_footprint;
const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset;
const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset;
const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset;
const auto left_offset = ignore_offset ? 0.0 : p.extra_left_offset;
// 构建多边形，表示对象的基础足迹
base_footprint.outer() = {
{p.front_offset + front_offset, p.left_offset + left_offset},
{p.front_offset + front_offset, p.right_offset - right_offset},
{p.rear_offset - rear_offset, p.right_offset - right_offset},
{p.rear_offset - rear_offset, p.left_offset + left_offset}
};
// 确保多边形是有效的（例如，顶点顺序正确）
boost::geometry::correct(base_footprint);
return base_footprint;
}
/**
* @brief 将基础足迹投影到给定姿态。
*
* @param base_footprint 基础足迹（多边形）。
* @param pose 对象的姿态。
* @return lanelet::BasicPolygon2d 返回投影后的足迹多边形。
*/
lanelet::BasicPolygon2d project_to_pose(
const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose)
{
const auto angle = tf2::getYaw(pose.orientation);
const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle);
lanelet::BasicPolygon2d footprint;
// 将旋转后的足迹平移到指定位置
for (const auto & p : rotated_footprint.outer())
footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y);
return footprint;
}
/**
* @brief 计算自车轨迹上的足迹集合。
*
* @param ego_data 自车数据。
* @param params 规划参数。
* @return std::vector 返回轨迹上每个点的足迹多边形列表。
*/
std::vector calculate_trajectory_footprints(
const EgoData & ego_data, const PlannerParam & params)
{
const auto base_footprint = make_base_footprint(params);
std::vector trajectory_footprints;
trajectory_footprints.reserve(ego_data.trajectory_points.size());
// 遍历自车轨迹上的每个点，计算其足迹
for (auto i = 0UL; i < ego_data.trajectory_points.size(); ++i) {
const auto & trajectory_pose = ego_data.trajectory_points[i].pose;
const auto angle = tf2::getYaw(trajectory_pose.orientation);
const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle);
lanelet::BasicPolygon2d footprint;
// 将旋转后的足迹平移到指定位置
for (const auto & p : rotated_footprint.outer())
footprint.emplace_back(
p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y);
trajectory_footprints.push_back(footprint);
}
return trajectory_footprints;
}
/**
* @brief 计算当前自车足迹。
*
* @param ego_data 自车数据。
* @param params 规划参数。
* @param ignore_offset 是否忽略额外偏移量。
* @return lanelet::BasicPolygon2d 返回当前自车足迹多边形。
*/
lanelet::BasicPolygon2d calculate_current_ego_footprint(
const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset)
{
const auto base_footprint = make_base_footprint(params, ignore_offset);
const auto angle = tf2::getYaw(ego_data.pose.orientation);
const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle);
lanelet::BasicPolygon2d footprint;
// 将旋转后的足迹平移到指定位置
for (const auto & p : rotated_footprint.outer())
footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y);
return footprint;
}
} // namespace autoware::motion_velocity_planner::out_of_lane
主要功能说明：
make_base_footprint：根据规划参数创建一个基础足迹多边形。可以选择是否忽略额外的偏移量。构建一个多边形来表示对象的基础足迹，并确保该多边形是有效的。
project_to_pose：将一个基础足迹（多边形）投影到给定的姿态。首先根据姿态的角度旋转足迹，然后将其平移到指定的位置。
calculate_trajectory_footprints：计算自车轨迹上每个点的足迹集合。遍历自车轨迹上的每个点，调用 make_base_footprint 和 project_to_pose 来生成并投影其足迹。
calculate_current_ego_footprint：计算当前自车的足迹。根据当前自车的姿态和规划参数，调用 make_base_footprint 和 project_to_pose 来生成并投影当前自车的足迹。
debug.cpp
#include "debug.hpp"
#include "types.hpp"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
namespace autoware::motion_velocity_planner::out_of_lane::debug
{
namespace
{
/**
* @brief 创建基础标记。
*
* @return visualization_msgs::msg::Marker 返回初始化的基础标记。
*/
visualization_msgs::msg::Marker get_base_marker()
{
visualization_msgs::msg::Marker base_marker;
base_marker.header.frame_id = "map";
base_marker.header.stamp = rclcpp::Time(0);
base_marker.id = 0;
base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
base_marker.action = visualization_msgs::msg::Marker::ADD;
base_marker.pose.position = universe_utils::createMarkerPosition(0.0, 0.0, 0);
base_marker.pose.orientation = universe_utils::createMarkerOrientation(0, 0, 0, 1.0);
base_marker.scale = universe_utils::createMarkerScale(0.1, 0.1, 0.1);
base_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5);
return base_marker;
}
/**
* @brief 添加多边形标记到 MarkerArray。
*
* @param debug_marker_array MarkerArray 引用。
* @param base_marker 基础标记。
* @param polygons 多边形集合。
*/
void add_polygons_markers(
visualization_msgs::msg::MarkerArray & debug_marker_array,
const visualization_msgs::msg::Marker & base_marker, const lanelet::BasicPolygons2d & polygons)
{
auto debug_marker = base_marker;
debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
// 遍历每个多边形，添加其线段到标记中
for (const auto & f : polygons) {
boost::geometry::for_each_segment(f, [&](const auto & s) {
const auto & [p1, p2] = s;
debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), 0.0));
debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), 0.0));
});
}
debug_marker_array.markers.push_back(debug_marker);
}
/**
* @brief 添加当前重叠区域标记。
*
* @param debug_marker_array MarkerArray 引用。
* @param current_footprint 当前足迹（多边形）。
* @param z 标记的高度（z 坐标）。
*/
void add_current_overlap_marker(
visualization_msgs::msg::MarkerArray & debug_marker_array,
const lanelet::BasicPolygon2d & current_footprint, const double z)
{
auto debug_marker = get_base_marker();
debug_marker.ns = "current_overlap";
debug_marker.points.clear();
// 添加多边形顶点到标记中，并闭合多边形
for (const auto & p : current_footprint)
debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z));
if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front());
debug_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5);
debug_marker_array.markers.push_back(debug_marker);
debug_marker.id++;
}
/**
* @brief 添加 TTC（时间到碰撞）标记。
*
* @param debug_marker_array MarkerArray 引用。
* @param ego_data 自车数据。
* @param out_of_lane_data 车道外数据。
* @param prev_nb 上次标记的数量。
*/
void add_ttc_markers(
visualization_msgs::msg::MarkerArray & debug_marker_array, const EgoData & ego_data,
const OutOfLaneData & out_of_lane_data, const size_t prev_nb)
{
auto debug_marker = get_base_marker();
debug_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
debug_marker.scale.z = 0.5;
debug_marker.ns = "ttcs";
// 遍历车道外点，添加 TTC 文本标记
for (const auto & p : out_of_lane_data.outside_points) {
if (p.to_avoid) {
debug_marker.color.r = 1.0;
debug_marker.color.g = 0.0;
} else {
debug_marker.color.r = 0.0;
debug_marker.color.g = 1.0;
}
if (p.ttc) {
debug_marker.pose = ego_data.trajectory_points[p.trajectory_index].pose;
debug_marker.text = std::to_string(*p.ttc);
debug_marker_array.markers.push_back(debug_marker);
debug_marker.id++;
}
}
// 删除多余的旧标记
debug_marker.action = visualization_msgs::msg::Marker::DELETE;
for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) {
debug_marker_array.markers.push_back(debug_marker);
}
}
/**
* @brief 添加停止线标记。
*
* @param debug_marker_array MarkerArray 引用。
* @param rtree 停止线 RTree 索引。
* @param z 标记的高度（z 坐标）。
* @param prev_nb 上次标记的数量。
* @return size_t 返回最大 ID。
*/
size_t add_stop_line_markers(
visualization_msgs::msg::MarkerArray & debug_marker_array, const StopLinesRtree & rtree,
const double z, const size_t prev_nb)
{
auto debug_marker = get_base_marker();
debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
debug_marker.ns = "stop_lines";
// 添加停止线和相关车道多边形的标记
const auto & add_lanelets_markers = [&](const auto & lanelets) {
for (const auto & ll : lanelets) {
debug_marker.points.clear();
for (const auto & p : ll.polygon2d().basicPolygon()) {
debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5));
}
debug_marker.points.push_back(debug_marker.points.front());
debug_marker_array.markers.push_back(debug_marker);
++debug_marker.id;
}
};
for (const auto & [_, stop_line] : rtree) {
debug_marker.points.clear();
debug_marker.color.r = 1.0;
for (const auto & p : stop_line.stop_line) {
debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5));
}
debug_marker_array.markers.push_back(debug_marker);
++debug_marker.id;
debug_marker.color.r = 0.0;
add_lanelets_markers(stop_line.lanelets);
}
// 删除多余的旧标记
const auto max_id = debug_marker.id;
debug_marker.action = visualization_msgs::msg::Marker::DELETE;
for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) {
debug_marker_array.markers.push_back(debug_marker);
}
return max_id;
}
/**
* @brief 添加车道外区域标记。
*
* @param marker_array MarkerArray 引用。
* @param base_marker 基础标记。
* @param out_lanelets 车道外车道集合。
*/
void add_out_lanelets(
visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker,
const lanelet::ConstLanelets & out_lanelets)
{
lanelet::BasicPolygons2d drivable_lane_polygons;
for (const auto & ll : out_lanelets) {
drivable_lane_polygons.push_back(ll.polygon2d().basicPolygon());
}
base_marker.ns = "out_lanelets";
base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0);
add_polygons_markers(marker_array, base_marker, drivable_lane_polygons);
}
/**
* @brief 添加车道外重叠区域标记。
*
* @param marker_array MarkerArray 引用。
* @param base_marker 基础标记。
* @param outside_points 车道外点列表。
* @param trajectory 自车轨迹点列表。
*/
void add_out_of_lane_overlaps(
visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker,
const std::vector & outside_points,
const std::vector & trajectory)
{
lanelet::BasicPolygons2d out_of_lane_overlaps;
lanelet::BasicPolygon2d out_of_lane_overlap;
// 添加车道外重叠区域的多边形标记
for (const auto & p : outside_points) {
for (const auto & overlap : p.out_overlaps) {
boost::geometry::convert(overlap, out_of_lane_overlap);
out_of_lane_overlaps.push_back(out_of_lane_overlap);
}
}
base_marker.ns = "out_of_lane_areas";
base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0);
add_polygons_markers(marker_array, base_marker, out_of_lane_overlaps);
// 添加重叠区域中心点到轨迹点的连线
for (const auto & p : outside_points) {
for (const auto & a : p.out_overlaps) {
marker_array.markers.back().points.push_back(trajectory[p.trajectory_index].pose.position);
const auto centroid = boost::geometry::return_centroid(a);
marker_array.markers.back().points.push_back(
geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y()));
}
}
}
/**
* @brief 添加预测路径标记。
*
* @param marker_array MarkerArray 引用。
* @param base_marker 基础标记。
* @param objects 预测对象列表。
* @param ego_pose 自车姿态。
*/
void add_predicted_paths(
visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker,
const autoware_perception_msgs::msg::PredictedObjects & objects,
const geometry_msgs::msg::Pose & ego_pose)
{
base_marker.ns = "objects";
base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0);
lanelet::BasicPolygons2d object_polygons;
constexpr double max_draw_distance = 50.0;
// 添加预测路径上的对象足迹标记
for (const auto & o : objects.objects) {
for (const auto & path : o.kinematics.predicted_paths) {
for (const auto & pose : path.path) {
// 限制绘制距离以提高性能
if (universe_utils::calcDistance2d(pose, ego_pose) < max_draw_distance) {
const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer();
lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end());
object_polygons.push_back(ll_poly);
}
}
}
}
add_polygons_markers(marker_array, base_marker, object_polygons);
}
} // namespace
/**
* @brief 创建调试标记数组。
*
* @param ego_data 自车数据。
* @param out_of_lane_data 车道外数据。
* @param objects 预测对象列表。
* @param debug_data 调试数据。
* @return visualization_msgs::msg::MarkerArray 返回包含所有调试标记的 MarkerArray。
*/
visualization_msgs::msg::MarkerArray create_debug_marker_array(
const EgoData & ego_data, const OutOfLaneData & out_of_lane_data,
const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data)
{
const auto z = ego_data.pose.position.z;
visualization_msgs::msg::MarkerArray debug_marker_array;
auto base_marker = get_base_marker();
base_marker.pose.position.z = z + 0.5;
base_marker.ns = "footprints";
base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0);
// 添加各种调试标记
add_out_lanelets(debug_marker_array, base_marker, ego_data.out_lanelets);
add_out_of_lane_overlaps(debug_marker_array, base_marker, out_of_lane_data.outside_points, ego_data.trajectory_points);
add_predicted_paths(debug_marker_array, base_marker, objects, ego_data.pose);
add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z);
add_ttc_markers(debug_marker_array, ego_data, out_of_lane_data, debug_data.prev_ttcs);
debug_data.prev_ttcs = out_of_lane_data.outside_points.size();
debug_data.prev_stop_line = add_stop_line_markers(
debug_marker_array, ego_data.stop_lines_rtree, z, debug_data.prev_stop_line);
return debug_marker_array;
}
/**
* @brief 创建虚拟墙。
*
* @param pose 自车姿态。
* @param stop 是否为停止标志。
* @param params 规划参数。
* @return motion_utils::VirtualWalls 返回创建的虚拟墙列表。
*/
motion_utils::VirtualWalls create_virtual_walls(
const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params)
{
motion_utils::VirtualWalls virtual_walls;
motion_utils::VirtualWall wall;
wall.text = "out_of_lane";
wall.longitudinal_offset = params.front_offset;
wall.style = stop ? motion_utils::VirtualWallType::stop : motion_utils::VirtualWallType::slowdown;
wall.pose = pose;
virtual_walls.push_back(wall);
return virtual_walls;
}
} // namespace autoware::motion_velocity_planner::out_of_lane::debug
主要功能说明：
get_base_marker：创建一个基础标记，用于后续标记的模板。
add_polygons_markers：将多边形集合添加到 MarkerArray 中，形成线段列表标记。
add_current_overlap_marker：添加当前重叠区域的多边形标记，并确保多边形闭合。
add_ttc_markers：添加 TTC（时间到碰撞）文本标记，显示每个车道外点的时间到碰撞值，并删除多余的旧标记。
add_stop_line_markers：添加停止线和相关车道多边形的标记，并删除多余的旧标记。
add_out_lanelets：添加车道外车道的多边形标记。
add_out_of_lane_overlaps：添加车道外重叠区域的多边形标记，并在重叠区域中心点和轨迹点之间添加连线。
add_predicted_paths：添加预测路径上的对象足迹标记，限制绘制距离以提高性能。
create_debug_marker_array：创建包含所有调试标记的 MarkerArray。调用上述函数添加各种调试标记，如车道外区域、预测路径、TTC 和停止线等。
create_virtual_walls：根据给定的姿态和规划参数创建虚拟墙，用于模拟停止或减速行为。
总结
无。