【一看就会】Autoware.universe的“规划”部分源码梳理【四十六】（autoware_obstacle_cruise_planner：障碍物巡航规划器）
提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
文章目录
- 前言
- 十二、autoware_obstacle_cruise_planner：障碍物巡航规划器
-
- 1.功能概述
- 2.工作流程
- 3.调用关系
- 4.订阅发布话题
-
- 订阅话题
- 发布话题
- 5.关键算法实现
- 6.主要参数配置
- 规划器选择
- 安全参数
- 优化器参数
- PID参数
- 巡航参数
-
- 7.文件结构和功能
-
- a) 核心实现文件
-
- node.cpp：
- planner_interface.cpp：
- b) 规划器实现
-
- optimization_based_planner/optimization_based_planner.cpp：
- optimization_based_planner/velocity_optimizer.cpp：
- pid_based_planner/pid_based_planner.cpp：
- c) 工具类
-
- polygon_utils.cpp：
- utils.cpp：
- 8.源码注释
-
- node.cpp
- planner_interface.cpp
- optimization_based_planner.cpp
- velocity_optimizer.cpp
- pid_based_planner.cpp
- polygon_utils.cpp
- utils.cpp
- 总结
前言
书接上文，本篇讲述planning这边的第十二部分——autoware_obstacle_cruise_planner：障碍物巡航规划器
十二、autoware_obstacle_cruise_planner：障碍物巡航规划器
1.功能概述
autoware_obstacle_cruise_planner是一个用于处理巡航过程中障碍物的规划器，主要功能：
根据前方障碍物调整巡航速度
实现自适应巡航控制(ACC)
保持安全跟车距离
处理动态和静态障碍物
2.工作流程
graph TD
A[接收输入数据] --> B[检测障碍物]
B --> C{有障碍物?}
C -->|是| D[选择规划器]
D --> E[生成速度轨迹]
E --> F[安全检查]
F --> G[发布轨迹]
C -->|否| H[保持巡航速度]
H --> G
3.调用关系
被behavior_velocity_planner调用
与obstacle_stop_planner协作
为motion_velocity_smoother提供输入
4.订阅发布话题
订阅话题
// 输入话题
- /perception/object_recognition/objects // 障碍物信息
- /planning/scenario_planning/trajectory // 规划轨迹
- /localization/current_pose // 当前位置
- /vehicle/status/velocity // 车辆速度
发布话题
// 输出话题
- /planning/obstacle_cruise_planner/trajectory // 巡航轨迹
- /debug/cruise_status // 调试信息
5.关键算法实现
class ObstacleCruisePlanner {
// 主规划流程
TrajectoryPoints plan(const PlannerInput& input) {
// 1. 检测前方障碍物
auto obstacles = detectObstacles(input);
// 2. 选择规划器
auto planner = selectPlanner(obstacles);
// 3. 生成速度轨迹
auto trajectory = planner->generateTrajectory(input, obstacles);
// 4. 安全检查
if (!validateTrajectory(trajectory)) {
trajectory = generateEmergencyTrajectory();
}
return trajectory;
}
private:
// 基于优化的速度规划
TrajectoryPoints optimizationBasedPlanning() {
// 1. 构建优化问题
OptimizationProblem problem;
// 2. 设置约束条件
setConstraints(problem);
// 3. 设置目标函数
setObjectives(problem);
// 4. 求解优化问题
return solveProblem(problem);
}
// 基于PID的速度规划
TrajectoryPoints pidBasedPlanning() {
// 1. 计算目标距离
double target_distance = calculateTargetDistance();
// 2. 计算速度误差
double velocity_error = calculateVelocityError();
// 3. PID控制
return generatePIDTrajectory(velocity_error);
}
};
6.主要参数配置
obstacle_cruise_planner:
规划器选择
planner_type: “optimization” # or “pid”
安全参数
safety:
min_distance: 5.0
time_margin: 2.0
优化器参数
optimization:
max_iterations: 100
time_horizon: 10.0
PID参数
pid:
kp: 1.0
ki: 0.1
kd: 0.01
巡航参数
cruise:
target_velocity: 30.0
min_velocity: 0.0
max_acceleration: 2.0
max_deceleration: -3.0
7.文件结构和功能
a) 核心实现文件
node.cpp：
ROS2节点实现
管理规划流程
处理消息订阅和发布
planner_interface.cpp：
规划器接口定义
抽象基类实现
通用功能封装
b) 规划器实现
optimization_based_planner/optimization_based_planner.cpp：
基于优化的规划器实现
处理复杂场景
优化速度轨迹
optimization_based_planner/velocity_optimizer.cpp：
速度优化器实现
生成最优速度曲线
处理约束条件
pid_based_planner/pid_based_planner.cpp：
基于PID的规划器实现
简单场景处理
实时响应控制
c) 工具类
polygon_utils.cpp：
多边形处理工具
碰撞检测功能
几何计算方法
utils.cpp：
通用工具函数
数据处理方法
辅助计算功能
8.源码注释
node.cpp
// Copyright 2022 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "autoware/obstacle_cruise_planner/node.hpp" // 障碍物巡航规划器节点头文件
#include "autoware/motion_utils/resample/resample.hpp" // 重采样工具
#include "autoware/motion_utils/trajectory/conversion.hpp" // 轨迹转换工具
#include "autoware/object_recognition_utils/predicted_path_utils.hpp" // 预测路径工具
#include "autoware/obstacle_cruise_planner/polygon_utils.hpp" // 多边形工具
#include "autoware/obstacle_cruise_planner/utils.hpp" // 工具函数
#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" // Boost几何工具
#include "autoware/universe_utils/ros/marker_helper.hpp" // ROS标记帮助工具
#include "autoware/universe_utils/ros/update_param.hpp" // ROS参数更新工具
#include // PCL ROS转换工具
#include // TF2与Eigen的转换工具
#include // PCL体素网格滤波器
#include // PCL聚类提取
#include // PCL与ROS消息转换
#include // 标准库算法
#include // 时间相关功能
namespace
{
// 创建速度限制清除命令消息
VelocityLimitClearCommand createVelocityLimitClearCommandMessage(
const rclcpp::Time & current_time, const std::string & module_name)
{
VelocityLimitClearCommand msg; // 创建速度限制清除命令消息
msg.stamp = current_time; // 设置时间戳
msg.sender = "obstacle_cruise_planner." + module_name; // 设置发送者
msg.command = true; // 设置命令为真
return msg; // 返回消息
}
// 从UUID获取障碍物
template
std::optional getObstacleFromUuid(
const std::vector & obstacles, const std::string & target_uuid)
{
const auto itr = std::find_if(obstacles.begin(), obstacles.end(), [&](const auto & obstacle) {
return obstacle.uuid == target_uuid; // 查找UUID匹配的障碍物
});
if (itr == obstacles.end()) {
return std::nullopt; // 如果未找到，返回空值
}
return *itr; // 返回找到的障碍物
}
// 计算前方车辆的距离
std::optional calcDistanceToFrontVehicle(
const std::vector & traj_points, const size_t ego_idx,
const geometry_msgs::msg::Point & obstacle_pos)
{
const size_t obstacle_idx = autoware::motion_utils::findNearestIndex(traj_points, obstacle_pos); // 查找障碍物在轨迹中的索引
const auto ego_to_obstacle_distance =
autoware::motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx); // 计算自车到障碍物的距离
if (ego_to_obstacle_distance < 0.0) return std::nullopt; // 如果距离小于零，返回空值
return ego_to_obstacle_distance; // 返回距离
}
// 重采样最高置信度的预测路径
std::vector resampleHighestConfidencePredictedPaths(
const std::vector & predicted_paths, const double time_interval,
const double time_horizon, const size_t num_paths)
{
std::vector sorted_paths = predicted_paths; // 复制预测路径
// 按置信度降序排序路径
std::sort(
sorted_paths.begin(), sorted_paths.end(),
[](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; });
std::vector selected_paths; // 存储选择的路径
size_t path_count = 0; // 路径计数
// 选择满足置信度阈值的路径
for (const auto & path : sorted_paths) {
if (path_count < num_paths) {
selected_paths.push_back(path); // 添加路径到选择列表
++path_count; // 增加计数
}
}
// 重采样每个选择的路径
std::vector resampled_paths;
for (const auto & path : selected_paths) {
if (path.path.size() < 2) {
continue; // 如果路径点少于2个，跳过
}
resampled_paths.push_back(
autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); // 重采样路径
}
return resampled_paths; // 返回重采样后的路径
}
// 计算与轨迹的角度差
double calcDiffAngleAgainstTrajectory(
const std::vector & traj_points, const geometry_msgs::msg::Pose & target_pose)
{
const size_t nearest_idx =
autoware::motion_utils::findNearestIndex(traj_points, target_pose.position); // 找到最近的轨迹点索引
const double traj_yaw = tf2::getYaw(traj_points.at(nearest_idx).pose.orientation); // 获取轨迹点的偏航角
const double target_yaw = tf2::getYaw(target_pose.orientation); // 获取目标姿态的偏航角
const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); // 计算偏航角差
return diff_yaw; // 返回偏航角差
}
/**
* @brief 计算障碍物相对于轨迹的纵向和接近速度。
*
* 此函数计算障碍物相对于轨迹的速度分量。
* 它返回障碍物相对于轨迹的纵向和接近速度分量。负的接近速度表示障碍物正在远离轨迹。
*
* @param traj_points 轨迹点。
* @param obstacle_pose 障碍物的当前位置。
* @param obstacle_twist 障碍物的速度。
* @return 包含纵向和接近速度分量的对。
*/
std::pair calculateObstacleVelocitiesRelativeToTrajectory(
const std::vector & traj_points, const geometry_msgs::msg::Pose & obstacle_pose,
const geometry_msgs::msg::Twist & obstacle_twist)
{
const size_t object_idx =
autoware::motion_utils::findNearestIndex(traj_points, obstacle_pose.position); // 找到障碍物在轨迹中的索引
const auto & nearest_point = traj_points.at(object_idx); // 获取最近的轨迹点
const double traj_yaw = tf2::getYaw(nearest_point.pose.orientation); // 获取轨迹点的偏航角
const double obstacle_yaw = tf2::getYaw(obstacle_pose.orientation); // 获取障碍物的偏航角
const Eigen::Rotation2Dd R_ego_to_obstacle(
autoware::universe_utils::normalizeRadian(obstacle_yaw - traj_yaw)); // 计算障碍物相对于轨迹的旋转
// 计算轨迹方向和从轨迹到障碍物的向量
const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); // 轨迹方向向量
const Eigen::Vector2d traj_to_obstacle(
obstacle_pose.position.x - nearest_point.pose.position.x,
obstacle_pose.position.y - nearest_point.pose.position.y); // 从轨迹到障碍物的向量
// 使用叉积判断障碍物在轨迹的左侧还是右侧
const double cross_product =
traj_direction.x() * traj_to_obstacle.y() - traj_direction.y() * traj_to_obstacle.x();
const int sign = (cross_product > 0) ? -1 : 1; // 根据叉积的符号确定方向
const Eigen::Vector2d obstacle_velocity(obstacle_twist.linear.x, obstacle_twist.linear.y); // 障碍物速度向量
const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; // 将障碍物速度投影到轨迹方向
return std::make_pair(projected_velocity[0], sign * projected_velocity[1]); // 返回纵向和接近速度分量
}
// 计算障碍物的最大长度
double calcObstacleMaxLength(const Shape & shape)
{
if (shape.type == Shape::BOUNDING_BOX) {
return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); // 计算边界框对角线长度
} else if (shape.type == Shape::CYLINDER) {
return shape.dimensions.x / 2.0; // 计算圆柱的半径
} else if (shape.type == Shape::POLYGON) {
double max_length_to_point = 0.0; // 最大长度
for (const auto rel_point : shape.footprint.points) {
const double length_to_point = std::hypot(rel_point.x, rel_point.y); // 计算到点的距离
if (max_length_to_point < length_to_point) {
max_length_to_point = length_to_point; // 更新最大长度
}
}
return max_length_to_point; // 返回最大长度
}
throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); // 抛出异常
}
// 获取扩展轨迹点
TrajectoryPoint getExtendTrajectoryPoint(
const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward)
{
TrajectoryPoint extend_trajectory_point; // 创建扩展轨迹点
extend_trajectory_point.pose = autoware::universe_utils::calcOffsetPose(
goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); // 计算扩展轨迹点的位置
extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; // 设置纵向速度
extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; // 设置横向速度
extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; // 设置加速度
return extend_trajectory_point; // 返回扩展轨迹点
}
// 扩展轨迹点
std::vector extendTrajectoryPoints(
const std::vector & input_points, const double extend_distance,
const double step_length)
{
auto output_points = input_points; // 复制输入轨迹点
const auto is_driving_forward_opt =
autoware::motion_utils::isDrivingForwardWithTwist(input_points); // 判断是否向前行驶
const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true;
if (extend_distance < std::numeric_limits::epsilon()) {
return output_points; // 如果扩展距离小于阈值，直接返回输入轨迹点
}
const auto goal_point = input_points.back(); // 获取目标点
double extend_sum = 0.0; // 扩展总距离
while (extend_sum <= (extend_distance - step_length)) {
const auto extend_trajectory_point =
getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); // 获取扩展轨迹点
output_points.push_back(extend_trajectory_point); // 添加到输出轨迹点
extend_sum += step_length; // 增加扩展距离
}
const auto extend_trajectory_point =
getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); // 获取最终扩展轨迹点
output_points.push_back(extend_trajectory_point); // 添加到输出轨迹点
return output_points; // 返回扩展后的轨迹点
}
// 获取目标对象类型
std::vector getTargetObjectType(rclcpp::Node & node, const std::string & param_prefix)
{
std::unordered_map types_map{
{"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR},
{"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS},
{"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE},
{"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}};
std::vector types; // 存储目标对象类型
for (const auto & type : types_map) {
if (node.declare_parameter(param_prefix + type.first)) { // 判断是否启用该类型
types.push_back(type.second); // 添加类型到列表
}
}
return types; // 返回目标对象类型列表
}
// 重采样轨迹点
std::vector resampleTrajectoryPoints(
const std::vector & traj_points, const double interval)
{
const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); // 转换为轨迹
const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); // 重采样轨迹
return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); // 转换为轨迹点数组
}
// 将PCL点转换为geometry_msgs::msg::Point
geometry_msgs::msg::Point toGeomPoint(const pcl::PointXYZ & point)
{
geometry_msgs::msg::Point geom_point; // 创建几何点
geom_point.x = point.x; // 设置x坐标
geom_point.y = point.y; // 设置y坐标
geom_point.z = point.z; // 设置z坐标
return geom_point; // 返回几何点
}
// 将autoware::universe_utils::Point2d转换为geometry_msgs::msg::Point
geometry_msgs::msg::Point toGeomPoint(const autoware::universe_utils::Point2d & point)
{
geometry_msgs::msg::Point geom_point; // 创建几何点
geom_point.x = point.x(); // 设置x坐标
geom_point.y = point.y(); // 设置y坐标
return geom_point; // 返回几何点
}
// 判断是否低于阈值（考虑滞后）
bool isLowerConsideringHysteresis(
const double current_val, const bool was_low, const double high_val, const double low_val)
{
if (was_low) {
if (high_val < current_val) {
return false; // 如果之前低于阈值且当前值高于高阈值，返回false
}
return true; // 如果之前低于阈值且当前值低于高阈值，返回true
}
if (current_val < low_val) {
return true; // 如果之前高于阈值且当前值低于低阈值，返回true
}
return false; // 如果之前高于阈值且当前值高于低阈值，返回false
}
// 连接两个向量
template
void concatenate(std::vector & first, const std::vector & last)
{
first.insert(first.end(), last.begin(), last.end()); // 将第二个向量的内容追加到第一个向量
}
} // namespace
namespace autoware::motion_planning
{
// 障碍物巡航规划器节点构造函数
ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationParam(
rclcpp::Node & node)
{ // 行为决策参数
decimate_trajectory_step_length =
node.declare_parameter("behavior_determination.decimate_trajectory_step_length"); // 轨迹降采样步长
pointcloud_search_radius =
node.declare_parameter("behavior_determination.pointcloud_search_radius"); // 点云搜索半径
pointcloud_voxel_grid_x =
node.declare_parameter("behavior_determination.pointcloud_voxel_grid_x"); // 点云体素网格x
pointcloud_voxel_grid_y =
node.declare_parameter("behavior_determination.pointcloud_voxel_grid_y"); // 点云体素网格y
pointcloud_voxel_grid_z =
node.declare_parameter("behavior_determination.pointcloud_voxel_grid_z"); // 点云体素网格z
pointcloud_cluster_tolerance =
node.declare_parameter("behavior_determination.pointcloud_cluster_tolerance"); // 点云聚类容差
pointcloud_min_cluster_size =
node.declare_parameter("behavior_determination.pointcloud_min_cluster_size"); // 点云最小聚类大小
pointcloud_max_cluster_size =
node.declare_parameter("behavior_determination.pointcloud_max_cluster_size"); // 点云最大聚类大小
obstacle_velocity_threshold_from_cruise_to_stop = node.declare_parameter(
"behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop"); // 从巡航到停止的障碍物速度阈值
obstacle_velocity_threshold_from_stop_to_cruise = node.declare_parameter(
"behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise"); // 从停止到巡航的障碍物速度阈值
crossing_obstacle_velocity_threshold = node.declare_parameter(
"behavior_determination.crossing_obstacle.obstacle_velocity_threshold"); // 横穿障碍物速度阈值
crossing_obstacle_traj_angle_threshold = node.declare_parameter(
"behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold"); // 横穿障碍物轨迹角度阈值
collision_time_margin = node.declare_parameter(
"behavior_determination.stop.crossing_obstacle.collision_time_margin"); // 碰撞时间容差
outside_obstacle_min_velocity_threshold = node.declare_parameter(
"behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold"); // 外部障碍物最小速度阈值
ego_obstacle_overlap_time_threshold = node.declare_parameter(
"behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold"); // 自车与障碍物重叠时间阈值
max_prediction_time_for_collision_check = node.declare_parameter(
"behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check"); // 碰撞检查最大预测时间
stop_obstacle_hold_time_threshold =
node.declare_parameter("behavior_determination.stop_obstacle_hold_time_threshold"); // 停止障碍物保持时间阈值
prediction_resampling_time_interval =
node.declare_parameter("behavior_determination.prediction_resampling_time_interval"); // 预测重采样时间间隔
prediction_resampling_time_horizon =
node.declare_parameter("behavior_determination.prediction_resampling_time_horizon"); // 预测重采样时间范围
max_lat_margin_for_stop =
node.declare_parameter("behavior_determination.stop.max_lat_margin"); // 停止时的最大横向容差
max_lat_margin_for_stop_against_unknown =
node.declare_parameter("behavior_determination.stop.max_lat_margin_against_unknown"); // 未知障碍物停止时的最大横向容差
max_lat_margin_for_cruise =
node.declare_parameter("behavior_determination.cruise.max_lat_margin"); // 巡航时的最大横向容差
enable_yield = node.declare_parameter("behavior_determination.cruise.yield.enable_yield"); // 是否启用让行
yield_lat_distance_threshold =
node.declare_parameter("behavior_determination.cruise.yield.lat_distance_threshold"); // 让行横向距离阈值
max_lat_dist_between_obstacles = node.declare_parameter(
"behavior_determination.cruise.yield.max_lat_dist_between_obstacles"); // 障碍物之间的最大横向距离
stopped_obstacle_velocity_threshold = node.declare_parameter(
"behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold"); // 停止障碍物速度阈值
max_obstacles_collision_time = node.declare_parameter(
"behavior_determination.cruise.yield.max_obstacles_collision_time"); // 障碍物碰撞时间阈值
max_lat_margin_for_slow_down =
node.declare_parameter("behavior_determination.slow_down.max_lat_margin"); // 减速时的最大横向容差
lat_hysteresis_margin_for_slow_down =
node.declare_parameter("behavior_determination.slow_down.lat_hysteresis_margin"); // 减速时的滞后容差
successive_num_to_entry_slow_down_condition = node.declare_parameter(
"behavior_determination.slow_down.successive_num_to_entry_slow_down_condition"); // 进入减速条件的连续次数
successive_num_to_exit_slow_down_condition = node.declare_parameter(
"behavior_determination.slow_down.successive_num_to_exit_slow_down_condition"); // 退出减速条件的连续次数
enable_to_consider_current_pose = node.declare_parameter(
"behavior_determination.consider_current_pose.enable_to_consider_current_pose"); // 是否考虑当前姿态
time_to_convergence = node.declare_parameter(
"behavior_determination.consider_current_pose.time_to_convergence"); // 收敛时间
min_velocity_to_reach_collision_point = node.declare_parameter(
"behavior_determination.stop.min_velocity_to_reach_collision_point"); // 到达碰撞点的最小速度
max_lat_time_margin_for_stop = node.declare_parameter(
"behavior_determination.stop.outside_obstacle.max_lateral_time_margin"); // 停止时的最大横向时间容差
max_lat_time_margin_for_cruise = node.declare_parameter(
"behavior_determination.cruise.outside_obstacle.max_lateral_time_margin"); // 巡航时的最大横向时间容差
num_of_predicted_paths_for_outside_cruise_obstacle = node.declare_parameter(
"behavior_determination.cruise.outside_obstacle.num_of_predicted_paths"); // 外部巡航障碍物的预测路径数量
num_of_predicted_paths_for_outside_stop_obstacle = node.declare_parameter(
"behavior_determination.stop.outside_obstacle.num_of_predicted_paths"); // 外部停止障碍物的预测路径数量
pedestrian_deceleration_rate = node.declare_parameter(
"behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate"); // 行人减速率
bicycle_deceleration_rate = node.declare_parameter(
"behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate"); // 自行车减速率
}
// 更新行为决策参数
void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
const std::vector & parameters)
{
// 更新行为决策参数
autoware::universe_utils::updateParam(
parameters, "behavior_determination.decimate_trajectory_step_length",
decimate_trajectory_step_length);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.pointcloud_voxel_grid_x", pointcloud_voxel_grid_x);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.pointcloud_voxel_grid_y", pointcloud_voxel_grid_y);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.pointcloud_voxel_grid_z", pointcloud_voxel_grid_z);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.pointcloud_cluster_tolerance",
pointcloud_cluster_tolerance);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.pointcloud_min_cluster_size", pointcloud_min_cluster_size);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.pointcloud_max_cluster_size", pointcloud_max_cluster_size);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold",
crossing_obstacle_velocity_threshold);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold",
crossing_obstacle_traj_angle_threshold);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.crossing_obstacle.collision_time_margin",
collision_time_margin);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold",
outside_obstacle_min_velocity_threshold);
autoware::universe_utils::updateParam(
parameters,
"behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold",
ego_obstacle_overlap_time_threshold);
autoware::universe_utils::updateParam(
parameters,
"behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check",
max_prediction_time_for_collision_check);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop_obstacle_hold_time_threshold",
stop_obstacle_hold_time_threshold);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.prediction_resampling_time_interval",
prediction_resampling_time_interval);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.prediction_resampling_time_horizon",
prediction_resampling_time_horizon);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.max_lat_margin_against_unknown",
max_lat_margin_for_stop_against_unknown);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.yield.lat_distance_threshold",
yield_lat_distance_threshold);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles",
max_lat_dist_between_obstacles);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold",
stopped_obstacle_velocity_threshold);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time",
max_obstacles_collision_time);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.slow_down.lat_hysteresis_margin",
lat_hysteresis_margin_for_slow_down);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition",
successive_num_to_entry_slow_down_condition);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition",
successive_num_to_exit_slow_down_condition);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose",
enable_to_consider_current_pose);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.consider_current_pose.time_to_convergence",
time_to_convergence);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.min_velocity_to_reach_collision_point",
min_velocity_to_reach_collision_point);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.outside_obstacle.max_lateral_time_margin",
max_lat_time_margin_for_stop);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin",
max_lat_time_margin_for_cruise);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths",
num_of_predicted_paths_for_outside_cruise_obstacle);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.outside_obstacle.num_of_predicted_paths",
num_of_predicted_paths_for_outside_stop_obstacle);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate",
pedestrian_deceleration_rate);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate",
bicycle_deceleration_rate);
}
// 障碍物巡航规划器节点构造函数
ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
: Node("obstacle_cruise_planner", node_options),
vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), // 获取车辆信息
debug_data_ptr_(std::make_shared()) // 创建调试数据指针
{
using std::placeholders::_1;
// 创建订阅者
traj_sub_ = create_subscription(
"~/input/trajectory", rclcpp::QoS{1},
std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); // 订阅轨迹
// 创建发布者
trajectory_pub_ = create_publisher("~/output/trajectory", 1); // 发布轨迹
vel_limit_pub_ =
create_publisher("~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); // 发布速度限制
clear_vel_limit_pub_ = create_publisher(
"~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); // 发布清除速度限制命令
// 创建调试发布者
debug_calculation_time_pub_ = create_publisher("~/debug/processing_time_ms", 1); // 发布处理时间
debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); // 发布巡航虚拟墙标记
debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); // 发布停止虚拟墙标记
debug_slow_down_wall_marker_pub_ =
create_publisher("~/debug/slow_down/virtual_wall", 1); // 发布减速虚拟墙标记
debug_marker_pub_ = create_publisher("~/debug/marker", 1); // 发布调试标记
debug_stop_planning_info_pub_ =
create_publisher("~/debug/stop_planning_info", 1); // 发布停止规划信息
debug_cruise_planning_info_pub_ =
create_publisher("~/debug/cruise_planning_info", 1); // 发布巡航规划信息
debug_slow_down_planning_info_pub_ =
create_publisher("~/debug/slow_down_planning_info", 1); // 发布减速规划信息
// 创建TF监听器
tf_buffer_ = std::make_unique(get_clock()); // 创建TF缓冲区
tf_listener_ = std::make_shared(*tf_buffer_); // 创建TF监听器
const auto longitudinal_info = LongitudinalInfo(*this); // 获取纵向信息
ego_nearest_param_ = EgoNearestParam(*this); // 初始化自车最近点参数
enable_debug_info_ = declare_parameter("common.enable_debug_info"); // 是否启用调试信息
enable_calculation_time_info_ = declare_parameter("common.enable_calculation_time_info"); // 是否启用处理时间信息
enable_slow_down_planning_ = declare_parameter("common.enable_slow_down_planning"); // 是否启用减速规划
use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); // 是否使用点云进行停止障碍物检测
use_pointcloud_for_slow_down_ =
declare_parameter("common.slow_down_obstacle_type.pointcloud"); // 是否使用点云进行减速障碍物检测
use_pointcloud_ = use_pointcloud_for_stop_ || use_pointcloud_for_slow_down_; // 是否使用点云
behavior_determination_param_ = BehaviorDeterminationParam(*this); // 初始化行为决策参数
{ // 规划算法
const std::string planning_algorithm_param =
declare_parameter("common.planning_algorithm"); // 获取规划算法参数
planning_algorithm_ = getPlanningAlgorithmType(planning_algorithm_param); // 获取规划算法类型
if (planning_algorithm_ == PlanningAlgorithm::OPTIMIZATION_BASE) {
planner_ptr_ = std::make_unique(
*this, longitudinal_info, vehicle_info_, ego_nearest_param_, debug_data_ptr_); // 创建基于优化的规划器
} else if (planning_algorithm_ == PlanningAlgorithm::PID_BASE) {
planner_ptr_ = std::make_unique(
*this, longitudinal_info, vehicle_info_, ego_nearest_param_, debug_data_ptr_); // 创建基于PID的规划器
} else {
throw std::logic_error("Designated algorithm is not supported."); // 抛出异常
}
min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); // 最小行为停止裕度
additional_safe_distance_margin_on_curve_ =
declare_parameter("common.stop_on_curve.additional_safe_distance_margin"); // 曲线上的额外安全距离裕度
enable_approaching_on_curve_ =
declare_parameter("common.stop_on_curve.enable_approaching"); // 是否启用曲线上的接近
min_safe_distance_margin_on_curve_ =
declare_parameter("common.stop_on_curve.min_safe_distance_margin"); // 曲线上的最小安全距离裕度
suppress_sudden_obstacle_stop_ =
declare_parameter("common.suppress_sudden_obstacle_stop"); // 是否抑制突然障碍物停止
planner_ptr_->setParam(
enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_,
enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);
}
{ // 停止/巡航/减速障碍物类型
inside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.inside."); // 内部停止障碍物类型
outside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.outside."); // 外部停止障碍物类型
inside_cruise_obstacle_types_ =
getTargetObjectType(*this, "common.cruise_obstacle_type.inside."); // 内部巡航障碍物类型
outside_cruise_obstacle_types_ =
getTargetObjectType(*this, "common.cruise_obstacle_type.outside."); // 外部巡航障碍物类型
slow_down_obstacle_types_ = getTargetObjectType(*this, "common.slow_down_obstacle_type."); // 减速障碍物类型
}
// 设置参数回调
set_param_res_ = this->add_on_set_parameters_callback(
std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1));
}
// 获取规划算法类型
ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType(
const std::string & param) const
{
if (param == "pid_base") {
return PlanningAlgorithm::PID_BASE; // PID基础
} else if (param == "optimization_base") {
return PlanningAlgorithm::OPTIMIZATION_BASE; // 优化基础
}
return PlanningAlgorithm::INVALID; // 无效
}
// 参数回调
rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
const std::vector & parameters)
{
planner_ptr_->onParam(parameters); // 调用规划器的参数更新
autoware::universe_utils::updateParam(
parameters, "common.enable_debug_info", enable_debug_info_); // 更新是否启用调试信息
autoware::universe_utils::updateParam(
parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); // 更新是否启用处理时间信息
autoware::universe_utils::updateParam(
parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); // 更新是否启用曲线上的接近
autoware::universe_utils::updateParam(
parameters, "common.stop_on_curve.additional_safe_distance_margin",
additional_safe_distance_margin_on_curve_); // 更新曲线上的额外安全距离裕度
autoware::universe_utils::updateParam(
parameters, "common.stop_on_curve.min_safe_distance_margin",
min_safe_distance_margin_on_curve_); // 更新曲线上的最小安全距离裕度
planner_ptr_->setParam(
enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_,
enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);
autoware::universe_utils::updateParam(
parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); // 更新是否启用减速规划
behavior_determination_param_.onParam(parameters); // 更新行为决策参数
rcl_interfaces::msg::SetParametersResult result; // 创建参数结果消息
result.successful = true; // 设置成功标志
result.reason = "success"; // 设置成功原因
return result; // 返回参数结果
}
// 轨迹回调
void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
{
const auto ego_odom_ptr = ego_odom_sub_.takeData(); // 获取自车里程计数据
const auto objects_ptr = objects_sub_.takeData(); // 获取目标数据
const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; // 获取点云数据
const auto acc_ptr = acc_sub_.takeData(); // 获取加速度数据
const bool can_detect_obstacles = objects_ptr || pointcloud_ptr; // 判断是否可以检测障碍物
if (!ego_odom_ptr || !can_detect_obstacles || !acc_ptr) {
return; // 如果数据不完整，直接返回
}
const auto & ego_odom = *ego_odom_ptr; // 获取自车里程计
const auto & acc = *acc_ptr; // 获取加速度
const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); // 转换为轨迹点数组
// 检查订阅变量是否准备好
if (traj_points.empty()) {
return; // 如果轨迹点为空，直接返回
}
stop_watch_.tic(__func__); // 开始计时
*debug_data_ptr_ = DebugData(); // 初始化调试数据
const auto is_driving_forward = autoware::motion_utils::isDrivingForwardWithTwist(traj_points); // 判断是否向前行驶
is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; // 更新是否向前行驶标志
const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = [&]() {
std::vector stop_obstacles; // 停止障碍物
std::vector cruise_obstacles; // 巡航障碍物
std::vector slow_down_obstacles; // 减速障碍物
if (objects_ptr) {
// 1. 将预测目标转换为障碍物
const auto target_obstacles = convertToObstacles(ego_odom, *objects_ptr, traj_points); // 转换为障碍物
// 2. 确定自车对每个障碍物的行为（停止、巡航、减速）
const auto & [stop_object_obstacles, cruise_object_obstacles, slow_down_object_obstacles] =
determineEgoBehaviorAgainstPredictedObjectObstacles(
ego_odom, *objects_ptr, traj_points, target_obstacles);
concatenate(stop_obstacles, stop_object_obstacles); // 合并停止障碍物
concatenate(cruise_obstacles, cruise_object_obstacles); // 合并巡航障碍物
concatenate(slow_down_obstacles, slow_down_object_obstacles); // 合并减速障碍物
}
if (pointcloud_ptr) {
const auto target_obstacles =
convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); // 转换为障碍物
const auto & [stop_pc_obstacles, cruise_pc_obstacles, slow_down_pc_obstacles] =
determineEgoBehaviorAgainstPointCloudObstacles(ego_odom, traj_points, target_obstacles);
concatenate(stop_obstacles, stop_pc_obstacles); // 合并停止障碍物
concatenate(cruise_obstacles, cruise_pc_obstacles); // 合并巡航障碍物
concatenate(slow_down_obstacles, slow_down_pc_obstacles); // 合并减速障碍物
}
return std::make_tuple(stop_obstacles, cruise_obstacles, slow_down_obstacles);
}();
// 3. 创建规划数据
const auto planner_data = createPlannerData(ego_odom, acc, traj_points); // 创建规划数据
// 4. 停止规划
const auto stop_traj_points = planner_ptr_->generateStopTrajectory(planner_data, stop_obstacles); // 生成停止轨迹
// 5. 巡航规划
std::optional cruise_vel_limit; // 巡航速度限制
const auto cruise_traj_points = planner_ptr_->generateCruiseTrajectory(
planner_data, stop_traj_points, cruise_obstacles, cruise_vel_limit); // 生成巡航轨迹
publishVelocityLimit(cruise_vel_limit, "cruise"); // 发布巡航速度限制
// 6. 减速规划
std::optional slow_down_vel_limit; // 减速速度限制
const auto slow_down_traj_points = planner_ptr_->generateSlowDownTrajectory(
planner_data, cruise_traj_points, slow_down_obstacles, slow_down_vel_limit); // 生成减速轨迹
publishVelocityLimit(slow_down_vel_limit, "slow_down"); // 发布减速速度限制
// 7. 发布轨迹
const auto output_traj =
autoware::motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); // 转换为轨迹
trajectory_pub_->publish(output_traj); // 发布轨迹
// 8. 发布调试数据
published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); // 发布发布时间
planner_ptr_->publishMetrics(now()); // 发布规划指标
publishDebugMarker(); // 发布调试标记
publishDebugInfo(); // 发布调试信息
// 9. 发布和打印处理时间
const double calculation_time = stop_watch_.toc(__func__); // 获取处理时间
publishCalculationTime(calculation_time); // 发布处理时间
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_calculation_time_info_, "%s := %f [ms]", __func__, calculation_time); // 打印处理时间
}
// 创建单步多边形
std::vector ObstacleCruisePlannerNode::createOneStepPolygons(
const std::vector & traj_points,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const
{
const auto & p = behavior_determination_param_; // 获取行为决策参数
const double front_length = vehicle_info.max_longitudinal_offset_m; // 车辆前部偏移量
const double rear_length = vehicle_info.rear_overhang_m; // 车辆后部偏移量
const double vehicle_width = vehicle_info.vehicle_width_m; // 车辆宽度
const size_t nearest_idx =
autoware::motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); // 找到最近的轨迹段索引
const auto nearest_pose = traj_points.at(nearest_idx).pose; // 获取最近的轨迹点姿态
const auto current_ego_pose_error =
autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); // 计算自车姿态误差
const double current_ego_lat_error = current_ego_pose_error.position.y; // 获取横向误差
const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); // 获取偏航角误差
double time_elapsed{0.0}; // 经过时间
std::vector output_polygons; // 输出多边形
Polygon2d tmp_polys{}; // 临时多边形
for (size_t i = 0; i < traj_points.size(); ++i) {
std::vector current_poses = {traj_points.at(i).pose}; // 当前姿态
// 假设姿态误差会随着时间收敛到零
if (p.enable_to_consider_current_pose && time_elapsed < p.time_to_convergence) {
const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence; // 剩余比例
geometry_msgs::msg::Pose indexed_pose_err;
indexed_pose_err.set__orientation(
autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); // 创建偏航角四元数
indexed_pose_err.set__position(
autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); // 创建位置
current_poses.push_back(
autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); // 转换姿态
if (traj_points.at(i).longitudinal_velocity_mps != 0.0) {
time_elapsed +=
p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); // 更新经过时间
} else {
time_elapsed = std::numeric_limits::max(); // 如果速度为零，设置为无穷大
}
}
Polygon2d idx_poly{}; // 索引多边形
for (const auto & pose : current_poses) {
if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) {
boost::geometry::append(
idx_poly,
autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width)
.outer()); // 添加车辆足迹
boost::geometry::append(
idx_poly, autoware::universe_utils::fromMsg(
autoware::universe_utils::calcOffsetPose(
pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0)
.position)
.to_2d()); // 添加横向偏移点
boost::geometry::append(
idx_poly, autoware::universe_utils::fromMsg(
autoware::universe_utils::calcOffsetPose(
pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0)
.position)
.to_2d()); // 添加横向偏移点
} else {
boost::geometry::append(
idx_poly, autoware::universe_utils::toFootprint(
pose, front_length, rear_length, vehicle_width + lat_margin * 2.0)
.outer()); // 添加车辆足迹
}
}
boost::geometry::append(tmp_polys, idx_poly.outer()); // 添加到临时多边形
Polygon2d hull_polygon; // 凸包多边形
boost::geometry::convex_hull(tmp_polys, hull_polygon); // 计算凸包
boost::geometry::correct(hull_polygon); // 修正多边形
output_polygons.push_back(hull_polygon); // 添加到输出多边形
tmp_polys = std::move(idx_poly); // 移动索引多边形
}
return output_polygons; // 返回输出多边形
}
// 将目标转换为障碍物
std::vector ObstacleCruisePlannerNode::convertToObstacles(
const Odometry & odometry, const PredictedObjects & objects,
const std::vector & traj_points) const
{
stop_watch_.tic(__func__); // 开始计时
const auto obj_stamp = rclcpp::Time(objects.header.stamp); // 获取目标时间戳
const auto & p = behavior_determination_param_; // 获取行为决策参数
constexpr double epsilon = 1e-6; // 小常量
const double max_lat_margin = std::max(
{p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown,
p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); // 最大横向容差
const double max_lat_time_margin =
std::max({p.max_lat_time_margin_for_stop, p.max_lat_time_margin_for_cruise}); // 最大横向时间容差
const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); // 找到自车在轨迹中的索引
std::vector target_obstacles; // 目标障碍物
for (const auto & predicted_object : objects.objects) {
const auto & object_id =
autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); // 获取目标ID
// brkay54: 当使用预测时，观察到在场景模拟器中目标的朝向错误。
const auto & current_obstacle_pose =
obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); // 获取当前目标姿态
// 1. 检查目标的标签是否为目标类型
const uint8_t label = predicted_object.classification.front().label; // 获取目标标签
const bool is_target_obstacle =
isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); // 判断是否为目标障碍物
if (!is_target_obstacle) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.",
object_id.c_str());
continue; // 如果不是目标障碍物，跳过
}
const auto projected_vel = calculateObstacleVelocitiesRelativeToTrajectory(
traj_points, current_obstacle_pose.pose,
predicted_object.kinematics.initial_twist_with_covariance.twist); // 计算障碍物相对于轨迹的速度
// 2. 检查障碍物是否在自车前方
const auto ego_to_obstacle_distance =
calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); // 计算自车到障碍物的距离
if (!ego_to_obstacle_distance) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.",
object_id.c_str());
continue; // 如果不是前方障碍物，跳过
}
// 3. 检查横向距离和时间是否小于阈值
const double lat_dist_from_obstacle_to_traj =
autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); // 计算障碍物到轨迹的横向距离
const double min_lat_dist_to_traj_poly = [&]() {
const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); // 计算障碍物的最大长度
return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m -
obstacle_max_length; // 计算最小横向距离
}();
if (max_lat_margin < min_lat_dist_to_traj_poly) {
if (projected_vel.second > 0.0) {
const auto time_to_traj =
min_lat_dist_to_traj_poly / std::max(epsilon, projected_vel.second); // 计算到达轨迹的时间
if (time_to_traj > max_lat_time_margin) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str());
continue; // 如果横向距离过大，跳过
}
} else {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str());
continue; // 如果横向距离过大，跳过
}
}
const auto target_obstacle = Obstacle(
obj_stamp, predicted_object, current_obstacle_pose.pose, *ego_to_obstacle_distance,
lat_dist_from_obstacle_to_traj, projected_vel.first, projected_vel.second); // 创建目标障碍物
target_obstacles.push_back(target_obstacle); // 添加到目标障碍物列表
}
const double calculation_time = stop_watch_.toc(__func__); // 获取处理时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__,
calculation_time); // 打印处理时间
return target_obstacles; // 返回目标障碍物列表
}
// 将点云转换为障碍物
std::vector ObstacleCruisePlannerNode::convertToObstacles(
const Odometry & odometry, const PointCloud2 & pointcloud,
const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const
{
stop_watch_.tic(__func__); // 开始计时
const auto & p = behavior_determination_param_; // 获取行为决策参数
std::vector target_obstacles; // 目标障碍物
std::optional transform_stamped{}; // 变换矩阵
try {
transform_stamped = tf_buffer_->lookupTransform(
traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp,
rclcpp::Duration::from_seconds(0.5)); // 查找变换矩阵
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR_STREAM(
get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to "
<< pointcloud.header.frame_id);
transform_stamped = std::nullopt; // 如果查找失败，设置为空
}
if (!pointcloud.data.empty() && transform_stamped) {
// 1. 转换点云
PointCloud::Ptr pointcloud_ptr(new PointCloud); // 创建点云指针
pcl::fromROSMsg(pointcloud, *pointcloud_ptr); // 将ROS消息转换为PCL点云
const Eigen::Matrix4f transform =
tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); // 转换为Eigen矩阵
pcl::transformPointCloud(*pointcloud_ptr, *pointcloud_ptr, transform); // 转换点云
// 2. 下采样和聚类点云
PointCloud::Ptr filtered_points_ptr(new PointCloud); // 创建过滤后的点云指针
pcl::VoxelGrid filter; // 创建体素网格滤波器
filter.setInputCloud(pointcloud_ptr); // 设置输入点云
filter.setLeafSize(
p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); // 设置体素大小
filter.filter(*filtered_points_ptr); // 下采样
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); // 创建Kd树
tree->setInputCloud(filtered_points_ptr); // 设置输入点云
std::vector clusters; // 聚类结果
pcl::EuclideanClusterExtraction ec; // 创建欧几里得聚类提取器
ec.setClusterTolerance(p.pointcloud_cluster_tolerance); // 设置聚类容差
ec.setMinClusterSize(p.pointcloud_min_cluster_size); // 设置最小聚类大小
ec.setMaxClusterSize(p.pointcloud_max_cluster_size); // 设置最大聚类大小
ec.setSearchMethod(tree); // 设置搜索方法
ec.setInputCloud(filtered_points_ptr); // 设置输入点云
ec.extract(clusters); // 提取聚类
const auto max_lat_margin =
std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); // 最大横向容差
const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); // 找到自车在轨迹中的索引
// 3. 将聚类转换为障碍物
for (const auto & cluster_indices : clusters) {
double ego_to_stop_collision_distance = std::numeric_limits::max(); // 自车到停止碰撞点的距离
double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); // 自车到减速前方碰撞点的距离
double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); // 自车到减速后方碰撞点的距离
double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); // 障碍物到轨迹的横向距离
double ego_to_obstacle_distance = std::numeric_limits::max(); // 自车到障碍物的距离
std::optional stop_collision_point = std::nullopt; // 停止碰撞点
std::optional slow_down_front_collision_point = std::nullopt; // 减速前方碰撞点
std::optional slow_down_back_collision_point = std::nullopt; // 减速后方碰撞点
for (const auto & index : cluster_indices.indices) {
const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); // 获取障碍物点
const auto current_lat_dist_from_obstacle_to_traj =
autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); // 计算当前横向距离
const auto min_lat_dist_to_traj_poly =
std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; // 计算最小横向距离
if (min_lat_dist_to_traj_poly < max_lat_margin) {
const auto current_ego_to_obstacle_distance =
calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); // 计算自车到障碍物的距离
if (current_ego_to_obstacle_distance) {
ego_to_obstacle_distance =
std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); // 更新自车到障碍物的距离
} else {
continue; // 如果距离无效，跳过
}
lat_dist_from_obstacle_to_traj =
std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); // 更新横向距离
if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) {
if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) {
stop_collision_point = obstacle_point; // 更新停止碰撞点
ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; // 更新自车到停止碰撞点的距离
}
} else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) {
if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) {
slow_down_front_collision_point = obstacle_point; // 更新减速前方碰撞点
ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; // 更新自车到减速前方碰撞点的距离
} else if (
*current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) {
slow_down_back_collision_point = obstacle_point; // 更新减速后方碰撞点
ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; // 更新自车到减速后方碰撞点的距离
}
}
}
}
if (stop_collision_point || slow_down_front_collision_point) {
target_obstacles.emplace_back(
pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point,
slow_down_back_collision_point, ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj);
}
}
}
const double calculation_time = stop_watch_.toc(__func__); // 获取处理时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__,
calculation_time); // 打印处理时间
return target_obstacles; // 返回目标障碍物列表
}
// 判断是否为内部停止障碍物
bool ObstacleCruisePlannerNode::isInsideStopObstacle(const uint8_t label) const
{
const auto & types = inside_stop_obstacle_types_; // 获取内部停止障碍物类型
return std::find(types.begin(), types.end(), label) != types.end(); // 判断是否在列表中
}
// 判断是否为外部停止障碍物
bool ObstacleCruisePlannerNode::isOutsideStopObstacle(const uint8_t label) const
{
const auto & types = outside_stop_obstacle_types_; // 获取外部停止障碍物类型
return std::find(types.begin(), types.end(), label) != types.end(); // 判断是否在列表中
}
// 判断是否为停止障碍物
bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const
{
return isInsideStopObstacle(label) || isOutsideStopObstacle(label); // 判断是否为内部或外部停止障碍物
}
// 判断是否为内部巡航障碍物
bool ObstacleCruisePlannerNode::isInsideCruiseObstacle(const uint8_t label) const
{
const auto & types = inside_cruise_obstacle_types_; // 获取内部巡航障碍物类型
return std::find(types.begin(), types.end(), label) != types.end(); // 判断是否在列表中
}
// 判断是否为外部巡航障碍物
bool ObstacleCruisePlannerNode::isOutsideCruiseObstacle(const uint8_t label) const
{
const auto & types = outside_cruise_obstacle_types_; // 获取外部巡航障碍物类型
return std::find(types.begin(), types.end(), label) != types.end(); // 判断是否在列表中
}
// 判断是否为巡航障碍物
bool ObstacleCruisePlannerNode::isCruiseObstacle(const uint8_t label) const
{
return isInsideCruiseObstacle(label) || isOutsideCruiseObstacle(label); // 判断是否为内部或外部巡航障碍物
}
// 判断是否为减速障碍物
bool ObstacleCruisePlannerNode::isSlowDownObstacle(const uint8_t label) const
{
const auto & types = slow_down_obstacle_types_; // 获取减速障碍物类型
return std::find(types.begin(), types.end(), label) != types.end(); // 判断是否在列表中
}
// 判断是否为前方碰撞障碍物
bool ObstacleCruisePlannerNode::isFrontCollideObstacle(
const std::vector & traj_points, const Obstacle & obstacle,
const size_t first_collision_idx) const
{
const auto obstacle_idx =
autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose.position); // 找到障碍物在轨迹中的索引
const double obstacle_to_col_points_distance =
autoware::motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); // 计算障碍物到碰撞点的距离
const double obstacle_max_length = calcObstacleMaxLength(obstacle.shape); // 计算障碍物的最大长度
// 如果障碍物在碰撞点前方较远的位置，则障碍物在自车后方
return obstacle_to_col_points_distance > -obstacle_max_length;
}
// 确定自车对预测目标障碍物的行为（停止、巡航、减速）
std::tuple, std::vector, std::vector>
ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles(
const Odometry & odometry, const PredictedObjects & objects,
const std::vector & traj_points, const std::vector & obstacles)
{
stop_watch_.tic(__func__); // 开始计时
// 计算降采样轨迹点和轨迹多边形
const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); // 降采样轨迹点
const auto decimated_traj_polys =
createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); // 创建单步多边形
debug_data_ptr_->detection_polygons = decimated_traj_polys; // 更新调试数据
// 确定自车的行为（停止、巡航、减速）
std::vector stop_obstacles; // 停止障碍物
std::vector cruise_obstacles; // 巡航障碍物
std::vector slow_down_obstacles; // 减速障碍物
slow_down_condition_counter_.resetCurrentUuids(); // 重置减速条件计数器
for (const auto & obstacle : obstacles) {
const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // 将障碍物转换为多边形
// 计算轨迹和障碍物之间的横向距离
double precise_lat_dist = std::numeric_limits::max(); // 精确横向距离
for (const auto & traj_poly : decimated_traj_polys) {
const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); // 计算当前精确横向距离
precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); // 更新精确横向距离
}
// 过滤障碍物（巡航、停止、减速）
const auto cruise_obstacle = createCruiseObstacle(
odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); // 创建巡航障碍物
if (cruise_obstacle) {
cruise_obstacles.push_back(*cruise_obstacle); // 添加到巡航障碍物列表
continue;
}
const auto stop_obstacle = createStopObstacleForPredictedObject(
odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); // 创建停止障碍物
if (stop_obstacle) {
stop_obstacles.push_back(*stop_obstacle); // 添加到停止障碍物列表
continue;
}
const auto slow_down_obstacle = createSlowDownObstacleForPredictedObject(
odometry, decimated_traj_points, obstacle, precise_lat_dist); // 创建减速障碍物
if (slow_down_obstacle) {
slow_down_obstacles.push_back(*slow_down_obstacle); // 添加到减速障碍物列表
continue;
}
}
const auto & p = behavior_determination_param_; // 获取行为决策参数
if (p.enable_yield) {
const auto yield_obstacles = findYieldCruiseObstacles(obstacles, decimated_traj_points); // 查找让行巡航障碍物
if (yield_obstacles) {
for (const auto & y : yield_obstacles.value()) {
// 检查是否存在具有相同UUID的成员
auto it = std::find_if(
cruise_obstacles.begin(), cruise_obstacles.end(),
[&y](const auto & c) { return y.uuid == c.uuid; });
// 如果未找到匹配的UUID，则将让行障碍物添加到巡航障碍物列表
if (it == cruise_obstacles.end()) {
cruise_obstacles.push_back(y);
}
}
}
}
slow_down_condition_counter_.removeCounterUnlessUpdated(); // 删除未更新的计数器
// 检查目标障碍物的一致性
checkConsistency(objects.header.stamp, objects, stop_obstacles);
// 更新之前的障碍物
prev_stop_object_obstacles_ = stop_obstacles; // 更新之前的停止障碍物
prev_cruise_object_obstacles_ = cruise_obstacles; // 更新之前的巡航障碍物
prev_slow_down_object_obstacles_ = slow_down_obstacles; // 更新之前的减速障碍物
const double calculation_time = stop_watch_.toc(__func__); // 获取处理时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", __func__,
calculation_time); // 打印处理时间
return {stop_obstacles, cruise_obstacles, slow_down_obstacles}; // 返回停止、巡航和减速障碍物
}
// 判断自车对点云障碍物的行为
std::tuple, std::vector, std::vector>
ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles(
const Odometry & odometry, const std::vector & traj_points,
const std::vector & obstacles)
{
stop_watch_.tic(__func__); // 开始计时
const auto & p = behavior_determination_param_; // 获取行为决策参数
// 计算降采样轨迹点和轨迹多边形
const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); // 降采样轨迹点
const auto decimated_traj_polys =
createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); // 创建单步多边形
debug_data_ptr_->detection_polygons = decimated_traj_polys; // 更新调试数据
// 确定自车的行为（停止、减速）
std::vector stop_obstacles; // 停止障碍物
std::vector slow_down_obstacles; // 减速障碍物
for (const auto & obstacle : obstacles) {
const auto & precise_lat_dist = obstacle.lat_dist_from_obstacle_to_traj; // 获取障碍物到轨迹的横向距离
// 过滤减速障碍物
const auto slow_down_obstacle = createSlowDownObstacleForPredictedObject(
odometry, decimated_traj_points, obstacle, precise_lat_dist); // 创建减速障碍物
if (slow_down_obstacle) {
slow_down_obstacles.push_back(*slow_down_obstacle); // 添加到减速障碍物列表
continue;
}
// 过滤停止障碍物
const auto stop_obstacle = createStopObstacleForPredictedObject(
odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); // 创建停止障碍物
if (stop_obstacle) {
stop_obstacles.push_back(*stop_obstacle); // 添加到停止障碍物列表
continue;
}
}
// 更新之前的障碍物
prev_stop_object_obstacles_ = stop_obstacles; // 更新之前的停止障碍物
prev_slow_down_object_obstacles_ = slow_down_obstacles; // 更新之前的减速障碍物
const double calculation_time = stop_watch_.toc(__func__); // 获取处理时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", __func__,
calculation_time); // 打印处理时间
return {stop_obstacles, {}, slow_down_obstacles}; // 返回停止和减速障碍物
}
// 创建外部停止障碍物的碰撞点
std::optional>
ObstacleCruisePlannerNode::createCollisionPointForOutsideStopObstacle(
const Odometry & odometry, const std::vector & traj_points,
const std::vector & traj_polys, const Obstacle & obstacle,
const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const
{
const auto & object_id = obstacle.uuid.substr(0, 4); // 获取障碍物ID
const auto & p = behavior_determination_param_; // 获取行为决策参数
std::vector collision_index; // 碰撞索引
const auto collision_points = polygon_utils::getCollisionPoints(
traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(),
is_driving_forward_, collision_index,
calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length +
std::hypot(
vehicle_info_.vehicle_length_m,
vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); // 计算碰撞点
if (collision_points.empty()) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"[Stop] Ignore outside obstacle (%s) since there is no collision point between the "
"predicted path "
"and the ego.",
object_id.c_str());
debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); // 将障碍物添加到忽略列表
return std::nullopt; // 返回空值
}
const double collision_time_margin =
calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); // 计算碰撞时间裕度
if (p.collision_time_margin < collision_time_margin) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"[Stop] Ignore outside obstacle (%s) since it will not collide with the ego.",
object_id.c_str());
debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); // 将障碍物添加到忽略列表
return std::nullopt; // 返回空值
}
return polygon_utils::getCollisionPoint(
traj_points, collision_index.front(), collision_points, is_driving_forward_, vehicle_info_); // 返回碰撞点
}
// 创建预测对象的停止障碍物
std::optional ObstacleCruisePlannerNode::createStopObstacleForPredictedObject(
const Odometry & odometry, const std::vector & traj_points,
const std::vector & traj_polys, const Obstacle & obstacle,
const double precise_lat_dist) const
{
const auto & p = behavior_determination_param_; // 获取行为决策参数
const auto & object_id = obstacle.uuid.substr(0, 4); // 获取障碍物ID
if (!isStopObstacle(obstacle.classification.label)) {
return std::nullopt; // 如果不是停止障碍物，返回空值
}
const double max_lat_margin_for_stop =
(obstacle.classification.label == ObjectClassification::UNKNOWN)
? p.max_lat_margin_for_stop_against_unknown // 未知障碍物的最大横向容差
: p.max_lat_margin_for_stop; // 停止障碍物的最大横向容差
// 如果障碍物不在轨迹内
if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) {
if (!isOutsideStopObstacle(obstacle.classification.label)) {
return std::nullopt; // 如果不是外部停止障碍物，返回空值
}
const auto time_to_traj = precise_lat_dist / std::max(1e-6, obstacle.approach_velocity); // 计算到达轨迹的时间
if (time_to_traj > p.max_lat_time_margin_for_stop) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"[Stop] Ignore outside obstacle (%s) since it's far from trajectory.", object_id.c_str());
return std::nullopt; // 如果距离过大，返回空值
}
// 对于行人和自行车，检查碰撞点
double resample_time_horizon = p.prediction_resampling_time_horizon; // 预测重采样时间范围
if (obstacle.classification.label == ObjectClassification::PEDESTRIAN) {
resample_time_horizon =
std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) /
(2.0 * p.pedestrian_deceleration_rate); // 计算行人的重采样时间范围
} else if (obstacle.classification.label == ObjectClassification::BICYCLE) {
resample_time_horizon =
std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) /
(2.0 * p.bicycle_deceleration_rate); // 计算自行车的重采样时间范围
}
// 获取最高置信度的预测路径
const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths(
obstacle.predicted_paths, p.prediction_resampling_time_interval, resample_time_horizon,
p.num_of_predicted_paths_for_outside_stop_obstacle);
if (resampled_predicted_paths.empty()) {
return std::nullopt; // 如果预测路径为空，返回空值
}
const auto getCollisionPoint =
[&]() -> std::optional> {
for (const auto & predicted_path : resampled_predicted_paths) {
const auto collision_point = createCollisionPointForOutsideStopObstacle(
odometry, traj_points, traj_polys, obstacle, predicted_path, max_lat_margin_for_stop); // 创建碰撞点
if (collision_point) {
return collision_point; // 如果碰撞点存在，返回碰撞点
}
}
return std::nullopt; // 如果所有路径都没有碰撞点，返回空值
};
const auto collision_point = getCollisionPoint(); // 获取碰撞点
if (collision_point) {
return StopObstacle{
obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, obstacle.shape,
obstacle.longitudinal_velocity, obstacle.approach_velocity, collision_point->first,
collision_point->second}; // 返回停止障碍物
}
return std::nullopt; // 如果没有碰撞点，返回空值
}
// 如果障碍物在轨迹内
if (!isInsideStopObstacle(obstacle.classification.label)) {
return std::nullopt; // 如果不是内部停止障碍物，返回空值
}
// 计算碰撞点
const auto traj_polys_with_lat_margin =
createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); // 创建单步多边形
const auto collision_point = polygon_utils::getCollisionPoint(
traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); // 获取碰撞点
if (!collision_point) {
return std::nullopt; // 如果没有碰撞点，返回空值
}
// 检查是否为瞬态障碍物
const double abs_ego_offset =
min_behavior_stop_margin_ + (is_driving_forward_
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m)); // 计算自车偏移量
const double time_to_reach_stop_point =
calcTimeToReachCollisionPoint(odometry, collision_point->first, traj_points, abs_ego_offset); // 计算到达碰撞点的时间
const bool is_transient_obstacle = [&]() {
if (time_to_reach_stop_point <= p.collision_time_margin) {
return false; // 如果到达碰撞点的时间小于碰撞时间裕度，不是瞬态障碍物
}
// 获取预测位置
const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths(
obstacle.predicted_paths, p.prediction_resampling_time_interval,
p.prediction_resampling_time_horizon, 1); // 重采样预测路径
if (resampled_predicted_paths.empty() || resampled_predicted_paths.front().path.empty()) {
return false; // 如果预测路径为空，返回false
}
const auto future_obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(
resampled_predicted_paths.front(), time_to_reach_stop_point - p.collision_time_margin); // 计算预测位置
Obstacle tmp_future_obs = obstacle; // 创建临时障碍物
tmp_future_obs.pose =
future_obj_pose ? future_obj_pose.value() : resampled_predicted_paths.front().path.back(); // 更新障碍物姿态
const auto future_collision_point = polygon_utils::getCollisionPoint(
traj_points, traj_polys_with_lat_margin, tmp_future_obs, is_driving_forward_, vehicle_info_); // 获取未来碰撞点
return !future_collision_point; // 如果没有未来碰撞点，是瞬态障碍物
}();
if (is_transient_obstacle) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"[Stop] Ignore inside obstacle (%s) since it is transient obstacle.", object_id.c_str());
debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); // 将障碍物添加到忽略列表
return std::nullopt; // 返回空值
}
return StopObstacle{
obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, obstacle.shape,
obstacle.longitudinal_velocity, obstacle.approach_velocity, collision_point->first,
collision_point->second}; // 返回停止障碍物
}
// 创建点云的停止障碍物
std::optional ObstacleCruisePlannerNode::createStopObstacleForPointCloud(
const std::vector & traj_points, const Obstacle & obstacle,
const double precise_lat_dist) const
{
const auto & p = behavior_determination_param_; // 获取行为决策参数
if (!use_pointcloud_for_stop_) {
return std::nullopt; // 如果不使用点云进行停止，返回空值
}
if (!obstacle.stop_collision_point) {
return std::nullopt; // 如果没有停止碰撞点，返回空值
}
const double max_lat_margin_for_stop =
(obstacle.classification.label == ObjectClassification::UNKNOWN)
? p.max_lat_margin_for_stop_against_unknown // 未知障碍物的最大横向容差
: p.max_lat_margin_for_stop; // 停止障碍物的最大横向容差
if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) {
return std::nullopt; // 如果横向距离大于最大横向容差，返回空值
}
const auto dist_to_collide_on_traj =
autoware::motion_utils::calcSignedArcLength(traj_points, 0, *obstacle.stop_collision_point); // 计算到达碰撞点的距离
return StopObstacle{
obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, obstacle.shape,
{}, {}, *obstacle.stop_collision_point, dist_to_collide_on_traj}; // 返回停止障碍物
}
// 创建预测对象的减速障碍物
std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPredictedObject(
const Odometry & odometry, const std::vector & traj_points,
const Obstacle & obstacle, const double precise_lat_dist)
{
const auto & object_id = obstacle.uuid.substr(0, 4); // 获取障碍物ID
const auto & p = behavior_determination_param_; // 获取行为决策参数
slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); // 添加当前UUID到计数器
const bool is_prev_obstacle_slow_down =
getObstacleFromUuid(prev_slow_down_object_obstacles_, obstacle.uuid).has_value(); // 判断之前是否为减速障碍物
if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) {
return std::nullopt; // 如果不启用减速规划或不是减速障碍物，返回空值
}
// 检查横向距离（考虑滞后）
const bool is_lat_dist_low = isLowerConsideringHysteresis(
precise_lat_dist, is_prev_obstacle_slow_down,
p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0,
p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0);
const bool is_slow_down_required = [&]() {
if (is_prev_obstacle_slow_down) {
// 检查是否退出减速
if (!is_lat_dist_low) {
const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); // 减少计数
if (count <= -p.successive_num_to_exit_slow_down_condition) {
slow_down_condition_counter_.reset(obstacle.uuid); // 重置计数器
return false; // 如果连续次数达到退出条件，返回false
}
}
return true; // 如果之前是减速障碍物且未达到退出条件，返回true
}
// 检查是否进入减速
if (is_lat_dist_low) {
const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); // 增加计数
if (p.successive_num_to_entry_slow_down_condition <= count) {
slow_down_condition_counter_.reset(obstacle.uuid); // 重置计数器
return true; // 如果连续次数达到进入条件，返回true
}
}
return false; // 如果未达到进入条件，返回false
}();
if (!is_slow_down_required) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(),
precise_lat_dist);
return std::nullopt; // 如果不需要减速，返回空值
}
const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // 将障碍物转换为多边形
// 计算碰撞点
const auto traj_polys_with_lat_margin = createOneStepPolygons(
traj_points, vehicle_info_, odometry.pose.pose,
p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); // 创建单步多边形
std::vector front_collision_polygons; // 前方碰撞多边形
size_t front_seg_idx = 0; // 前方碰撞段索引
std::vector back_collision_polygons; // 后方碰撞多边形
size_t back_seg_idx = 0; // 后方碰撞段索引
for (size_t i = 0; i < traj_polys_with_lat_margin.size(); ++i) {
std::vector collision_polygons; // 碰撞多边形
bg::intersection(traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); // 计算交集
if (!collision_polygons.empty()) {
if (front_collision_polygons.empty()) {
front_collision_polygons = collision_polygons; // 更新前方碰撞多边形
front_seg_idx = i == 0 ? i : i - 1; // 更新前方碰撞段索引
}
back_collision_polygons = collision_polygons; // 更新后方碰撞多边形
back_seg_idx = i == 0 ? i : i - 1; // 更新后方碰撞段索引
} else {
if (!back_collision_polygons.empty()) {
break; // 如果已经找到后方碰撞多边形，退出循环
}
}
}
if (front_collision_polygons.empty() || back_collision_polygons.empty()) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
"[SlowDown] Ignore obstacle (%s) since there is no collision point", object_id.c_str());
return std::nullopt; // 如果没有碰撞点，返回空值
}
// 计算前方碰撞点
double front_min_dist = std::numeric_limits::max(); // 前方最小距离
geometry_msgs::msg::Point front_collision_point; // 前方碰撞点
for (const auto & collision_poly : front_collision_polygons) {
for (const auto & collision_point : collision_poly.outer()) {
const auto collision_geom_point = toGeomPoint(collision_point); // 将碰撞点转换为几何点
const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment(
traj_points, front_seg_idx, collision_geom_point); // 计算纵向偏移
if (dist < front_min_dist) {
front_min_dist = dist; // 更新前方最小距离
front_collision_point = collision_geom_point; // 更新前方碰撞点
}
}
}
// 计算后方碰撞点
double back_max_dist = -std::numeric_limits::max(); // 后方最大距离
geometry_msgs::msg::Point back_collision_point = front_collision_point; // 后方碰撞点
for (const auto & collision_poly : back_collision_polygons) {
for (const auto & collision_point : collision_poly.outer()) {
const auto collision_geom_point = toGeomPoint(collision_point); // 将碰撞点转换为几何点
const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment(
traj_points, back_seg_idx, collision_geom_point); // 计算纵向偏移
if (back_max_dist < dist) {
back_max_dist = dist; // 更新后方最大距离
back_collision_point = collision_geom_point; // 更新后方碰撞点
}
}
}
return SlowDownObstacle{
obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, obstacle.longitudinal_velocity,
obstacle.approach_velocity, precise_lat_dist, front_collision_point, back_collision_point}; // 返回减速障碍物
}
// 创建点云的减速障碍物
std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPointCloud(
const Obstacle & obstacle, const double precise_lat_dist)
{
if (!enable_slow_down_planning_ || !use_pointcloud_for_slow_down_) {
return std::nullopt; // 如果不启用减速规划或不使用点云进行减速，返回空值
}
if (!obstacle.slow_down_front_collision_point) {
return std::nullopt; // 如果没有减速前方碰撞点，返回空值
}
auto front_collision_point = *obstacle.slow_down_front_collision_point; // 获取减速前方碰撞点
auto back_collision_point =
obstacle.slow_down_back_collision_point.value_or(front_collision_point); // 获取减速后方碰撞点
return SlowDownObstacle{
obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, {}, {}, precise_lat_dist,
front_collision_point, back_collision_point}; // 返回减速障碍物
}
// 检查目标障碍物的一致性
void ObstacleCruisePlannerNode::checkConsistency(
const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
std::vector & stop_obstacles)
{
for (const auto & prev_closest_stop_obstacle : prev_closest_stop_object_obstacles_) {
const auto predicted_object_itr = std::find_if(
predicted_objects.objects.begin(), predicted_objects.objects.end(),
[&prev_closest_stop_obstacle](const PredictedObject & po) {
return autoware::universe_utils::toHexString(po.object_id) ==
prev_closest_stop_obstacle.uuid; // 查找匹配的预测对象
});
// 如果之前的最近停止障碍物从感知结果中消失，不再处理
if (predicted_object_itr == predicted_objects.objects.end()) {
continue;
}
const auto is_disappeared_from_stop_obstacle = std::none_of(
stop_obstacles.begin(), stop_obstacles.end(),
[&prev_closest_stop_obstacle](const StopObstacle & so) {
return so.uuid == prev_closest_stop_obstacle.uuid; // 检查是否从停止障碍物列表中消失
});
if (is_disappeared_from_stop_obstacle) {
// 重新评估是否为停止候选，如果满足“保持停止”条件，则覆盖当前决策
const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); // 计算经过时间
if (
predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
stop_obstacles.push_back(prev_closest_stop_obstacle); // 将之前的最近停止障碍物添加到列表
}
}
}
prev_closest_stop_object_obstacles_ =
obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); // 更新之前的最近停止障碍物
}
// 判断障碍物是否横穿轨迹
bool ObstacleCruisePlannerNode::isObstacleCrossing(
const std::vector & traj_points, const Obstacle & obstacle) const
{
const double diff_angle = calcDiffAngleAgainstTrajectory(traj_points, obstacle.pose); // 计算角度差
// 判断障碍物是否横穿轨迹
const bool is_obstacle_crossing_trajectory =
behavior_determination_param_.crossing_obstacle_traj_angle_threshold < std::abs(diff_angle) &&
behavior_determination_param_.crossing_obstacle_traj_angle_threshold <
M_PI - std::abs(diff_angle);
if (!is_obstacle_crossing_trajectory) {
return false; // 如果不横穿轨迹，返回false
}
// 只有高速横穿轨迹的障碍物才被考虑
return true;
}
// 计算碰撞时间裕度
double ObstacleCruisePlannerNode::calcCollisionTimeMargin(
const Odometry & odometry, const std::vector & collision_points,
const std::vector & traj_points, const bool is_driving_forward) const
{
const auto & p = behavior_determination_param_; // 获取行为决策参数
const double abs_ego_offset =
min_behavior_stop_margin_ + (is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m)); // 计算自车偏移量
const double time_to_reach_stop_point = calcTimeToReachCollisionPoint(
odometry, collision_points.front().point, traj_points, abs_ego_offset); // 计算到达碰撞点的时间
const double time_to_leave_collision_point =
time_to_reach_stop_point +
abs_ego_offset /
std::max(p.min_velocity_to_reach_collision_point, odometry.twist.twist.linear.x); // 计算离开碰撞点的时间
const double time_to_start_cross =
(rclcpp::Time(collision_points.front().stamp) - now()).seconds(); // 计算开始横穿的时间
const double time_to_end_cross = (rclcpp::Time(collision_points.back().stamp) - now()).seconds(); // 计算结束横穿的时间
if (time_to_leave_collision_point < time_to_start_cross) { // 如果自车先到达碰撞点
return time_to_start_cross - time_to_reach_stop_point; // 返回开始横穿的时间裕度
}
if (time_to_end_cross < time_to_reach_stop_point) { // 如果障碍物先到达碰撞点
return time_to_reach_stop_point - time_to_end_cross; // 返回结束横穿的时间裕度
}
return 0.0; // 如果自车和障碍物会碰撞，返回0
}
// 创建规划数据
PlannerData ObstacleCruisePlannerNode::createPlannerData(
const Odometry & odometry, const AccelWithCovarianceStamped & acc,
const std::vector & traj_points) const
{
PlannerData planner_data; // 创建规划数据
planner_data.current_time = now(); // 设置当前时间
planner_data.traj_points = traj_points; // 设置轨迹点
planner_data.ego_pose = odometry.pose.pose; // 设置自车姿态
planner_data.ego_vel = odometry.twist.twist.linear.x; // 设置自车速度
planner_data.ego_acc = acc.accel.accel.linear.x; // 设置自车加速度
planner_data.is_driving_forward = is_driving_forward_; // 设置是否向前行驶
return planner_data; // 返回规划数据
}
// 发布速度限制
void ObstacleCruisePlannerNode::publishVelocityLimit(
const std::optional & vel_limit, const std::string & module_name)
{
if (vel_limit) {
vel_limit_pub_->publish(*vel_limit); // 发布速度限制
need_to_clear_vel_limit_.at(module_name) = true; // 设置需要清除速度限制
return;
}
if (!need_to_clear_vel_limit_.at(module_name)) {
return; // 如果不需要清除速度限制，直接返回
}
// 清除速度限制
const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now(), module_name); // 创建清除速度限制命令
clear_vel_limit_pub_->publish(clear_vel_limit_msg); // 发布清除速度限制命令
need_to_clear_vel_limit_.at(module_name) = false; // 设置不需要清除速度限制
}
// 发布调试标记
void ObstacleCruisePlannerNode::publishDebugMarker() const
{
stop_watch_.tic(__func__); // 开始计时
// 1. 发布调试标记
MarkerArray debug_marker; // 创建调试标记数组
// 巡航障碍物
std::vector stop_collision_points; // 停止碰撞点
for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) {
// 障碍物
const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker(
debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.6, 0.1); // 创建障碍物标记
debug_marker.markers.push_back(obstacle_marker); // 添加到调试标记数组
// 碰撞点
for (size_t j = 0; j < debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.size();
++j) {
stop_collision_points.push_back(
debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.at(j).point); // 添加碰撞点
}
}
for (size_t i = 0; i < stop_collision_points.size(); ++i) {
auto collision_point_marker = autoware::universe_utils::createDefaultMarker(
"map", now(), "cruise_collision_points", i, Marker::SPHERE,
autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25),
autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); // 创建碰撞点标记
collision_point_marker.pose.position = stop_collision_points.at(i); // 设置碰撞点位置
debug_marker.markers.push_back(collision_point_marker); // 添加到调试标记数组
}
// 停止障碍物
for (size_t i = 0; i < debug_data_ptr_->obstacles_to_stop.size(); ++i) {
// 障碍物
const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker(
debug_data_ptr_->obstacles_to_stop.at(i).pose, i, "obstacles_to_stop", 1.0, 0.0, 0.0); // 创建障碍物标记
debug_marker.markers.push_back(obstacle_marker); // 添加到调试标记数组
// 碰撞点
auto collision_point_marker = autoware::universe_utils::createDefaultMarker(
"map", now(), "stop_collision_points", 0, Marker::SPHERE,
autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25),
autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); // 创建碰撞点标记
collision_point_marker.pose.position =
debug_data_ptr_->obstacles_to_stop.at(i).collision_point; // 设置碰撞点位置
debug_marker.markers.push_back(collision_point_marker); // 添加到调试标记数组
}
// 减速障碍物
for (size_t i = 0; i < debug_data_ptr_->obstacles_to_slow_down.size(); ++i) {
// 障碍物
const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker(
debug_data_ptr_->obstacles_to_slow_down.at(i).pose, i, "obstacles_to_slow_down", 0.7, 0.7,
0.0); // 创建障碍物标记
debug_marker.markers.push_back(obstacle_marker); // 添加到调试标记数组
// 碰撞点
auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker(
"map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE,
autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25),
autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); // 创建前方碰撞点标记
front_collision_point_marker.pose.position =
debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; // 设置前方碰撞点位置
auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker(
"map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE,
autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25),
autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); // 创建后方碰撞点标记
back_collision_point_marker.pose.position =
debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; // 设置后方碰撞点位置
debug_marker.markers.push_back(front_collision_point_marker); // 添加前方碰撞点标记
debug_marker.markers.push_back(back_collision_point_marker); // 添加后方碰撞点标记
}
// 故意忽略的障碍物
for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) {
const auto marker = obstacle_cruise_utils::getObjectMarker(
debug_data_ptr_->intentionally_ignored_obstacles.at(i).pose, i,
"intentionally_ignored_obstacles", 0.0, 1.0, 0.0); // 创建障碍物标记
debug_marker.markers.push_back(marker); // 添加到调试标记数组
}
{ // 轨迹多边形
auto marker = autoware::universe_utils::createDefaultMarker(
"map", now(), "detection_polygons", 0, Marker::LINE_LIST,
autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0),
autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); // 创建多边形标记
for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) {
for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) {
const auto & current_point = detection_polygon.outer().at(dp_idx); // 当前点
const auto & next_point =
detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); // 下一个点
marker.points.push_back(
autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); // 添加当前点
marker.points.push_back(
autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); // 添加下一个点
}
}
debug_marker.markers.push_back(marker); // 添加到调试标记数组
}
// 减速调试墙标记
autoware::universe_utils::appendMarkerArray(
debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); // 添加减速调试墙标记
debug_marker_pub_->publish(debug_marker); // 发布调试标记
// 2. 发布虚拟墙（巡航和停止）
debug_cruise_wall_marker_pub_->publish(debug_data_ptr_->cruise_wall_marker); // 发布巡航虚拟墙
debug_stop_wall_marker_pub_->publish(debug_data_ptr_->stop_wall_marker); // 发布停止虚拟墙
debug_slow_down_wall_marker_pub_->publish(debug_data_ptr_->slow_down_wall_marker); // 发布减速虚拟墙
// 3. 打印处理时间
const double calculation_time = stop_watch_.toc(__func__); // 获取处理时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]",
__func__, calculation_time); // 打印处理时间
}
// 发布调试信息
void ObstacleCruisePlannerNode::publishDebugInfo() const
{
// 停止
const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now()); // 获取停止规划调试信息
debug_stop_planning_info_pub_->publish(stop_debug_msg); // 发布停止规划调试信息
// 巡航
const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now()); // 获取巡航规划调试信息
debug_cruise_planning_info_pub_->publish(cruise_debug_msg); // 发布巡航规划调试信息
// 减速
const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now()); // 获取减速规划调试信息
debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); // 发布减速规划调试信息
}
// 发布处理时间
void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const
{
Float64Stamped calculation_time_msg; // 创建处理时间消息
calculation_time_msg.stamp = now(); // 设置时间戳
calculation_time_msg.data = calculation_time; // 设置处理时间
debug_calculation_time_pub_->publish(calculation_time_msg); // 发布处理时间
}
// 计算到达碰撞点的时间
double ObstacleCruisePlannerNode::calcTimeToReachCollisionPoint(
const Odometry & odometry, const geometry_msgs::msg::Point & collision_point,
const std::vector & traj_points, const double abs_ego_offset) const
{
const auto & p = behavior_determination_param_; // 获取行为决策参数
const double dist_from_ego_to_obstacle =
std::abs(autoware::motion_utils::calcSignedArcLength(
traj_points, odometry.pose.pose.position, collision_point)) -
abs_ego_offset; // 计算自车到碰撞点的距离
return dist_from_ego_to_obstacle /
std::max(p.min_velocity_to_reach_collision_point, std::abs(odometry.twist.twist.linear.x)); // 计算到达碰撞点的时间
}
} // namespace autoware::motion_planning
#include
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion_planning::ObstacleCruisePlannerNode) // 注册节点
代码功能概述
障碍物检测：从点云和目标检测中获取障碍物信息。
行为决策：根据障碍物类型和位置，决定自车的行为（停止、巡航、减速）。
轨迹规划：根据行为决策，生成相应的轨迹。
调试信息：发布调试标记和处理时间信息。
关键点
障碍物分类：根据目标类型（如车辆、行人、自行车等）进行分类。
碰撞检测：计算自车与障碍物之间的碰撞点和时间。
轨迹调整：根据碰撞检测结果调整轨迹，确保安全行驶。
调试功能：提供详细的调试信息，便于开发和测试。
planner_interface.cpp
// 版权声明，遵循Apache License 2.0协议
// 该代码由TIER IV, Inc.提供，遵循Apache License 2.0协议
#include "autoware/obstacle_cruise_planner/planner_interface.hpp" // 包含障碍物巡航规划器接口头文件
// 包含其他相关头文件，用于运动规划、信号处理等
#include "autoware/motion_utils/distance/distance.hpp"
#include "autoware/motion_utils/marker/marker_helper.hpp"
#include "autoware/motion_utils/resample/resample.hpp"
#include "autoware/motion_utils/trajectory/conversion.hpp"
#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/signal_processing/lowpass_filter_1d.hpp"
#include "autoware/universe_utils/ros/marker_helper.hpp"
#include // 包含Boost库的几何距离计算
#include // 包含Boost库的几何策略
#include // 包含C++标准库中的optional类型
#include // 包含C++标准库中的string类型
namespace // 匿名命名空间，用于定义局部函数和变量
{
// 创建StopSpeedExceeded消息的函数
StopSpeedExceeded createStopSpeedExceededMsg(
const rclcpp::Time & current_time, const bool stop_flag)
{
StopSpeedExceeded msg{}; // 创建一个StopSpeedExceeded消息实例
msg.stamp = current_time; // 设置时间戳
msg.stop_speed_exceeded = stop_flag; // 设置是否超过停止速度的标志
return msg; // 返回创建的消息
}
// 创建StopReasonArray消息的函数
tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(
const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose,
const StopObstacle & stop_obstacle)
{
// 创建消息头
std_msgs::msg::Header header;
header.frame_id = "map"; // 设置参考框架
header.stamp = planner_data.current_time; // 设置时间戳
// 创建停止因素
StopFactor stop_factor;
stop_factor.stop_pose = stop_pose; // 设置停止位置
geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; // 获取碰撞点
stop_factor_point.z = stop_pose.position.z; // 设置碰撞点的z坐标
stop_factor.dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength(
planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); // 计算到停止位置的距离
stop_factor.stop_factor_points.emplace_back(stop_factor_point); // 添加停止因素点
// 创建停止原因
StopReason stop_reason_msg;
stop_reason_msg.reason = StopReason::OBSTACLE_STOP; // 设置停止原因类型为障碍物停止
stop_reason_msg.stop_factors.emplace_back(stop_factor); // 添加停止因素
// 创建停止原因数组
StopReasonArray stop_reason_array;
stop_reason_array.header = header; // 设置消息头
stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); // 添加停止原因
return stop_reason_array; // 返回创建的停止原因数组
}
// 创建空的StopReasonArray消息的函数
StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time)
{
// 创建消息头
std_msgs::msg::Header header;
header.frame_id = "map"; // 设置参考框架
header.stamp = current_time; // 设置时间戳
// 创建停止原因
StopReason stop_reason_msg;
stop_reason_msg.reason = StopReason::OBSTACLE_STOP; // 设置停止原因类型为障碍物停止
// 创建停止原因数组
StopReasonArray stop_reason_array;
stop_reason_array.header = header; // 设置消息头
stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); // 添加停止原因
return stop_reason_array; // 返回创建的停止原因数组
}
// 创建VelocityFactorArray消息的函数
VelocityFactorArray makeVelocityFactorArray(
const rclcpp::Time & time, const std::optional pose = std::nullopt)
{
VelocityFactorArray velocity_factor_array; // 创建一个VelocityFactorArray消息实例
velocity_factor_array.header.frame_id = "map"; // 设置参考框架
velocity_factor_array.header.stamp = time; // 设置时间戳
if (pose) { // 如果提供了位置信息
using distance_type = VelocityFactor::_distance_type; // 定义距离类型
VelocityFactor velocity_factor; // 创建一个VelocityFactor消息实例
velocity_factor.behavior = PlanningBehavior::ROUTE_OBSTACLE; // 设置行为类型为路线障碍物
velocity_factor.pose = pose.value(); // 设置位置信息
velocity_factor.distance = std::numeric_limits::quiet_NaN(); // 设置距离为NaN
velocity_factor.status = VelocityFactor::UNKNOWN; // 设置状态为未知
velocity_factor.detail = std::string(); // 设置详细信息为空字符串
velocity_factor_array.factors.push_back(velocity_factor); // 将VelocityFactor添加到数组中
}
return velocity_factor_array; // 返回创建的VelocityFactorArray消息
}
// 计算最小停止距离的函数
double calcMinimumDistanceToStop(
const double initial_vel, const double max_acc, const double min_acc)
{
if (initial_vel < 0.0) { // 如果初始速度为负
return -std::pow(initial_vel, 2) / 2.0 / max_acc; // 返回计算的最小停止距离
}
return -std::pow(initial_vel, 2) / 2.0 / min_acc; // 返回计算的最小停止距离
}
// 从对象列表中根据UUID获取特定对象的函数
template
std::optional getObjectFromUuid(const std::vector & objects, const std::string & target_uuid)
{
const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) {
return object.uuid == target_uuid; // 查找UUID匹配的对象
});
if (itr == objects.end()) { // 如果未找到匹配的对象
return std::nullopt; // 返回空值
}
return *itr; // 返回找到的对象
}
// 计算到达目标时间的函数（二分查找法）
double findReachTime(
const double jerk, const double accel, const double velocity, const double distance,
const double t_min, const double t_max)
{
const double j = jerk; // 加速度变化率
const double a = accel; // 加速度
const double v = velocity; // 速度
const double d = distance; // 距离
const double min = t_min; // 最小时间
const double max = t_max; // 最大时间
auto f = [](const double t, const double j, const double a, const double v, const double d) {
return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; // 计算时间函数
};
if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { // 检查搜索范围是否有效
throw std::logic_error("[obstacle_cruise_planner](findReachTime): search range is invalid");
}
const double eps = 1e-5; // 精度阈值
const int warn_iter = 100; // 警告迭代次数
double lower = min; // 下界
double upper = max; // 上界
double t;
int iter = 0;
for (int i = 0;; i++) {
t = 0.5 * (lower + upper); // 计算中间值
const double fx = f(t, j, a, v, d); // 计算函数值
if (std::abs(fx) < eps) { // 如果函数值小于精度阈值
break; // 跳出循环
} else if (fx > 0.0) {
upper = t; // 更新上界
} else {
lower = t; // 更新下界
}
iter++;
if (iter > warn_iter) // 如果迭代次数超过警告阈值
std::cerr << "[obstacle_cruise_planner](findReachTime): current iter is over warning"
<< std::endl;
}
return t; // 返回计算的时间
}
// 根据距离计算减速后的速度
double calcDecelerationVelocityFromDistanceToTarget(
const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel,
const double current_velocity, const double distance_to_target)
{
if (max_slowdown_jerk > 0 || max_slowdown_accel > 0) { // 检查减速参数是否有效
throw std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative");
}
// case0: 目标距离在自身后方
if (distance_to_target <= 0) return current_velocity;
auto ft = [](const double t, const double j, const double a, const double v, const double d) {
return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; // 计算时间函数
};
auto vt = [](const double t, const double j, const double a, const double v) {
return j * t * t / 2.0 + a * t + v; // 计算速度函数
};
const double j_max = max_slowdown_jerk; // 最大减速率
const double a0 = current_accel; // 当前加速度
const double a_max = max_slowdown_accel; // 最大减速度
const double v0 = current_velocity; // 当前速度
const double l = distance_to_target; // 目标距离
const double t_const_jerk = (a_max - a0) / j_max; // 计算恒定减速时间
const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0); // 计算恒定减速停止距离
const double d_const_acc_stop = l - d_const_jerk_stop; // 计算恒定减速度停止距离
if (d_const_acc_stop < 0) {
// case0: 目标距离在恒定减速距离内
// 使用二分查找法代替求解三次方程
const double t_jerk = findReachTime(j_max, a0, v0, l, 0, t_const_jerk);
const double velocity = vt(t_jerk, j_max, a0, v0);
return velocity;
} else {
const double v1 = vt(t_const_jerk, j_max, a0, v0); // 计算恒定减速度后的速度
const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; // 计算停止判别式
// case3: 目标距离大于停止距离
if (discriminant_of_stop <= 0) {
return 0.0;
}
// case2: 目标距离在恒定减速度内
// 解方程 d = 0.5*a^2+v*t
const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max;
return vt(t_acc, 0.0, a_max, v1);
}
return current_velocity;
}
// 对轨迹点进行重采样的函数
std::vector resampleTrajectoryPoints(
const std::vector & traj_points, const double interval)
{
const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); // 将轨迹点转换为轨迹
const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); // 对轨迹进行重采样
return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); // 将重采样后的轨迹转换为轨迹点数组
}
// 将geometry_msgs::msg::Point转换为autoware::universe_utils::Point2d的函数
autoware::universe_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p)
{
return autoware::universe_utils::Point2d{p.x, p.y}; // 返回转换后的二维点
}
} // namespace
// 生成停止轨迹的函数
std::vector PlannerInterface::generateStopTrajectory(
const PlannerData & planner_data, const std::vector & stop_obstacles)
{
stop_watch_.tic(__func__); // 开始计时
stop_planning_debug_info_.reset(); // 重置停止规划调试信息
stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.ego_vel); // 设置自身速度
stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.ego_acc); // 设置自身加速度
const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m); // 计算自身偏移量
if (stop_obstacles.empty()) { // 如果没有停止障碍物
stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); // 发布空的停止原因数组
velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time)); // 发布速度因子数组
// 删除虚拟墙标记
const auto markers =
autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
prev_stop_distance_info_ = std::nullopt; // 清空之前的停止距离信息
return planner_data.traj_points; // 返回原始轨迹点
}
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
"stop planning"); // 输出调试信息
std::optional determined_stop_obstacle{}; // 定义确定的停止障碍物
std::optional determined_zero_vel_dist{}; // 定义确定的零速度距离
std::optional determined_desired_margin{}; // 定义确定的期望安全距离
const auto closest_stop_obstacles =
obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); // 获取最近的停止障碍物
for (const auto & stop_obstacle : closest_stop_obstacles) { // 遍历最近的停止障碍物
const auto ego_segment_idx =
ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); // 获取自身所在轨迹段索引
const double dist_to_collide_on_ref_traj =
autoware::motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) +
stop_obstacle.dist_to_collide_on_decimated_traj; // 计算在参考轨迹上碰撞点的距离
const double desired_margin = [&]() {
const double margin_from_obstacle =
calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle); // 计算从障碍物的安全距离
// 使用终端安全距离作为障碍物停止的安全距离
const auto ref_traj_length = autoware::motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, planner_data.traj_points.size() - 1); // 计算参考轨迹长度
if (dist_to_collide_on_ref_traj > ref_traj_length) {
return longitudinal_info_.terminal_safe_distance_margin;
}
// 如果行为停止点在最近的障碍物停止点前方一定范围内，则将最近的障碍物停止距离设置为最近的行为停止距离
const auto closest_behavior_stop_idx = autoware::motion_utils::searchZeroVelocityIndex(
planner_data.traj_points, ego_segment_idx + 1); // 查找最近的行为停止点索引
if (closest_behavior_stop_idx) {
const double closest_behavior_stop_dist_on_ref_traj =
autoware::motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, *closest_behavior_stop_idx); // 计算最近的行为停止点距离
const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj -
(dist_to_collide_on_ref_traj - margin_from_obstacle); // 计算停止距离差
if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) {
return min_behavior_stop_margin_; // 返回最小行为停止安全距离
}
}
return margin_from_obstacle; // 返回从障碍物的安全距离
}();
// 计算针对障碍物的停止点
double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin); // 计算候选零速度距离
if (suppress_sudden_obstacle_stop_) { // 如果抑制突然停止
const auto acceptable_stop_acc = [&]() -> std::optional {
if (stop_param_.getParamType(stop_obstacle.classification) == "default") { // 如果是默认参数类型
return longitudinal_info_.limit_min_accel; // 返回最小加速度限制
}
const double distance_to_judge_suddenness = std::min(
calcMinimumDistanceToStop(
planner_data.ego_vel, longitudinal_info_.limit_max_accel,
stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold),
stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); // 计算判断突然性的距离
if (candidate_zero_vel_dist > distance_to_judge_suddenness) { // 如果候选零速度距离大于判断距离
return longitudinal_info_.limit_min_accel; // 返回最小加速度限制
}
if (stop_param_.getParam(stop_obstacle.classification).abandon_to_stop) { // 如果放弃停止
RCLCPP_WARN(
rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"),
"[Cruise] abandon to stop against %s object",
stop_param_.types_maps.at(stop_obstacle.classification.label).c_str());
return std::nullopt; // 返回空值
} else {
return stop_param_.getParam(stop_obstacle.classification).limit_min_acc; // 返回最小加速度限制
}
}();
if (!acceptable_stop_acc) { // 如果没有可接受的停止加速度
continue; // 跳过当前障碍物
}
const double acceptable_stop_pos =
autoware::motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, planner_data.ego_pose.position) +
calcMinimumDistanceToStop(
planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value()); // 计算可接受的停止位置
if (acceptable_stop_pos > candidate_zero_vel_dist) { // 如果可接受的停止位置大于候选零速度距离
candidate_zero_vel_dist = acceptable_stop_pos; // 更新候选零速度距离
}
}
if (determined_stop_obstacle) { // 如果已经确定了停止障碍物
const bool is_same_param_types =
(stop_obstacle.classification.label == determined_stop_obstacle->classification.label); // 判断是否为相同参数类型
if (
(is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj >
determined_stop_obstacle->dist_to_collide_on_decimated_traj) ||
(!is_same_param_types && candidate_zero_vel_dist > determined_zero_vel_dist)) {
continue; // 如果当前障碍物的碰撞距离更大，则跳过
}
}
determined_zero_vel_dist = candidate_zero_vel_dist; // 更新确定的零速度距离
determined_stop_obstacle = stop_obstacle; // 更新确定的停止障碍物
determined_desired_margin = desired_margin; // 更新确定的期望安全距离
}
if (!determined_zero_vel_dist) { // 如果没有确定的零速度距离
// 删除虚拟墙标记
const auto markers =
autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
prev_stop_distance_info_ = std::nullopt; // 清空之前的停止距离信息
return planner_data.traj_points; // 返回原始轨迹点
}
// 如果需要，保持之前的停止距离
if (
std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
prev_stop_distance_info_) {
// 假设当前轨迹的前点在之前的轨迹前点前方
const size_t traj_front_point_prev_seg_idx =
autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
const double diff_dist_front_points = autoware::motion_utils::calcSignedArcLength(
prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
traj_front_point_prev_seg_idx);
const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
if (
std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) <
longitudinal_info_.hold_stop_distance_threshold) {
determined_zero_vel_dist.value() = prev_zero_vel_dist; // 更新确定的零速度距离
}
}
// 插入停止点
auto output_traj_points = planner_data.traj_points; // 复制轨迹点
const auto zero_vel_idx =
autoware::motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); // 插入停止点
if (zero_vel_idx) {
// 创建虚拟墙标记
const auto markers = autoware::motion_utils::createStopVirtualWallMarker(
output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0,
abs_ego_offset, "", planner_data.is_driving_forward);
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // 添加需要停止的障碍物
// 发布停止原因
const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; // 获取停止位置
const auto stop_reasons_msg =
makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); // 创建停止原因数组
stop_reasons_pub_->publish(stop_reasons_msg); // 发布停止原因
velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); // 发布速度因子数组
// 存储停止原因调试数据
debug_data_ptr_->stop_metrics =
makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); // 创建停止指标
// 发布如果自身车辆将以限制加速度超过停止点
const bool will_over_run = determined_zero_vel_dist.value() >
autoware::motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, planner_data.ego_pose.position) +
determined_stop_obstacle->dist_to_collide_on_decimated_traj +
determined_desired_margin.value() + 0.1; // 判断是否会超过停止点
const auto stop_speed_exceeded_msg =
createStopSpeedExceededMsg(planner_data.current_time, will_over_run); // 创建停止速度超过消息
stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); // 发布停止速度超过消息
prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value()); // 更新之前的停止距离信息
}
stop_planning_debug_info_.set(
StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE,
determined_stop_obstacle->dist_to_collide_on_decimated_traj); // 设置当前障碍物距离
stop_planning_debug_info_.set(
StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY,
determined_stop_obstacle->velocity); // 设置当前障碍物速度
stop_planning_debug_info_.set(
StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, determined_desired_margin.value()); // 设置目标障碍物距离
stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); // 设置目标速度
stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); // 设置目标加速度
const double calculation_time = stop_watch_.toc(__func__); // 计算运行时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::SlowDownPlanner"), enable_calculation_time_info_,
" %s := %f [ms]", __func__, calculation_time); // 输出运行时间信息
return output_traj_points; // 返回生成的停止轨迹点
}
// 计算从障碍物的安全距离的函数
double PlannerInterface::calculateMarginFromObstacleOnCurve(
const PlannerData & planner_data, const StopObstacle & stop_obstacle) const
{
if (!enable_approaching_on_curve_ || use_pointcloud_) { // 如果未启用曲线接近或使用点云
return longitudinal_info_.safe_distance_margin; // 返回默认安全距离
}
const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m); // 计算自身偏移量
// 计算到障碍物的短轨迹点
const size_t obj_segment_idx = autoware::motion_utils::findNearestSegmentIndex(
planner_data.traj_points, stop_obstacle.collision_point); // 获取障碍物所在轨迹段索引
std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; // 创建短轨迹点
double sum_short_traj_length{0.0}; // 短轨迹长度
for (int i = obj_segment_idx; 0 <= i; --i) { // 从障碍物位置向前遍历
short_traj_points.push_back(planner_data.traj_points.at(i)); // 添加轨迹点
if (
1 < short_traj_points.size() &&
longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) {
break; // 如果短轨迹长度超过安全距离，则停止
}
sum_short_traj_length += autoware::universe_utils::calcDistance2d(
planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); // 累加短轨迹长度
}
std::reverse(short_traj_points.begin(), short_traj_points.end()); // 反转短轨迹点
if (short_traj_points.size() < 2) { // 如果短轨迹点数量不足
return longitudinal_info_.safe_distance_margin; // 返回默认安全距离
}
// 计算从自身位置到障碍物的直线距离
const auto calculate_distance_from_straight_ego_path =
[&](const auto & ego_pose, const auto & object_polygon) {
const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose(
ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); // 计算前方位置
const auto ego_straight_segment = autoware::universe_utils::Segment2d{
convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; // 创建直线段
return boost::geometry::distance(ego_straight_segment, object_polygon); // 计算直线段与障碍物的距离
};
const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); // 对短轨迹点进行重采样
const auto object_polygon =
autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); // 将障碍物转换为多边形
const auto collision_idx = [&]() -> std::optional {
for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { // 遍历重采样后的短轨迹点
const double dist_to_obj = calculate_distance_from_straight_ego_path(
resampled_short_traj_points.at(i).pose, object_polygon); // 计算与障碍物的距离
if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { // 如果距离小于车辆宽度的一半
return i; // 返回碰撞索引
}
}
return std::nullopt; // 如果没有碰撞，则返回空值
}();
if (!collision_idx) { // 如果没有碰撞
return min_safe_distance_margin_on_curve_; // 返回最小安全距离
}
if (*collision_idx == 0) { // 如果碰撞索引为0
return longitudinal_info_.safe_distance_margin; // 返回默认安全距离
}
// 计算从障碍物的安全距离
const double partial_segment_length = [&]() {
const double collision_segment_length = autoware::universe_utils::calcDistance2d(
resampled_short_traj_points.at(*collision_idx - 1),
resampled_short_traj_points.at(*collision_idx)); // 计算碰撞段长度
const double prev_dist = calculate_distance_from_straight_ego_path(
resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); // 计算前一个点与障碍物的距离
const double next_dist = calculate_distance_from_straight_ego_path(
resampled_short_traj_points.at(*collision_idx).pose, object_polygon); // 计算当前点与障碍物的距离
return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) *
collision_segment_length; // 计算部分段长度
}();
const double short_margin_from_obstacle =
partial_segment_length +
autoware::motion_utils::calcSignedArcLength(
resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) -
abs_ego_offset + additional_safe_distance_margin_on_curve_; // 计算从障碍物的安全距离
return std::min(
longitudinal_info_.safe_distance_margin,
std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); // 返回最终的安全距离
}
// 计算到碰撞点的距离的函数
double PlannerInterface::calcDistanceToCollisionPoint(
const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point)
{
const double offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m); // 计算偏移量
const size_t ego_segment_idx =
ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); // 获取自身所在轨迹段索引
const size_t collision_segment_idx =
autoware::motion_utils::findNearestSegmentIndex(planner_data.traj_points, collision_point); // 获取碰撞点所在轨迹段索引
const auto dist_to_collision_point = autoware::motion_utils::calcSignedArcLength(
planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, collision_point,
collision_segment_idx); // 计算到碰撞点的距离
return dist_to_collision_point - offset; // 返回减去偏移量后的距离
}
// 生成减速轨迹的函数
std::vector PlannerInterface::generateSlowDownTrajectory(
const PlannerData & planner_data, const std::vector & cruise_traj_points,
const std::vector & obstacles,
[[maybe_unused]] std::optional & vel_limit)
{
stop_watch_.tic(__func__); // 开始计时
auto slow_down_traj_points = cruise_traj_points; // 复制巡航轨迹点
slow_down_debug_multi_array_ = Float32MultiArrayStamped(); // 初始化调试数据数组
const double dist_to_ego = [&]() {
const size_t ego_seg_idx =
ego_nearest_param_.findSegmentIndex(slow_down_traj_points, planner_data.ego_pose); // 获取自身所在轨迹段索引
return autoware::motion_utils::calcSignedArcLength(
slow_down_traj_points, 0, planner_data.ego_pose.position, ego_seg_idx); // 计算到自身位置的距离
}();
const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m); // 计算偏移量
// 定义在轨迹中插入减速点的函数
const auto insert_point_in_trajectory = [&](const double lon_dist) -> std::optional {
const auto inserted_idx =
autoware::motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); // 插入目标点
if (inserted_idx) {
if (inserted_idx.value() + 1 <= slow_down_traj_points.size() - 1) { // 如果插入点在轨迹范围内
// 对速度进行零阶保持插值
slow_down_traj_points.at(inserted_idx.value()).longitudinal_velocity_mps =
slow_down_traj_points.at(inserted_idx.value() + 1).longitudinal_velocity_mps;
}
return inserted_idx.value(); // 返回插入点索引
}
return std::nullopt; // 如果未插入，则返回空值
};
std::vector new_prev_slow_down_output; // 定义新的上一次减速输出
for (size_t i = 0; i < obstacles.size(); ++i) { // 遍历减速障碍物
const auto & obstacle = obstacles.at(i); // 获取当前障碍物
const auto prev_output = getObjectFromUuid(prev_slow_down_output_, obstacle.uuid); // 获取上一次的减速输出
const bool is_obstacle_moving = [&]() -> bool {
const auto object_vel_norm = std::hypot(obstacle.velocity, obstacle.lat_velocity); // 计算障碍物速度的模
if (!prev_output) return object_vel_norm > moving_object_speed_threshold; // 如果没有上一次输出，则判断速度是否超过阈值
if (prev_output->is_moving)
return object_vel_norm > moving_object_speed_threshold - moving_object_hysteresis_range; // 如果上一次障碍物是移动的，则判断速度是否超过阈值范围
return object_vel_norm > moving_object_speed_threshold + moving_object_hysteresis_range; // 如果上一次障碍物是静止的，则判断速度是否超过阈值范围
}();
// 计算减速开始距离，并插入减速速度
const auto dist_vec_to_slow_down = calculateDistanceToSlowDownWithConstraints(
planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego, is_obstacle_moving); // 计算减速距离
if (!dist_vec_to_slow_down) { // 如果减速距离无效
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_,
"[SlowDown] Ignore obstacle (%s) since distance to slow down is not valid",
obstacle.uuid.c_str());
continue; // 跳过当前障碍物
}
const auto dist_to_slow_down_start = std::get<0>(*dist_vec_to_slow_down); // 获取减速开始距离
const auto dist_to_slow_down_end = std::get<1>(*dist_vec_to_slow_down); // 获取减速结束距离
const auto feasible_slow_down_vel = std::get<2>(*dist_vec_to_slow_down); // 获取可行的减速速度
// 计算减速结束距离，并插入减速速度
const auto slow_down_start_idx = insert_point_in_trajectory(dist_to_slow_down_start); // 插入减速开始点
const auto slow_down_end_idx = dist_to_slow_down_start < dist_to_slow_down_end
? insert_point_in_trajectory(dist_to_slow_down_end)
: std::nullopt; // 插入减速结束点
if (!slow_down_end_idx) { // 如果没有插入减速结束点
continue; // 跳过当前障碍物
}
// 计算减速速度
const double stable_slow_down_vel = [&]() {
if (prev_output) { // 如果有上一次输出
return autoware::signal_processing::lowpassFilter(
feasible_slow_down_vel, prev_output->target_vel, slow_down_param_.lpf_gain_slow_down_vel); // 使用低通滤波器计算稳定的减速速度
}
return feasible_slow_down_vel; // 如果没有上一次输出，则直接返回可行的减速速度
}();
// 在减速开始点和结束点之间插入减速速度
for (size_t j = (slow_down_start_idx ? *slow_down_start_idx : 0); j <= *slow_down_end_idx;
++j) {
auto & traj_point = slow_down_traj_points.at(j); // 获取轨迹点
traj_point.longitudinal_velocity_mps =
std::min(traj_point.longitudinal_velocity_mps, static_cast(stable_slow_down_vel)); // 更新轨迹点速度
}
// 添加调试数据
slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist); // 添加精确横向距离
slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start); // 添加减速开始距离
slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end); // 添加减速结束距离
slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel); // 添加可行的减速速度
slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); // 添加稳定的减速速度
slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0); // 添加减速开始点索引
slow_down_debug_multi_array_.data.push_back(*slow_down_end_idx); // 添加减速结束点索引
// 添加虚拟墙
if (slow_down_start_idx && slow_down_end_idx) { // 如果插入了减速开始点和结束点
const size_t ego_idx =
ego_nearest_param_.findIndex(slow_down_traj_points, planner_data.ego_pose); // 获取自身所在轨迹点索引
const size_t slow_down_wall_idx = [&]() {
if (ego_idx < *slow_down_start_idx) return *slow_down_start_idx; // 如果自身在减速开始点之前
if (ego_idx < *slow_down_end_idx) return ego_idx; // 如果自身在减速结束点之前
return *slow_down_end_idx; // 如果自身在减速结束点之后
}();
const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down",
planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); // 创建减速虚拟墙标记
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); // 添加到调试数据中
}
// 添加调试虚拟墙
if (slow_down_start_idx) { // 如果插入了减速开始点
const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start",
planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); // 创建减速开始虚拟墙标记
autoware::universe_utils::appendMarkerArray(
markers, &debug_data_ptr_->slow_down_debug_wall_marker); // 添加到调试数据中
}
if (slow_down_end_idx) { // 如果插入了减速结束点
const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end",
planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); // 创建减速结束虚拟墙标记
autoware::universe_utils::appendMarkerArray(
markers, &debug_data_ptr_->slow_down_debug_wall_marker); // 添加到调试数据中
}
// 添加调试数据
debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); // 添加需要减速的障碍物
if (!debug_data_ptr_->stop_metrics.has_value()) { // 如果没有停止指标
debug_data_ptr_->slow_down_metrics =
makeMetrics("PlannerInterface", "slow_down", planner_data); // 创建减速指标
}
// 更新上一次减速输出
new_prev_slow_down_output.push_back(SlowDownOutput{
obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx,
stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist, is_obstacle_moving});
}
// 更新上一次减速输出
prev_slow_down_output_ = new_prev_slow_down_output;
const double calculation_time = stop_watch_.toc(__func__); // 计算运行时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::SlowDownPlanner"), enable_calculation_time_info_,
" %s := %f [ms]", __func__, calculation_time); // 输出运行时间信息
return slow_down_traj_points; // 返回生成的减速轨迹点
}
// 计算减速速度的函数
double PlannerInterface::calculateSlowDownVelocity(
const SlowDownObstacle & obstacle, const std::optional & prev_output,
const bool is_obstacle_moving) const
{
const auto & p =
slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving); // 获取障碍物参数
const double stable_precise_lat_dist = [&]() {
if (prev_output) { // 如果有上一次输出
return autoware::signal_processing::lowpassFilter(
obstacle.precise_lat_dist, prev_output->precise_lat_dist,
slow_down_param_.lpf_gain_lat_dist); // 使用低通滤波器计算稳定的精确横向距离
}
return obstacle.precise_lat_dist; // 如果没有上一次输出，则直接返回精确横向距离
}();
const double ratio = std::clamp(
(std::abs(stable_precise_lat_dist) - p.min_lat_margin) / (p.max_lat_margin - p.min_lat_margin),
0.0, 1.0); // 计算比例
const double slow_down_vel =
p.min_ego_velocity + ratio * (p.max_ego_velocity - p.min_ego_velocity); // 计算减速速度
return slow_down_vel; // 返回减速速度
}
// 计算减速距离的函数
std::optional>
PlannerInterface::calculateDistanceToSlowDownWithConstraints(
const PlannerData & planner_data, const std::vector & traj_points,
const SlowDownObstacle & obstacle, const std::optional & prev_output,
const double dist_to_ego, const bool is_obstacle_moving) const
{
const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m); // 计算偏移量
const double obstacle_vel = obstacle.velocity; // 获取障碍物速度
// 计算减速速度
const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output, is_obstacle_moving);
// 计算碰撞点距离
const double dist_to_front_collision =
autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); // 前碰撞点距离
const double dist_to_back_collision =
autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); // 后碰撞点距离
// 计算到碰撞点的偏移距离（考虑相对速度）
const double relative_vel = planner_data.ego_vel - obstacle.velocity; // 计算相对速度
const double offset_dist_to_collision = [&]() {
if (dist_to_front_collision < dist_to_ego + abs_ego_offset) { // 如果前碰撞点在自身前方
return 0.0; // 返回0
}
// 强制相对速度为正（如果自身速度低于障碍物速度）
constexpr double min_relative_vel = 1.0;
const double time_to_collision = (dist_to_front_collision - dist_to_ego - abs_ego_offset) /
std::max(min_relative_vel, relative_vel); // 计算碰撞时间
constexpr double time_to_collision_margin = 1.0; // 碰撞时间裕度
const double cropped_time_to_collision =
std::max(0.0, time_to_collision - time_to_collision_margin); // 裁剪碰撞时间
return obstacle_vel * cropped_time_to_collision; // 返回偏移距离
}();
// 计算减速准备距离
const double min_slow_down_prepare_dist = 3.0; // 最小减速准备距离
const double slow_down_prepare_dist = std::max(
min_slow_down_prepare_dist, slow_down_vel * slow_down_param_.time_margin_on_target_velocity); // 计算减速准备距离
// 计算减速距离
const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision -
abs_ego_offset - dist_to_ego - slow_down_prepare_dist; // 减速距离
const double slow_down_dist =
dist_to_back_collision - dist_to_front_collision + slow_down_prepare_dist; // 减速总距离
// 计算减速开始和结束距离
const double dist_to_slow_down_start = dist_to_ego + deceleration_dist; // 减速开始距离
const double dist_to_slow_down_end = dist_to_ego + deceleration_dist + slow_down_dist; // 减速结束距离
if (100.0 < dist_to_slow_down_start) { // 如果减速开始距离过大
// 注意：减速距离过远
return std::nullopt; // 返回空值
}
// 对减速开始和结束距离应用低通滤波器
const auto apply_lowpass_filter = [&](const double dist_to_slow_down, const auto prev_point) {
if (prev_output && prev_point) { // 如果有上一次输出和前一个点
const size_t seg_idx =
autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); // 获取前一个点所在轨迹段索引
const double prev_dist_to_slow_down =
autoware::motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); // 计算上一次的减速距离
return autoware::signal_processing::lowpassFilter(
dist_to_slow_down, prev_dist_to_slow_down, slow_down_param_.lpf_gain_dist_to_slow_down); // 应用低通滤波器
}
return dist_to_slow_down; // 如果没有上一次输出，则直接返回当前距离
};
const double filtered_dist_to_slow_down_start =
apply_lowpass_filter(dist_to_slow_down_start, prev_output->start_point); // 对减速开始距离应用低通滤波器
const double filtered_dist_to_slow_down_end =
apply_lowpass_filter(dist_to_slow_down_end, prev_output->end_point); // 对减速结束距离应用低通滤波器
// 计算考虑约束条件的速度
const double feasible_slow_down_vel = [&]() {
if (deceleration_dist < 0) { // 如果减速距离为负
if (prev_output) { // 如果有上一次输出
return prev_output->target_vel; // 返回上一次的目标速度
}
return std::max(planner_data.ego_vel, slow_down_vel); // 返回自身速度和减速速度的最大值
}
if (planner_data.ego_vel < slow_down_vel) { // 如果自身速度低于减速速度
return slow_down_vel; // 返回减速速度
}
const double one_shot_feasible_slow_down_vel = [&]() {
if (planner_data.ego_acc < longitudinal_info_.min_accel) { // 如果自身加速度低于最小加速度
const double squared_vel =
std::pow(planner_data.ego_vel, 2) + 2 * deceleration_dist * longitudinal_info_.min_accel; // 计算速度平方
if (squared_vel < 0) { // 如果速度平方为负
return slow_down_vel; // 返回减速速度
}
return std::max(std::sqrt(squared_vel), slow_down_vel); // 返回计算的速度和减速速度的最大值
}
// TODO(murooka) 更精确地计算最终加速度应为零
const double min_feasible_slow_down_vel = calcDecelerationVelocityFromDistanceToTarget(
longitudinal_info_.slow_down_min_jerk, longitudinal_info_.slow_down_min_accel,
planner_data.ego_acc, planner_data.ego_vel, deceleration_dist); // 计算最小可行的减速速度
return min_feasible_slow_down_vel; // 返回最小可行的减速速度
}();
if (prev_output) { // 如果有上一次输出
// 注意：如果纵向控制性不好，one_shot_slow_down_vel可能会变大，因为这里使用了实际的自身速度和加速度进行计算
// 在这里抑制one_shot_slow_down_vel变大
const double feasible_slow_down_vel =
std::min(one_shot_feasible_slow_down_vel, prev_output->feasible_target_vel); // 计算可行的减速速度
return std::max(slow_down_vel, feasible_slow_down_vel); // 返回减速速度和可行的减速速度的最大值
}
return std::max(slow_down_vel, one_shot_feasible_slow_down_vel); // 返回减速速度和一次减速速度的最大值
}();
return std::make_tuple(
filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); // 返回减速距离和速度
}
// 创建指标的函数
std::vector PlannerInterface::makeMetrics(
const std::string & module_name, const std::string & reason,
const std::optional & planner_data,
const std::optional & stop_pose,
const std::optional & stop_obstacle)
{
auto metrics = std::vector(); // 创建指标数组
// 创建状态
{
// 决策
Metric decision_metric;
decision_metric.name = module_name + "/decision"; // 指标名称
decision_metric.unit = "string"; // 指标单位
decision_metric.value = reason; // 指标值
metrics.push_back(decision_metric); // 添加到指标数组
}
if (stop_pose.has_value() && planner_data.has_value()) { // 如果有停止位置和规划数据
Metric stop_position_metric;
stop_position_metric.name = module_name + "/stop_position"; // 指标名称
stop_position_metric.unit = "string"; // 指标单位
const auto & p = stop_pose.value().position; // 获取停止位置
stop_position_metric.value =
"{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; // 指标值
metrics.push_back(stop_position_metric); // 添加到指标数组
Metric stop_orientation_metric;
stop_orientation_metric.name = module_name + "/stop_orientation"; // 指标名称
stop_orientation_metric.unit = "string"; // 指标单位
const auto & o = stop_pose.value().orientation; // 获取停止方向
stop_orientation_metric.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " +
std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; // 指标值
metrics.push_back(stop_orientation_metric); // 添加到指标数组
const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength(
planner_data.value().traj_points, planner_data.value().ego_pose.position,
stop_pose.value().position); // 计算到停止位置的距离
Metric dist_to_stop_pose_metric;
dist_to_stop_pose_metric.name = module_name + "/distance_to_stop_pose"; // 指标名称
dist_to_stop_pose_metric.unit = "double"; // 指标单位
dist_to_stop_pose_metric.value = std::to_string(dist_to_stop_pose); // 指标值
metrics.push_back(dist_to_stop_pose_metric); // 添加到指标数组
}
if (stop_obstacle.has_value()) { // 如果有停止障碍物
// 障碍物信息
Metric collision_point_metric;
const auto & p = stop_obstacle.value().collision_point; // 获取碰撞点
collision_point_metric.name = module_name + "/collision_point"; // 指标名称
collision_point_metric.unit = "string"; // 指标单位
collision_point_metric.value =
"{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; // 指标值
metrics.push_back(collision_point_metric); // 添加到指标数组
}
return metrics; // 返回指标数组
}
// 发布指标的函数
void PlannerInterface::publishMetrics(const rclcpp::Time & current_time)
{
// 创建指标数组
MetricArray metrics_msg;
metrics_msg.stamp = current_time; // 设置时间戳
auto addMetrics = [&metrics_msg](std::optional> & opt_metrics) {
if (opt_metrics) { // 如果有指标
metrics_msg.metric_array.insert(
metrics_msg.metric_array.end(), opt_metrics->begin(), opt_metrics->end()); // 添加到指标数组
}
};
addMetrics(debug_data_ptr_->stop_metrics); // 添加停止指标
addMetrics(debug_data_ptr_->slow_down_metrics); // 添加减速指标
addMetrics(debug_data_ptr_->cruise_metrics); // 添加巡航指标
metrics_pub_->publish(metrics_msg); // 发布指标
clearMetrics(); // 清空指标
}
// 清空指标的函数
void PlannerInterface::clearMetrics()
{
debug_data_ptr_->stop_metrics = std::nullopt; // 清空停止指标
debug_data_ptr_->slow_down_metrics = std::nullopt; // 清空减速指标
debug_data_ptr_->cruise_metrics = std::nullopt; // 清空巡航指标
}
optimization_based_planner.cpp
// Copyright 2022 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp"
#include "autoware/interpolation/linear_interpolation.hpp"
#include "autoware/interpolation/spline_interpolation.hpp"
#include "autoware/interpolation/zero_order_hold.hpp"
#include "autoware/motion_utils/marker/marker_helper.hpp"
#include "autoware/motion_utils/resample/resample.hpp"
#include "autoware/motion_utils/trajectory/interpolation.hpp"
#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/obstacle_cruise_planner/utils.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/ros/marker_helper.hpp"
#ifdef ROS_DISTRO_GALACTIC
#include
#else
#include
#endif
#include
constexpr double ZERO_VEL_THRESHOLD = 0.01; // 零速度阈值
constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; // 接近距离阈值
// 基于优化的规划器构造函数
OptimizationBasedPlanner::OptimizationBasedPlanner(
rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr)
: PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr)
{
smoothed_traj_sub_ = node.create_subscription(
"/planning/scenario_planning/trajectory", rclcpp::QoS{1},
[this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; });
// 参数初始化
dense_resampling_time_interval_ = node.declare_parameter(
"cruise.optimization_based_planner.dense_resampling_time_interval"); // 密集重采样时间间隔
sparse_resampling_time_interval_ = node.declare_parameter(
"cruise.optimization_based_planner.sparse_resampling_time_interval"); // 稀疏重采样时间间隔
dense_time_horizon_ =
node.declare_parameter("cruise.optimization_based_planner.dense_time_horizon"); // 密集时间范围
max_time_horizon_ =
node.declare_parameter("cruise.optimization_based_planner.max_time_horizon"); // 最大时间范围
t_dangerous_ = node.declare_parameter("cruise.optimization_based_planner.t_dangerous"); // 危险时间
velocity_margin_ =
node.declare_parameter("cruise.optimization_based_planner.velocity_margin"); // 速度裕度
replan_vel_deviation_ =
node.declare_parameter("cruise.optimization_based_planner.replan_vel_deviation"); // 重新规划速度偏差
engage_velocity_ =
node.declare_parameter("cruise.optimization_based_planner.engage_velocity"); // 启动速度
engage_acceleration_ =
node.declare_parameter("cruise.optimization_based_planner.engage_acceleration"); // 启动加速度
engage_exit_ratio_ =
node.declare_parameter("cruise.optimization_based_planner.engage_exit_ratio"); // 启动退出比例
stop_dist_to_prohibit_engage_ = node.declare_parameter(
"cruise.optimization_based_planner.stop_dist_to_prohibit_engage"); // 禁止启动的距离
const double max_s_weight =
node.declare_parameter("cruise.optimization_based_planner.max_s_weight"); // 最大s权重
const double max_v_weight =
node.declare_parameter("cruise.optimization_based_planner.max_v_weight"); // 最大v权重
const double over_s_safety_weight =
node.declare_parameter("cruise.optimization_based_planner.over_s_safety_weight"); // 超过s安全权重
const double over_s_ideal_weight =
node.declare_parameter("cruise.optimization_based_planner.over_s_ideal_weight"); // 超过s理想权重
const double over_v_weight =
node.declare_parameter("cruise.optimization_based_planner.over_v_weight"); // 超过v权重
const double over_a_weight =
node.declare_parameter("cruise.optimization_based_planner.over_a_weight"); // 超过a权重
const double over_j_weight =
node.declare_parameter("cruise.optimization_based_planner.over_j_weight"); // 超过j权重
// 速度优化器
velocity_optimizer_ptr_ = std::make_shared(
max_s_weight, max_v_weight, over_s_safety_weight, over_s_ideal_weight, over_v_weight,
over_a_weight, over_j_weight);
// 发布者
optimized_sv_pub_ = node.create_publisher("~/optimized_sv_trajectory", 1); // 优化后的sv轨迹
optimized_st_graph_pub_ = node.create_publisher("~/optimized_st_graph", 1); // 优化后的st图
boundary_pub_ = node.create_publisher("~/boundary", 1); // 边界
debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); // 调试墙标记
}
// 生成巡航轨迹
std::vector OptimizationBasedPlanner::generateCruiseTrajectory(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const std::vector & obstacles,
[[maybe_unused]] std::optional & vel_limit)
{
// 创建时间向量
const std::vector time_vec = createTimeVector();
if (time_vec.size() < 2) {
RCLCPP_ERROR(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"Resolution size is not enough");
prev_output_ = stop_traj_points;
return stop_traj_points;
}
// 获取轨迹上最近的点
const size_t closest_idx = findEgoSegmentIndex(stop_traj_points, planner_data.ego_pose);
// 计算最大速度
double v_max = 0.0;
for (const auto & point : stop_traj_points) {
v_max = std::max(v_max, static_cast(point.longitudinal_velocity_mps));
}
// 获取当前速度和加速度
double v0;
double a0;
std::tie(v0, a0) = calcInitialMotion(planner_data, stop_traj_points, prev_output_);
a0 = std::min(longitudinal_info_.max_accel, std::max(a0, longitudinal_info_.min_accel));
// 检查轨迹大小
if (stop_traj_points.size() - closest_idx <= 2) {
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"The number of points on the trajectory is too small or failed to calculate front offset");
prev_output_ = stop_traj_points;
return stop_traj_points;
}
// 检查是否到达目标
if (checkHasReachedGoal(planner_data, stop_traj_points)) {
prev_output_ = stop_traj_points;
return stop_traj_points;
}
// 获取障碍物的s边界
const auto s_boundaries = getSBoundaries(planner_data, stop_traj_points, obstacles, time_vec);
if (!s_boundaries) {
prev_output_ = stop_traj_points;
return stop_traj_points;
}
// 优化
VelocityOptimizer::OptimizationData data;
data.time_vec = time_vec;
data.s0 = 0.0;
data.a0 = a0;
data.v_max = v_max;
data.a_max = longitudinal_info_.max_accel;
data.a_min = longitudinal_info_.min_accel;
data.j_max = longitudinal_info_.max_jerk;
data.j_min = longitudinal_info_.min_jerk;
data.limit_a_max = longitudinal_info_.limit_max_accel;
data.limit_a_min = longitudinal_info_.limit_min_accel;
data.limit_j_max = longitudinal_info_.limit_max_jerk;
data.limit_j_min = longitudinal_info_.limit_min_jerk;
data.t_dangerous = t_dangerous_;
data.idling_time = longitudinal_info_.idling_time;
data.s_boundary = *s_boundaries;
data.v0 = v0;
RCLCPP_DEBUG(rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "v0 %f", v0);
const auto optimized_result = velocity_optimizer_ptr_->optimize(data);
// 发布调试轨迹
const double traj_front_to_vehicle_offset =
autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx);
publishDebugTrajectory(
planner_data, traj_front_to_vehicle_offset, time_vec, *s_boundaries, optimized_result);
// 从t转换为s
const auto processed_result =
processOptimizedResult(data.v0, optimized_result, traj_front_to_vehicle_offset);
if (!processed_result) {
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"Processed Result is empty");
prev_output_ = stop_traj_points;
return stop_traj_points;
}
const auto & opt_position = processed_result->s;
const auto & opt_velocity = processed_result->v;
// 检查大小
if (opt_position.size() == 1 && opt_velocity.front() < ZERO_VEL_THRESHOLD) {
auto output = stop_traj_points;
output.at(closest_idx).longitudinal_velocity_mps = data.v0;
for (size_t i = closest_idx + 1; i < output.size(); ++i) {
output.at(i).longitudinal_velocity_mps = 0.0;
}
prev_output_ = output;
debug_data_ptr_->cruise_metrics =
makeMetrics("OptimizationBasedPlanner", "cruise", planner_data);
return output;
} else if (opt_position.size() == 1) {
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"Optimized Trajectory is too small");
prev_output_ = stop_traj_points;
return stop_traj_points;
}
// 获取零速度位置
double closest_stop_dist = std::numeric_limits::max();
for (size_t i = 0; i < opt_velocity.size(); ++i) {
if (opt_velocity.at(i) < ZERO_VEL_THRESHOLD) {
closest_stop_dist = std::min(closest_stop_dist, opt_position.at(i));
break;
}
}
const auto traj_stop_dist =
autoware::motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx);
if (traj_stop_dist) {
closest_stop_dist = std::min(*traj_stop_dist + traj_front_to_vehicle_offset, closest_stop_dist);
}
// 重采样优化速度
size_t break_id = stop_traj_points.size();
std::vector resampled_opt_position;
for (size_t i = closest_idx; i < stop_traj_points.size(); ++i) {
const double query_s = std::max(
autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front());
if (query_s > opt_position.back()) {
break_id = i;
break;
}
resampled_opt_position.push_back(query_s);
}
// 为每个位置重采样优化速度
auto resampled_opt_velocity =
autoware::interpolation::lerp(opt_position, opt_velocity, resampled_opt_position);
for (size_t i = break_id; i < stop_traj_points.size(); ++i) {
resampled_opt_velocity.push_back(stop_traj_points.at(i).longitudinal_velocity_mps);
}
// 创建输出数据
std::vector output = stop_traj_points;
for (size_t i = 0; i < closest_idx; ++i) {
output.at(i).longitudinal_velocity_mps = data.v0;
}
for (size_t i = closest_idx; i < output.size(); ++i) {
output.at(i).longitudinal_velocity_mps =
resampled_opt_velocity.at(i - closest_idx) + velocity_margin_;
}
output.back().longitudinal_velocity_mps = 0.0; // 终端速度为零
// 插入最近的停止点
autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output);
debug_data_ptr_->cruise_metrics = makeMetrics("OptimizationBasedPlanner", "cruise", planner_data);
prev_output_ = output;
return output;
}
// 创建时间向量
std::vector OptimizationBasedPlanner::createTimeVector()
{
std::vector time_vec;
for (double t = 0.0; t < dense_time_horizon_; t += dense_resampling_time_interval_) {
time_vec.push_back(t);
}
if (dense_time_horizon_ - time_vec.back() < 1e-3) {
time_vec.back() = dense_time_horizon_;
} else {
time_vec.push_back(dense_time_horizon_);
}
for (double t = dense_time_horizon_ + sparse_resampling_time_interval_; t < max_time_horizon_;
t += sparse_resampling_time_interval_) {
time_vec.push_back(t);
}
if (max_time_horizon_ - time_vec.back() < 1e-3) {
time_vec.back() = max_time_horizon_;
} else {
time_vec.push_back(max_time_horizon_);
}
return time_vec;
}
// 计算初始运动状态（v0, a0）
std::tuple OptimizationBasedPlanner::calcInitialMotion(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const std::vector & prev_traj_points)
{
const auto & ego_vel = planner_data.ego_vel;
const auto & ego_pose = planner_data.ego_pose;
const auto & input_traj_points = stop_traj_points;
const double vehicle_speed{std::abs(ego_vel)};
const auto current_closest_point =
ego_nearest_param_.calcInterpolatedPoint(input_traj_points, planner_data.ego_pose);
const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)};
double initial_vel{};
double initial_acc{};
// 第一次运行
if (prev_traj_points.empty()) {
initial_vel = vehicle_speed;
initial_acc = 0.0;
return std::make_tuple(initial_vel, initial_acc);
}
TrajectoryPoint prev_output_closest_point;
if (smoothed_trajectory_ptr_) {
prev_output_closest_point = autoware::motion_utils::calcInterpolatedPoint(
*smoothed_trajectory_ptr_, ego_pose, ego_nearest_param_.dist_threshold,
ego_nearest_param_.yaw_threshold);
} else {
prev_output_closest_point =
ego_nearest_param_.calcInterpolatedPoint(prev_traj_points, ego_pose);
}
// 当速度跟踪偏差较大时
const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps};
const double vel_error{vehicle_speed - std::abs(desired_vel)};
if (std::abs(vel_error) > replan_vel_deviation_) {
initial_vel = vehicle_speed; // 使用当前车辆速度
initial_acc = prev_output_closest_point.acceleration_mps2;
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"calcInitialMotion : Large deviation error for speed control. Use current speed for "
"initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f",
desired_vel, vehicle_speed, vel_error, replan_vel_deviation_);
return std::make_tuple(initial_vel, initial_acc);
}
// 如果当前车辆速度较低且目标速度较高，使用启动速度
const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_;
if (vehicle_speed < engage_vel_thr) {
if (target_vel >= engage_velocity_) {
const auto stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint(
input_traj_points, ego_pose, ego_nearest_param_.dist_threshold,
ego_nearest_param_.yaw_threshold);
if (!stop_dist.has_value()) {
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use "
"engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f)",
vehicle_speed, target_vel, engage_velocity_, engage_vel_thr);
return std::make_tuple(engage_velocity_, engage_acceleration_); // 返回启动速度和加速度
} else if (stop_dist.value() > stop_dist_to_prohibit_engage_) {
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use "
"engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f",
vehicle_speed, target_vel, engage_velocity_, engage_vel_thr, stop_dist.value());
return std::make_tuple(engage_velocity_, engage_acceleration_); // 返回启动速度和加速度
} else {
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist.value());
}
} else if (target_vel > 0.0) {
auto clock{rclcpp::Clock{RCL_ROS_TIME}};
RCLCPP_WARN_THROTTLE(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), clock, 3000,
"calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ",
target_vel, engage_velocity_);
}
}
// 正常更新：使用上一次输出的最近点
initial_vel = prev_output_closest_point.longitudinal_velocity_mps; // 使用上一次输出的速度
initial_acc = prev_output_closest_point.acceleration_mps2; // 使用上一次输出的加速度
return std::make_tuple(initial_vel, initial_acc); // 返回初始速度和加速度
}
// 检查是否到达目标
bool OptimizationBasedPlanner::checkHasReachedGoal(
const PlannerData & planner_data, const std::vector & stop_traj_points)
{
// 如果目标接近且当前速度较低，则不优化轨迹
const auto closest_stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint(
stop_traj_points, planner_data.ego_pose, ego_nearest_param_.dist_threshold,
ego_nearest_param_.yaw_threshold); // 计算到最近停止点的距离
if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.ego_vel < 0.6) {
return true; // 如果距离小于0.5且速度小于0.6，返回true
}
return false; // 否则返回false
}
// 获取s边界
std::optional OptimizationBasedPlanner::getSBoundaries(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const std::vector & obstacles, const std::vector & time_vec)
{
if (stop_traj_points.empty()) {
return std::nullopt; // 如果停止轨迹点为空，返回空值
}
const auto traj_length =
calcTrajectoryLengthFromCurrentPose(stop_traj_points, planner_data.ego_pose); // 计算轨迹长度
if (!traj_length) {
return {}; // 如果计算失败，返回空值
}
SBoundaries s_boundaries(time_vec.size()); // 创建s边界
for (size_t i = 0; i < s_boundaries.size(); ++i) {
s_boundaries.at(i).max_s = *traj_length; // 设置最大s值
}
double min_slow_down_point_length = std::numeric_limits::max(); // 最小减速点长度
std::optional min_slow_down_idx = {}; // 最小减速点索引
for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) {
const auto & obj = obstacles.at(o_idx); // 获取障碍物
if (obj.collision_points.empty()) {
continue; // 如果碰撞点为空，跳过
}
// 从障碍物获取s边界
const auto obj_s_boundaries =
getSBoundaries(planner_data, stop_traj_points, obj, time_vec, *traj_length);
if (!obj_s_boundaries) {
continue; // 如果获取失败，跳过
}
// 更新s边界
for (size_t i = 0; i < obj_s_boundaries->size(); ++i) {
if (obj_s_boundaries->at(i).max_s < s_boundaries.at(i).max_s) {
s_boundaries.at(i) = obj_s_boundaries->at(i); // 更新最大s值
}
}
// 查找最近的障碍物以供rviz标记使用
const double obj_vel = std::abs(obj.velocity); // 获取障碍物速度
const double rss_dist = calcRSSDistance(planner_data.ego_vel, obj_vel); // 计算RSS距离
const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; // 安全距离裕度
const double ego_obj_length = autoware::motion_utils::calcSignedArcLength(
stop_traj_points, planner_data.ego_pose.position, obj.collision_points.front().point); // 计算自车到障碍物的长度
const double slow_down_point_length = ego_obj_length - (rss_dist + safe_distance_margin); // 计算减速点长度
if (slow_down_point_length < min_slow_down_point_length) {
min_slow_down_point_length = slow_down_point_length; // 更新最小减速点长度
min_slow_down_idx = o_idx; // 更新最小减速点索引
}
}
// 发布减速或停止的墙标记
if (min_slow_down_idx) {
const auto & current_time = planner_data.current_time; // 获取当前时间
const auto marker_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
stop_traj_points, planner_data.ego_pose.position, min_slow_down_point_length); // 计算标记位置
if (marker_pose) {
MarkerArray wall_msg; // 创建标记数组
const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
marker_pose.value(), "obstacle to follow", current_time, 0); // 创建减速虚拟墙标记
autoware::universe_utils::appendMarkerArray(markers, &wall_msg); // 添加标记到数组
// 发布rviz标记
debug_wall_marker_pub_->publish(wall_msg);
}
}
return s_boundaries; // 返回s边界
}
// 获取s边界
std::optional OptimizationBasedPlanner::getSBoundaries(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const CruiseObstacle & object, const std::vector & time_vec, const double traj_length)
{
if (object.collision_points.empty()) {
return {}; // 如果碰撞点为空，返回空值
}
const bool onEgoTrajectory =
checkOnTrajectory(planner_data, stop_traj_points, object.collision_points.front()); // 检查障碍物是否在自车轨迹上
const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; // 安全距离裕度
// 如果障碍物在当前自车轨迹上，假设障碍物沿着自车轨迹移动
if (onEgoTrajectory) {
RCLCPP_DEBUG(
rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
"On Trajectory Object");
return getSBoundariesForOnTrajectoryObject(
planner_data, time_vec, safe_distance_margin, object, traj_length); // 获取在轨迹上的s边界
}
// 忽略不在轨迹上的低速障碍物
return getSBoundariesForOffTrajectoryObject(
planner_data, time_vec, safe_distance_margin, object, traj_length); // 获取不在轨迹上的s边界
}
// 获取在轨迹上的s边界
std::optional OptimizationBasedPlanner::getSBoundariesForOnTrajectoryObject(
const PlannerData & planner_data, const std::vector & time_vec,
const double safety_distance, const CruiseObstacle & object, const double traj_length)
{
const double & min_object_accel_for_rss = longitudinal_info_.min_object_accel_for_rss; // 获取RSS的最小障碍物加速度
SBoundaries s_boundaries(time_vec.size()); // 创建s边界
for (size_t i = 0; i < s_boundaries.size(); ++i) {
s_boundaries.at(i).max_s = traj_length; // 设置每个时间点的最大s值为轨迹长度
}
const double v_obj = std::abs(object.velocity); // 获取障碍物的速度
const double dist_to_collision_point =
calcDistanceToCollisionPoint(planner_data, object.collision_points.front().point); // 计算到碰撞点的距离
double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); // 计算当前s值
const double current_v_obj = v_obj; // 当前障碍物速度
const double initial_s_upper_bound =
current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); // 计算初始s上界
s_boundaries.front().max_s = std::clamp(initial_s_upper_bound, 0.0, s_boundaries.front().max_s); // 限制s上界
s_boundaries.front().is_object = true; // 标记为障碍物
for (size_t i = 1; i < time_vec.size(); ++i) {
const double dt = time_vec.at(i) - time_vec.at(i - 1); // 计算时间间隔
current_s_obj = current_s_obj + current_v_obj * dt; // 更新s值
const double s_upper_bound =
current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); // 计算s上界
s_boundaries.at(i).max_s = std::clamp(s_upper_bound, 0.0, s_boundaries.at(i).max_s); // 限制s上界
s_boundaries.at(i).is_object = true; // 标记为障碍物
}
return s_boundaries; // 返回s边界
}
// 获取不在轨迹上的s边界
std::optional OptimizationBasedPlanner::getSBoundariesForOffTrajectoryObject(
const PlannerData & planner_data, const std::vector & time_vec,
const double safety_distance, const CruiseObstacle & object, const double traj_length)
{
const auto & current_time = planner_data.current_time; // 获取当前时间
const double & min_object_accel_for_rss = longitudinal_info_.min_object_accel_for_rss; // 获取RSS的最小障碍物加速度
const double v_obj = std::abs(object.velocity); // 获取障碍物的速度
SBoundaries s_boundaries(time_vec.size()); // 创建s边界
for (size_t i = 0; i < s_boundaries.size(); ++i) {
s_boundaries.at(i).max_s = traj_length; // 设置每个时间点的最大s值为轨迹长度
}
for (const auto & collision_point : object.collision_points) {
const double object_time = (rclcpp::Time(collision_point.stamp) - current_time).seconds(); // 计算障碍物的时间
if (object_time < 0) {
// 忽略过去的位置
continue;
}
const double dist_to_collision_point =
calcDistanceToCollisionPoint(planner_data, collision_point.point); // 计算到碰撞点的距离
if (!dist_to_collision_point) {
continue; // 如果计算失败，跳过
}
const double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); // 计算当前s值
const double s_upper_bound =
current_s_obj + (v_obj * v_obj) / (2 * std::fabs(min_object_accel_for_rss)); // 计算s上界
size_t object_time_segment_idx = 0; // 对象时间段索引
for (size_t i = 0; i < time_vec.size() - 1; ++i) {
if (time_vec.at(i) < object_time && object_time < time_vec.at(i + 1)) {
object_time_segment_idx = i; // 找到对象时间段索引
break;
}
}
for (size_t i = 0; i <= object_time_segment_idx + 1; ++i) {
if (time_vec.at(i) < object_time && s_upper_bound < s_boundaries.at(i).max_s) {
s_boundaries.at(i).max_s = std::max(0.0, s_upper_bound); // 更新s上界
s_boundaries.at(i).is_object = true; // 标记为障碍物
}
}
}
return s_boundaries; // 返回s边界
}
// 检查障碍物是否在轨迹上
bool OptimizationBasedPlanner::checkOnTrajectory(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const PointWithStamp & point)
{
// 如果碰撞点在未来，返回false
if ((rclcpp::Time(point.stamp) - planner_data.current_time).seconds() > 0.1) {
return false;
}
const double lateral_offset =
std::fabs(autoware::motion_utils::calcLateralOffset(stop_traj_points, point.point)); // 计算横向偏移
if (lateral_offset < vehicle_info_.max_lateral_offset_m + 0.1) {
return true; // 如果横向偏移小于阈值，返回true
}
return false; // 否则返回false
}
// 计算从自车到碰撞点的轨迹长度
std::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose(
const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose)
{
const size_t ego_segment_idx = ego_nearest_param_.findSegmentIndex(traj_points, ego_pose); // 找到自车轨迹段索引
const double traj_length = autoware::motion_utils::calcSignedArcLength(
traj_points, ego_pose.position, ego_segment_idx, traj_points.size() - 1); // 计算轨迹长度
const auto dist_to_closest_stop_point = autoware::motion_utils::calcDistanceToForwardStopPoint(
traj_points, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); // 计算到最近停止点的距离
if (dist_to_closest_stop_point) {
return std::min(traj_length, dist_to_closest_stop_point.value()); // 返回轨迹长度和最近停止点距离的最小值
}
return traj_length; // 返回轨迹长度
}
// 将基础链接转换为中心点
geometry_msgs::msg::Pose OptimizationBasedPlanner::transformBaseLink2Center(
const geometry_msgs::msg::Pose & pose_base_link)
{
tf2::Transform tf_map2base; // 创建TF变换
tf2::fromMsg(pose_base_link, tf_map2base); // 从基础链接姿态转换
// 设置在地图坐标系下的顶点
tf2::Vector3 base2center;
base2center.setX(std::abs(vehicle_info_.vehicle_length_m / 2.0 - vehicle_info_.rear_overhang_m)); // 计算中心点
base2center.setY(0.0); // 设置y坐标为0
base2center.setZ(0.0); // 设置z坐标为0
base2center.setW(1.0); // 设置w坐标为1
// 将顶点从地图坐标系转换为对象坐标系
const auto map2center = tf_map2base * base2center;
geometry_msgs::msg::Pose center_pose; // 创建中心点姿态
center_pose.position =
autoware::universe_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); // 设置位置
center_pose.orientation = pose_base_link.orientation; // 设置方向
return center_pose; // 返回中心点姿态
}
// 处理优化结果
std::optional
OptimizationBasedPlanner::processOptimizedResult(
const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset)
{
if (
opt_result.t.empty() || opt_result.s.empty() || opt_result.v.empty() || opt_result.a.empty() ||
opt_result.j.empty()) {
return std::nullopt; // 如果优化结果为空，返回空值
}
size_t break_id = opt_result.s.size(); // 初始化断点ID
VelocityOptimizer::OptimizationResult processed_result; // 创建处理结果
processed_result.t.push_back(0.0); // 添加时间0
processed_result.s.push_back(offset); // 添加偏移量
processed_result.v.push_back(v0); // 添加初始速度 processed_result.a.push_back(opt_result.a.front()); // 添加初始加速度
processed_result.j.push_back(opt_result.j.front()); // 添加初始加加速度
for (size_t i = 1; i < opt_result.s.size(); ++i) {
const double prev_s = processed_result.s.back(); // 获取上一个s值
const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; // 获取当前s值并加上偏移量
const double current_v = opt_result.v.at(i); // 获取当前速度
if (prev_s >= current_s) {
processed_result.v.back() = 0.0; // 如果当前s小于等于上一个s，将速度设置为0
break_id = i; // 更新断点ID
break; // 退出循环
} else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) {
processed_result.v.back() = current_v; // 如果s值变化小于阈值，保持当前速度
processed_result.s.back() = current_s; // 更新s值
continue; // 继续循环
}
processed_result.t.push_back(opt_result.t.at(i)); // 添加当前时间
processed_result.s.push_back(current_s); // 添加当前s值
processed_result.v.push_back(current_v); // 添加当前速度
processed_result.a.push_back(opt_result.a.at(i)); // 添加当前加速度
processed_result.j.push_back(opt_result.j.at(i)); // 添加当前加加速度
}
// 在断点之后填充零速度
for (size_t i = break_id; i < opt_result.s.size(); ++i) {
const double prev_s = processed_result.s.back(); // 获取上一个s值
const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; // 获取当前s值并加上偏移量
if (prev_s >= current_s) {
processed_result.v.back() = 0.0; // 如果当前s小于等于上一个s，将速度设置为0
continue; // 继续循环
} else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) {
processed_result.v.back() = 0.0; // 如果s值变化小于阈值，将速度设置为0
processed_result.s.back() = current_s; // 更新s值
continue; // 继续循环
}
processed_result.t.push_back(opt_result.t.at(i)); // 添加当前时间
processed_result.s.push_back(current_s); // 添加当前s值
processed_result.v.push_back(0.0); // 将速度设置为0
processed_result.a.push_back(0.0); // 将加速度设置为0
processed_result.j.push_back(0.0); // 将加加速度设置为0
}
return processed_result; // 返回处理后的结果
}
// 发布调试轨迹
void OptimizationBasedPlanner::publishDebugTrajectory(
const PlannerData & planner_data, const double offset, const std::vector & time_vec,
const SBoundaries & s_boundaries, const VelocityOptimizer::OptimizationResult & opt_result)
{
const auto & current_time = planner_data.current_time; // 获取当前时间
// 发布优化结果和边界
Trajectory boundary_traj; // 创建边界轨迹
boundary_traj.header.stamp = current_time; // 设置时间戳
boundary_traj.points.resize(s_boundaries.size()); // 设置轨迹点数量
double boundary_s_max = 0.0; // 初始化最大s值
for (size_t i = 0; i < s_boundaries.size(); ++i) {
const double bound_s = s_boundaries.at(i).max_s; // 获取s边界的最大值
const double bound_t = time_vec.at(i); // 获取时间
boundary_traj.points.at(i).pose.position.x = bound_s; // 设置轨迹点位置
boundary_traj.points.at(i).pose.position.y = bound_t; // 设置轨迹点位置
boundary_s_max = std::max(bound_s, boundary_s_max); // 更新最大s值
}
boundary_pub_->publish(boundary_traj); // 发布边界轨迹
Trajectory optimized_sv_traj; // 创建优化后的sv轨迹
optimized_sv_traj.header.stamp = current_time; // 设置时间戳
optimized_sv_traj.points.resize(opt_result.s.size()); // 设置轨迹点数量
for (size_t i = 0; i < opt_result.s.size(); ++i) {
const double s = opt_result.s.at(i); // 获取s值
const double v = opt_result.v.at(i); // 获取速度
optimized_sv_traj.points.at(i).pose.position.x = s + offset; // 设置轨迹点位置
optimized_sv_traj.points.at(i).pose.position.y = v; // 设置轨迹点速度
}
optimized_sv_pub_->publish(optimized_sv_traj); // 发布优化后的sv轨迹
Trajectory optimized_st_graph; // 创建优化后的st图
optimized_st_graph.header.stamp = current_time; // 设置时间戳
optimized_st_graph.points.resize(opt_result.s.size()); // 设置轨迹点数量
for (size_t i = 0; i < opt_result.s.size(); ++i) {
const double bound_s = opt_result.s.at(i); // 获取s值
const double bound_t = opt_result.t.at(i); // 获取时间
optimized_st_graph.points.at(i).pose.position.x = bound_s; // 设置轨迹点位置
optimized_st_graph.points.at(i).pose.position.y = bound_t; // 设置轨迹点时间
}
optimized_st_graph_pub_->publish(optimized_st_graph); // 发布优化后的st图
}
代码功能概述
障碍物检测：从传感器数据（如点云和预测对象）中检测障碍物。
行为决策：根据障碍物的位置和速度，决定自车的行为（停止、巡航、减速）。
轨迹生成：使用优化算法生成安全的巡航轨迹。
调试信息：发布调试标记和处理时间信息，以便于开发和测试。
关键点
障碍物分类：根据目标类型（如车辆、行人、自行车等）进行分类。
碰撞检测：计算自车与障碍物之间的碰撞点和时间。
轨迹调整：根据碰撞检测结果调整轨迹，确保安全行驶。
调试功能：提供详细的调试信息，便于开发和测试。
velocity_optimizer.cpp
// 版权声明，遵循Apache License 2.0协议
// 该代码由TIER IV, Inc.提供
#include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" // 包含PID基础规划器头文件
// 包含其他相关头文件，用于插值、标记辅助、几何计算等
#include "autoware/interpolation/spline_interpolation.hpp"
#include "autoware/motion_utils/marker/marker_helper.hpp"
#include "autoware/obstacle_cruise_planner/utils.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/ros/marker_helper.hpp"
#include "autoware/universe_utils/ros/update_param.hpp"
#include "tier4_planning_msgs/msg/velocity_limit.hpp" // 包含速度限制消息头文件
using autoware::signal_processing::LowpassFilter1d; // 使用一维低通滤波器
namespace // 匿名命名空间，用于定义局部函数和变量
{
// 创建速度限制消息的函数
VelocityLimit createVelocityLimitMsg(
const rclcpp::Time & current_time, const double vel, const double acc, const double max_jerk,
const double min_jerk)
{
VelocityLimit msg; // 创建速度限制消息实例
msg.stamp = current_time; // 设置时间戳
msg.sender = "obstacle_cruise_planner.cruise"; // 设置发送者
msg.use_constraints = true; // 启用约束
msg.max_velocity = vel; // 设置最大速度
if (acc < 0) { // 如果加速度为负
msg.constraints.min_acceleration = acc; // 设置最小加速度
}
msg.constraints.max_jerk = max_jerk; // 设置最大加速度变化率
msg.constraints.min_jerk = min_jerk; // 设置最小加速度变化率
return msg; // 返回创建的消息
}
// 获取符号的模板函数
template
T getSign(const T val)
{
if (0 < val) { // 如果值大于0
return static_cast(1); // 返回1
} else if (val < 0) { // 如果值小于0
return static_cast(-1); // 返回-1
}
return static_cast(0); // 如果值为0，返回0
}
} // namespace
// PID基础规划器类
PIDBasedPlanner::PIDBasedPlanner(
rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr)
: PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr)
{
// 从参数服务器获取巡航时的最小加速度
min_accel_during_cruise_ =
node.declare_parameter("cruise.pid_based_planner.min_accel_during_cruise");
// 是否使用基于速度限制的规划器
use_velocity_limit_based_planner_ =
node.declare_parameter("cruise.pid_based_planner.use_velocity_limit_based_planner");
// 配置基于速度限制的规划器参数
{ // velocity limit based planner
const double kp =
node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.kp");
const double ki =
node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.ki");
const double kd =
node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.kd");
velocity_limit_based_planner_param_.pid_vel_controller =
std::make_unique(kp, ki, kd); // 创建PID控制器
velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel");
velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.vel_to_acc_weight");
velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc =
node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc");
velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration");
}
// 配置基于速度插入的规划器参数
{ // velocity insertion based planner
// 加速度PID控制器
const double kp_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc");
const double ki_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc");
const double kd_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc");
velocity_insertion_based_planner_param_.pid_acc_controller =
std::make_unique(kp_acc, ki_acc, kd_acc);
// 加速度变化率PID控制器
const double kp_jerk = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk");
const double ki_jerk = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk");
const double kd_jerk = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk");
velocity_insertion_based_planner_param_.pid_jerk_controller =
std::make_unique(kp_jerk, ki_jerk, kd_jerk);
velocity_insertion_based_planner_param_.output_acc_ratio_during_accel =
node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel");
velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel =
node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel");
velocity_insertion_based_planner_param_
.enable_jerk_limit_to_output_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc");
}
// 配置其他参数
min_cruise_target_vel_ =
node.declare_parameter("cruise.pid_based_planner.min_cruise_target_vel");
time_to_evaluate_rss_ =
node.declare_parameter("cruise.pid_based_planner.time_to_evaluate_rss");
const auto error_function_type =
node.declare_parameter("cruise.pid_based_planner.error_function_type");
error_func_ = [&]() -> std::function {
if (error_function_type == "linear") { // 线性误差函数
return [](const double val) { return val; };
} else if (error_function_type == "quadratic") { // 二次误差函数
return [](const double val) { return getSign(val) * std::pow(val, 2); };
}
throw std::domain_error("error function type is not supported"); // 如果不支持的误差函数类型
}();
// 初始化低通滤波器
const double lpf_normalized_error_cruise_dist_gain = node.declare_parameter(
"cruise.pid_based_planner.lpf_normalized_error_cruise_dist_gain");
lpf_normalized_error_cruise_dist_ptr_ =
std::make_shared(lpf_normalized_error_cruise_dist_gain);
lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5);
lpf_obstacle_vel_ptr_ = std::make_shared(0.5);
lpf_error_cruise_dist_ptr_ = std::make_shared(0.5);
lpf_output_vel_ptr_ = std::make_shared(0.5);
lpf_output_acc_ptr_ = std::make_shared(0.5);
lpf_output_jerk_ptr_ = std::make_shared(0.5);
}
// 生成巡航轨迹的函数
std::vector PIDBasedPlanner::generateCruiseTrajectory(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const std::vector & obstacles, std::optional & vel_limit)
{
stop_watch_.tic(__func__); // 开始计时
cruise_planning_debug_info_.reset(); // 重置巡航规划调试信息
// 计算需要巡航的障碍物信息
const auto cruise_obstacle_info = calcObstacleToCruise(planner_data, obstacles);
// 执行巡航规划
const auto cruise_traj_points =
planCruise(planner_data, stop_traj_points, vel_limit, cruise_obstacle_info);
const double calculation_time = stop_watch_.toc(__func__); // 计算运行时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_calculation_time_info_,
" %s := %f [ms]", __func__, calculation_time); // 输出运行时间信息
prev_traj_points_ = cruise_traj_points; // 更新上一次轨迹点
return cruise_traj_points; // 返回生成的巡航轨迹点
}
// 计算需要巡航的障碍物信息的函数
std::optional PIDBasedPlanner::calcObstacleToCruise(
const PlannerData & planner_data, const std::vector & obstacles)
{
// 设置调试信息
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.ego_vel);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, planner_data.ego_acc);
auto modified_target_obstacles = obstacles; // TODO(murooka) 这个变量的作用是什么？
// 查找最高概率的巡航障碍物
std::optional cruise_obstacle_info;
for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) {
const auto & obstacle = obstacles.at(o_idx);
if (obstacle.collision_points.empty()) { // 如果没有碰撞点
continue;
}
// 计算到障碍物的距离
const double dist_to_obstacle =
calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point) +
(obstacle.velocity - planner_data.ego_vel) * time_to_evaluate_rss_;
// 根据RSS计算目标距离
const double target_dist_to_obstacle = calcRSSDistance(
planner_data.ego_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin);
// 计算误差距离和归一化误差
const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle;
if (cruise_obstacle_info) {
if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { // 如果误差更大，则跳过
continue;
}
}
cruise_obstacle_info =
CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle);
}
if (!cruise_obstacle_info) { // 如果没有找到需要巡航的障碍物
lpf_dist_to_obstacle_ptr_->reset(); // 重置低通滤波器
lpf_obstacle_vel_ptr_->reset();
lpf_error_cruise_dist_ptr_->reset();
return {}; // 返回空值
}
// 如果找到需要巡航的障碍物
{ // 调试数据
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY,
planner_data.ego_vel - cruise_obstacle_info->obstacle.velocity);
const double non_zero_relative_vel = [&]() {
const double relative_vel = planner_data.ego_vel - cruise_obstacle_info->obstacle.velocity;
constexpr double epsilon = 0.001;
if (epsilon < std::abs(relative_vel)) { // 避免除以0
return relative_vel;
}
return getSign(relative_vel) * epsilon;
}();
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION,
cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel);
}
{ // 到障碍物的距离
const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle;
const double filtered_dist_to_obstacle =
lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED,
filtered_dist_to_obstacle);
cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle;
}
{ // 障碍物速度
const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity;
const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED,
filtered_obstacle_velocity);
cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity;
}
{ // 巡航误差距离
const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist;
const double filtered_error_cruise_dist =
lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist);
cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist;
}
{ // 巡航目标距离
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE,
cruise_obstacle_info->target_dist_to_obstacle);
}
return cruise_obstacle_info; // 返回需要巡航的障碍物信息
}
// 执行巡航规划的函数
std::vector PIDBasedPlanner::planCruise(
const PlannerData & planner_data, const std::vector & stop_traj_points,
std::optional & vel_limit,
const std::optional & cruise_obstacle_info)
{
// 执行巡航
if (cruise_obstacle_info) {
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
"cruise planning"); // 输出调试信息
{ // 更新调试标记
// 创建巡航虚拟墙标记
const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist;
const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle;
const size_t ego_idx = findEgoIndex(stop_traj_points, planner_data.ego_pose);
const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m);
const double dist_to_rss_wall =
std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset);
const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset(
stop_traj_points, dist_to_rss_wall, ego_idx);
auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0);
// 使用不同的颜色来区分巡航和减速虚拟墙
markers.markers.front().color =
autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5);
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker);
// 添加需要巡航的障碍物
debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle);
debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data);
}
// 执行巡航规划
if (!use_velocity_limit_based_planner_) { // 如果不使用基于速度限制的规划器
const auto cruise_traj =
doCruiseWithTrajectory(planner_data, stop_traj_points, *cruise_obstacle_info);
return cruise_traj; // 返回生成的巡航轨迹
}
vel_limit = doCruiseWithVelocityLimit(planner_data, *cruise_obstacle_info); // 使用速度限制进行巡航
return stop_traj_points; // 返回停止轨迹点
}
// 如果没有启用自适应巡航，则重置之前的巡航数据
prev_target_acc_ = {}; // 重置目标加速度
lpf_normalized_error_cruise_dist_ptr_->reset(); // 重置归一化误差距离低通滤波器
lpf_output_acc_ptr_->reset(); // 重置输出加速度低通滤波器
lpf_output_vel_ptr_->reset(); // 重置输出速度低通滤波器
lpf_output_jerk_ptr_->reset(); // 重置输出加速度变化率低通滤波器
// 删除虚拟墙标记
const auto markers =
autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0);
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker);
return stop_traj_points; // 返回停止轨迹点
}
// 使用速度限制进行巡航的函数
VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit(
const PlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info)
{
const auto & p = velocity_limit_based_planner_param_; // 获取基于速度限制的规划器参数
// 计算归一化误差距离
const double filtered_normalized_error_cruise_dist = [&]() {
const double normalized_error_cruise_dist =
cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle;
return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist);
}();
// 修改误差距离
const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist);
// 使用PID控制器计算目标速度
const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist);
const double additional_vel = [&]() {
if (modified_error_cruise_dist > 0) { // 如果误差为正
return pid_output_vel * p.output_ratio_during_accel; // 返回加速时的输出速度
}
return pid_output_vel; // 返回PID输出速度
}();
// 计算目标速度
const double positive_target_vel = lpf_output_vel_ptr_->filter(
std::max(min_cruise_target_vel_, planner_data.ego_vel + additional_vel));
// 计算目标加速度
const double target_acc = [&]() {
if (p.disable_target_acceleration) { // 如果禁用目标加速度
return min_accel_during_cruise_; // 返回巡航时的最小加速度
}
double target_acc = p.vel_to_acc_weight * additional_vel; // 计算目标加速度
// 应用加速度限制
target_acc = std::clamp(
target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel);
if (!prev_target_acc_) { // 如果没有上一次的目标加速度
return lpf_output_acc_ptr_->filter(target_acc); // 返回过滤后的目标加速度
}
if (p.enable_jerk_limit_to_output_acc) { // 如果启用加速度变化率限制
const double jerk = (target_acc - *prev_target_acc_) / 0.1; // 计算加速度变化率
const double limited_jerk =
std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); // 限制加速度变化率
target_acc = *prev_target_acc_ + limited_jerk * 0.1; // 更新目标加速度
}
return lpf_output_acc_ptr_->filter(target_acc); // 返回过滤后的目标加速度
}();
prev_target_acc_ = target_acc; // 更新上一次的目标加速度
// 设置调试信息
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc);
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
"target_velocity %f", positive_target_vel); // 输出目标速度
// 创建速度限制消息
const auto vel_limit = createVelocityLimitMsg(
planner_data.current_time, positive_target_vel, target_acc, longitudinal_info_.max_jerk,
longitudinal_info_.min_jerk);
return vel_limit; // 返回速度限制消息
}
// 使用轨迹进行巡航的函数
std::vector PIDBasedPlanner::doCruiseWithTrajectory(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const CruiseObstacleInfo & cruise_obstacle_info)
{
const auto & p = velocity_insertion_based_planner_param_; // 获取基于速度插入的规划器参数
// 计算归一化误差距离
const double filtered_normalized_error_cruise_dist = [&]() {
const double normalized_error_cruise_dist =
cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle;
return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist);
}();
// 修改误差距离
const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist);
// 使用PID控制器计算目标加速度
const double target_acc = [&]() {
double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist);
if (0 < filtered_normalized_error_cruise_dist) { // 如果归一化误差为正
target_acc *= p.output_acc_ratio_during_accel; // 应用加速时的输出比例
}
// 应用加速度限制
target_acc = std::clamp(
target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel);
if (!prev_target_acc_) { // 如果没有上一次的目标加速度
return target_acc;
}
if (p.enable_jerk_limit_to_output_acc) { // 如果启用加速度变化率限制
const double jerk = (target_acc - *prev_target_acc_) / 0.1; // 计算加速度变化率
const double limited_jerk =
std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); // 限制加速度变化率
target_acc = *prev_target_acc_ + limited_jerk * 0.1; // 更新目标加速度
}
return target_acc;
}();
prev_target_acc_ = target_acc; // 更新上一次的目标加速度
// 计算目标加速度变化率比例
const double target_jerk_ratio = [&]() {
double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist);
if (0 < filtered_normalized_error_cruise_dist) { // 如果归一化误差为正
target_jerk_ratio *= p.output_jerk_ratio_during_accel; // 应用加速时的输出比例
}
target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); // 限制目标加速度变化率比例
return target_jerk_ratio;
}();
// 设置调试信息
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio);
// 获取最近的轨迹点
const auto prev_traj_closest_point = [&]() -> TrajectoryPoint {
return ego_nearest_param_.calcInterpolatedPoint(prev_traj_points_, planner_data.ego_pose);
}();
const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; // 获取当前速度
const double a0 = prev_traj_closest_point.acceleration_mps2; // 获取当前加速度
// 生成加速度受限的轨迹
auto cruise_traj_points = getAccelerationLimitedTrajectory(
stop_traj_points, planner_data.ego_pose, v0, a0, target_acc, target_jerk_ratio);
// 如果轨迹中存在速度为0的点，则将后续点的速度设置为0
const auto zero_vel_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(cruise_traj_points);
if (!zero_vel_idx_opt) { // 如果没有速度为0的点
return cruise_traj_points; // 返回生成的轨迹
}
for (size_t i = zero_vel_idx_opt.value(); i < cruise_traj_points.size(); ++i) {
cruise_traj_points.at(i).longitudinal_velocity_mps = 0.0; // 将后续点的速度设置为0
}
return cruise_traj_points; // 返回生成的巡航轨迹
}
// 生成加速度受限的轨迹的函数
std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory(
const std::vector traj_points, const geometry_msgs::msg::Pose & start_pose,
const double v0, const double a0, const double target_acc, const double target_jerk_ratio) const
{
// 计算速度函数
const auto vt = [&](const double v0, const double a0, const double jerk, const double t,
const double target_acc) {
const double t1 = (target_acc - a0) / jerk; // 计算时间
const double v = [&]() {
if (t < t1) { // 如果时间小于t1
return v0 + a0 * t + jerk * t * t / 2.0; // 返回速度
}
const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; // 计算v1
return v1 + target_acc * (t - t1); // 返回目标速度
}();
constexpr double max_vel = 100.0; // 最大速度
return std::clamp(v, 0.0, max_vel); // 限制速度
};
// 计算sv图
const double s_traj = autoware::motion_utils::calcArcLength(traj_points); // 计算轨迹长度
const double s_max = 50.0; // 最大距离
const double max_sampling_num = 100.0; // 最大采样数量
const double t_delta_min = 0.1; // 最小时间步长
const double s_delta_min = 0.1; // 最小距离步长
// 初始化速度和时间
const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0;
const double a1 = a0; // + jerk * 0.1;
const double jerk = target_acc > a1 ? longitudinal_info_.max_jerk * target_jerk_ratio
: longitudinal_info_.min_jerk * target_jerk_ratio;
double t_current = 0.0; // 当前时间
std::vector s_vec{0.0}; // 距离向量
std::vector v_vec{v1}; // 速度向量
for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) {
if (v_vec.back() <= 0.0) { // 如果速度小于等于0
s_vec.push_back(s_vec.back() + s_delta_min); // 添加距离
v_vec.push_back(0.0); // 添加速度
} else {
const double v_current = vt(
v1, a1, jerk, t_current + t_delta_min,
target_acc); // 计算当前速度
const double s_delta = std::max(s_delta_min, v_current * t_delta_min); // 计算距离增量
const double s_current = s_vec.back() + s_delta; // 计算当前距离
s_vec.push_back(s_current); // 添加距离
v_vec.push_back(v_current); // 添加速度
const double t_delta = [&]() {
if (v_current <= 0) { // 如果速度小于等于0
return 0.0;
}
constexpr double t_delta_max = 100.0; // 最大时间步长
return std::min(t_delta_max, s_delta / v_current); // 限制时间步长
}();
t_current += t_delta; // 更新当前时间
}
if (s_traj < s_vec.back() || s_max < s_vec.back()) { // 如果距离超过轨迹长度或最大距离
break;
}
}
// 去重
std::vector unique_s_vec{s_vec.front()}; // 去重后的距离向量
std::vector unique_v_vec{v_vec.front()}; // 去重后的速度向量
for (size_t i = 0; i < s_vec.size(); ++i) {
if (s_vec.at(i) == unique_s_vec.back()) { // 如果距离重复
continue;
}
unique_s_vec.push_back(s_vec.at(i)); // 添加距离
unique_v_vec.push_back(v_vec.at(i)); // 添加速度
}
if (unique_s_vec.size() < 2) { // 如果去重后的向量大小小于2
return traj_points; // 返回原始轨迹点
}
// 生成加速度受限的轨迹
auto acc_limited_traj_points = traj_points; // 复制轨迹点
const size_t ego_seg_idx = findEgoIndex(acc_limited_traj_points, start_pose); // 获取自身所在轨迹段索引
double sum_dist = 0.0; // 累计距离
for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) {
if (i != ego_seg_idx) { // 如果不是自身所在轨迹段
sum_dist += autoware::universe_utils::calcDistance2d(
acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); // 累加距离
}
const double vel = [&]() {
if (unique_s_vec.back() < sum_dist) { // 如果距离超过最大距离
return unique_v_vec.back(); // 返回最后一个速度值
}
return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); // 插值计算速度
}();
acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp(
vel, 0.0,
static_cast(
acc_limited_traj_points.at(i)
.longitudinal_velocity_mps)); // 限制速度
}
return acc_limited_traj_points; // 返回生成的轨迹
}
// 更新巡航参数的函数
void PIDBasedPlanner::updateCruiseParam(const std::vector & parameters)
{
// 更新巡航时的最小加速度
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_);
{ // 更新基于速度限制的规划器参数
auto & p = velocity_limit_based_planner_param_;
double kp = p.pid_vel_controller->getKp(); // 获取PID控制器的Kp值
double ki = p.pid_vel_controller->getKi(); // 获取PID控制器的Ki值
double kd = p.pid_vel_controller->getKd(); // 获取PID控制器的Kd值
// 更新PID控制器参数
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kp", kp);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.ki", ki);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kd", kd);
p.pid_vel_controller->updateParam(kp, ki, kd);
// 更新其他参数
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel",
p.output_ratio_during_accel);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc",
p.enable_jerk_limit_to_output_acc);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration",
p.disable_target_acceleration);
}
{ // 更新基于速度插入的规划器参数
auto & p = velocity_insertion_based_planner_param_;
// 更新加速度PID控制器参数
double kp_acc = p.pid_acc_controller->getKp();
double ki_acc = p.pid_acc_controller->getKi();
double kd_acc = p.pid_acc_controller->getKd();
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc);
p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc);
// 更新加速度变化率PID控制器参数
double kp_jerk = p.pid_jerk_controller->getKp();
double ki_jerk = p.pid_jerk_controller->getKi();
double kd_jerk = p.pid_jerk_controller->getKd();
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk);
p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk);
// 更新其他参数
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel",
p.output_acc_ratio_during_accel);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel",
p.output_jerk_ratio_during_accel);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc",
p.enable_jerk_limit_to_output_acc);
}
// 更新其他参数
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_);
}
代码功能概述
-
PID基础规划器：
- 用于生成巡航轨迹，支持基于速度限制和基于轨迹的两种巡航方式。
- 使用PID控制器来控制速度和加速度，确保车辆在接近障碍物时能够平稳减速或保持安全距离。
-
主要功能模块：
- 速度限制消息生成：
createVelocityLimitMsg函数用于生成速度限制消息。 - 符号获取：
getSign函数用于获取数值的符号。 - 巡航障碍物信息计算：
calcObstacleToCruise函数用于计算需要巡航的障碍物信息。 - 巡航规划：
planCruise函数根据配置选择使用速度限制或轨迹方式进行巡航。 - 速度限制巡航：
doCruiseWithVelocityLimit函数使用PID控制器计算目标速度和加速度。 - 轨迹方式巡航：
doCruiseWithTrajectory函数生成加速度受限的轨迹。 - 参数更新：
updateCruiseParam函数用于动态更新巡航相关参数。
- 速度限制消息生成：
-
调试信息：
- 代码中通过
cruise_planning_debug_info_记录了大量调试信息，包括车辆速度、加速度、目标速度、目标加速度等，方便开发和调试。
- 代码中通过
-
低通滤波器：
- 使用低通滤波器对误差距离、目标速度、目标加速度等进行平滑处理，减少噪声影响。
pid_based_planner.cpp
// 版权声明，遵循Apache License 2.0协议
// 该代码由TIER IV, Inc.提供
#include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" // 包含PID基础规划器头文件
// 包含其他相关头文件，用于插值、标记辅助、几何计算等
#include "autoware/interpolation/spline_interpolation.hpp"
#include "autoware/motion_utils/marker/marker_helper.hpp"
#include "autoware/obstacle_cruise_planner/utils.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/ros/marker_helper.hpp"
#include "autoware/universe_utils/ros/update_param.hpp"
#include "tier4_planning_msgs/msg/velocity_limit.hpp" // 包含速度限制消息头文件
using autoware::signal_processing::LowpassFilter1d; // 使用一维低通滤波器
namespace // 匿名命名空间，用于定义局部函数和变量
{
// 创建速度限制消息的函数
VelocityLimit createVelocityLimitMsg(
const rclcpp::Time & current_time, const double vel, const double acc, const double max_jerk,
const double min_jerk)
{
VelocityLimit msg; // 创建速度限制消息实例
msg.stamp = current_time; // 设置时间戳
msg.sender = "obstacle_cruise_planner.cruise"; // 设置发送者
msg.use_constraints = true; // 启用约束
msg.max_velocity = vel; // 设置最大速度
if (acc < 0) { // 如果加速度为负
msg.constraints.min_acceleration = acc; // 设置最小加速度
}
msg.constraints.max_jerk = max_jerk; // 设置最大加速度变化率
msg.constraints.min_jerk = min_jerk; // 设置最小加速度变化率
return msg; // 返回创建的消息
}
// 获取符号的模板函数
template
T getSign(const T val)
{
if (0 < val) { // 如果值大于0
return static_cast(1); // 返回1
} else if (val < 0) { // 如果值小于0
return static_cast(-1); // 返回-1
}
return static_cast(0); // 如果值为0，返回0
}
} // namespace
// PID基础规划器类
PIDBasedPlanner::PIDBasedPlanner(
rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr)
: PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr)
{
// 从参数服务器获取巡航时的最小加速度
min_accel_during_cruise_ =
node.declare_parameter("cruise.pid_based_planner.min_accel_during_cruise");
// 是否使用基于速度限制的规划器
use_velocity_limit_based_planner_ =
node.declare_parameter("cruise.pid_based_planner.use_velocity_limit_based_planner");
// 配置基于速度限制的规划器参数
{ // velocity limit based planner
const double kp =
node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.kp");
const double ki =
node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.ki");
const double kd =
node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.kd");
velocity_limit_based_planner_param_.pid_vel_controller =
std::make_unique(kp, ki, kd); // 创建PID控制器
velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel");
velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.vel_to_acc_weight");
velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc =
node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc");
velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter(
"cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration");
}
// 配置基于速度插入的规划器参数
{ // velocity insertion based planner
// 加速度PID控制器
const double kp_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc");
const double ki_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc");
const double kd_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc");
velocity_insertion_based_planner_param_.pid_acc_controller =
std::make_unique(kp_acc, ki_acc, kd_acc);
// 加速度变化率PID控制器
const double kp_jerk = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk");
const double ki_jerk = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk");
const double kd_jerk = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk");
velocity_insertion_based_planner_param_.pid_jerk_controller =
std::make_unique(kp_jerk, ki_jerk, kd_jerk);
velocity_insertion_based_planner_param_.output_acc_ratio_during_accel =
node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel");
velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel =
node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel");
velocity_insertion_based_planner_param_
.enable_jerk_limit_to_output_acc = node.declare_parameter(
"cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc");
}
// 配置其他参数
min_cruise_target_vel_ =
node.declare_parameter("cruise.pid_based_planner.min_cruise_target_vel");
time_to_evaluate_rss_ =
node.declare_parameter("cruise.pid_based_planner.time_to_evaluate_rss");
const auto error_function_type =
node.declare_parameter("cruise.pid_based_planner.error_function_type");
error_func_ = [&]() -> std::function {
if (error_function_type == "linear") { // 线性误差函数
return [](const double val) { return val; };
} else if (error_function_type == "quadratic") { // 二次误差函数
return [](const double val) { return getSign(val) * std::pow(val, 2); };
}
throw std::domain_error("error function type is not supported"); // 如果不支持的误差函数类型
}();
// 初始化低通滤波器
const double lpf_normalized_error_cruise_dist_gain = node.declare_parameter(
"cruise.pid_based_planner.lpf_normalized_error_cruise_dist_gain");
lpf_normalized_error_cruise_dist_ptr_ =
std::make_shared(lpf_normalized_error_cruise_dist_gain);
lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5);
lpf_obstacle_vel_ptr_ = std::make_shared(0.5);
lpf_error_cruise_dist_ptr_ = std::make_shared(0.5);
lpf_output_vel_ptr_ = std::make_shared(0.5);
lpf_output_acc_ptr_ = std::make_shared(0.5);
lpf_output_jerk_ptr_ = std::make_shared(0.5);
}
// 生成巡航轨迹的函数
std::vector PIDBasedPlanner::generateCruiseTrajectory(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const std::vector & obstacles, std::optional & vel_limit)
{
stop_watch_.tic(__func__); // 开始计时
cruise_planning_debug_info_.reset(); // 重置巡航规划调试信息
// 计算需要巡航的障碍物信息
const auto cruise_obstacle_info = calcObstacleToCruise(planner_data, obstacles);
// 执行巡航规划
const auto cruise_traj_points =
planCruise(planner_data, stop_traj_points, vel_limit, cruise_obstacle_info);
const double calculation_time = stop_watch_.toc(__func__); // 计算运行时间
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_calculation_time_info_,
" %s := %f [ms]", __func__, calculation_time); // 输出运行时间信息
prev_traj_points_ = cruise_traj_points; // 更新上一次轨迹点
return cruise_traj_points; // 返回生成的巡航轨迹点
}
// 计算需要巡航的障碍物信息的函数
std::optional PIDBasedPlanner::calcObstacleToCruise(
const PlannerData & planner_data, const std::vector & obstacles)
{
// 设置调试信息
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.ego_vel);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, planner_data.ego_acc);
auto modified_target_obstacles = obstacles; // TODO(murooka) 这个变量的作用是什么？
// 查找最高概率的巡航障碍物
std::optional cruise_obstacle_info;
for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) {
const auto & obstacle = obstacles.at(o_idx);
if (obstacle.collision_points.empty()) { // 如果没有碰撞点
continue;
}
// 计算到障碍物的距离
const double dist_to_obstacle =
calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point) +
(obstacle.velocity - planner_data.ego_vel) * time_to_evaluate_rss_;
// 根据RSS计算目标距离
const double target_dist_to_obstacle = calcRSSDistance(
planner_data.ego_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin);
// 计算误差距离和归一化误差
const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle;
if (cruise_obstacle_info) {
if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { // 如果误差更大，则跳过
continue;
}
}
cruise_obstacle_info =
CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle);
}
if (!cruise_obstacle_info) { // 如果没有找到需要巡航的障碍物
lpf_dist_to_obstacle_ptr_->reset(); // 重置低通滤波器
lpf_obstacle_vel_ptr_->reset();
lpf_error_cruise_dist_ptr_->reset();
return {}; // 返回空值
}
// 如果找到需要巡航的障碍物
{ // 调试数据
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY,
planner_data.ego_vel - cruise_obstacle_info->obstacle.velocity);
const double non_zero_relative_vel = [&]() {
const double relative_vel = planner_data.ego_vel - cruise_obstacle_info->obstacle.velocity;
constexpr double epsilon = 0.001;
if (epsilon < std::abs(relative_vel)) { // 避免除以0
return relative_vel;
}
return getSign(relative_vel) * epsilon;
}();
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION,
cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel);
}
{ // 到障碍物的距离
const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle;
const double filtered_dist_to_obstacle =
lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED,
filtered_dist_to_obstacle);
cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle;
}
{ // 障碍物速度
const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity;
const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED,
filtered_obstacle_velocity);
cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity;
}
{ // 巡航误差距离
const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist;
const double filtered_error_cruise_dist =
lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist);
cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist;
}
{ // 巡航目标距离
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE,
cruise_obstacle_info->target_dist_to_obstacle);
}
return cruise_obstacle_info; // 返回需要巡航的障碍物信息
}
// 执行巡航规划的函数
std::vector PIDBasedPlanner::planCruise(
const PlannerData & planner_data, const std::vector & stop_traj_points,
std::optional & vel_limit,
const std::optional & cruise_obstacle_info)
{
// 执行巡航
if (cruise_obstacle_info) {
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
"cruise planning"); // 输出调试信息
{ // 更新调试标记
// 创建巡航虚拟墙标记
const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist;
const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle;
const size_t ego_idx = findEgoIndex(stop_traj_points, planner_data.ego_pose);
const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m);
const double dist_to_rss_wall =
std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset);
const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset(
stop_traj_points, dist_to_rss_wall, ego_idx);
auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0);
// 使用不同的颜色来区分巡航和减速虚拟墙
markers.markers.front().color =
autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5);
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker);
// 添加需要巡航的障碍物
debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle);
debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data);
}
// 执行巡航规划
if (!use_velocity_limit_based_planner_) { // 如果不使用基于速度限制的规划器
const auto cruise_traj =
doCruiseWithTrajectory(planner_data, stop_traj_points, *cruise_obstacle_info);
return cruise_traj; // 返回生成的巡航轨迹
}
vel_limit = doCruiseWithVelocityLimit(planner_data, *cruise_obstacle_info); // 使用速度限制进行巡航
return stop_traj_points; // 返回停止轨迹点
}
// 如果没有启用自适应巡航，则重置之前的巡航数据
prev_target_acc_ = {}; // 重置目标加速度
lpf_normalized_error_cruise_dist_ptr_->reset(); // 重置归一化误差距离低通滤波器
lpf_output_acc_ptr_->reset(); // 重置输出加速度低通滤波器
lpf_output_vel_ptr_->reset(); // 重置输出速度低通滤波器
lpf_output_jerk_ptr_->reset(); // 重置输出加速度变化率低通滤波器
// 删除虚拟墙标记
const auto markers =
autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0);
autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker);
return stop_traj_points; // 返回停止轨迹点
}
// 使用速度限制进行巡航的函数
VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit(
const PlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info)
{
const auto & p = velocity_limit_based_planner_param_; // 获取基于速度限制的规划器参数
// 计算归一化误差距离
const double filtered_normalized_error_cruise_dist = [&]() {
const double normalized_error_cruise_dist =
cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle;
return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist);
}();
// 修改误差距离
const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist);
// 使用PID控制器计算目标速度
const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist);
const double additional_vel = [&]() {
if (modified_error_cruise_dist > 0) { // 如果误差为正
return pid_output_vel * p.output_ratio_during_accel; // 返回加速时的输出速度
}
return pid_output_vel; // 返回PID输出速度
}();
// 计算目标速度
const double positive_target_vel = lpf_output_vel_ptr_->filter(
std::max(min_cruise_target_vel_, planner_data.ego_vel + additional_vel));
// 计算目标加速度
const double target_acc = [&]() {
if (p.disable_target_acceleration) { // 如果禁用目标加速度
return min_accel_during_cruise_; // 返回巡航时的最小加速度
}
double target_acc = p.vel_to_acc_weight * additional_vel; // 计算目标加速度
// 应用加速度限制
target_acc = std::clamp(
target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel);
if (!prev_target_acc_) { // 如果没有上一次的目标加速度
return lpf_output_acc_ptr_->filter(target_acc); // 返回过滤后的目标加速度
}
if (p.enable_jerk_limit_to_output_acc) { // 如果启用加速度变化率限制
const double jerk = (target_acc - *prev_target_acc_) / 0.1; // 计算加速度变化率
const double limited_jerk =
std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); // 限制加速度变化率
target_acc = *prev_target_acc_ + limited_jerk * 0.1; // 更新目标加速度
}
return lpf_output_acc_ptr_->filter(target_acc); // 返回过滤后的目标加速度
}();
prev_target_acc_ = target_acc; // 更新上一次的目标加速度
// 设置调试信息
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc);
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
"target_velocity %f", positive_target_vel); // 输出目标速度
// 创建速度限制消息
const auto vel_limit = createVelocityLimitMsg(
planner_data.current_time, positive_target_vel, target_acc, longitudinal_info_.max_jerk,
longitudinal_info_.min_jerk);
return vel_limit; // 返回速度限制消息
}
// 使用轨迹进行巡航的函数
std::vector PIDBasedPlanner::doCruiseWithTrajectory(
const PlannerData & planner_data, const std::vector & stop_traj_points,
const CruiseObstacleInfo & cruise_obstacle_info)
{
const auto & p = velocity_insertion_based_planner_param_; // 获取基于速度插入的规划器参数
// 计算归一化误差距离
const double filtered_normalized_error_cruise_dist = [&]() {
const double normalized_error_cruise_dist =
cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle;
return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist);
}();
// 修改误差距离
const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist);
// 使用PID控制器计算目标加速度
const double target_acc = [&]() {
double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist);
if (0 < filtered_normalized_error_cruise_dist) { // 如果归一化误差为正
target_acc *= p.output_acc_ratio_during_accel; // 应用加速时的输出比例
}
// 应用加速度限制
target_acc = std::clamp(
target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel);
if (!prev_target_acc_) { // 如果没有上一次的目标加速度
return target_acc;
}
if (p.enable_jerk_limit_to_output_acc) { // 如果启用加速度变化率限制
const double jerk = (target_acc - *prev_target_acc_) / 0.1; // 计算加速度变化率
const double limited_jerk =
std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); // 限制加速度变化率
target_acc = *prev_target_acc_ + limited_jerk * 0.1; // 更新目标加速度
}
return target_acc;
}();
prev_target_acc_ = target_acc; // 更新上一次的目标加速度
// 计算目标加速度变化率比例
const double target_jerk_ratio = [&]() {
double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist);
if (0 < filtered_normalized_error_cruise_dist) { // 如果归一化误差为正
target_jerk_ratio *= p.output_jerk_ratio_during_accel; // 应用加速时的输出比例
}
target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); // 限制目标加速度变化率比例
return target_jerk_ratio;
}();
// 设置调试信息
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc);
cruise_planning_debug_info_.set(
CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio);
// 获取最近的轨迹点
const auto prev_traj_closest_point = [&]() -> TrajectoryPoint {
return ego_nearest_param_.calcInterpolatedPoint(prev_traj_points_, planner_data.ego_pose);
}();
const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; // 获取当前速度
const double a0 = prev_traj_closest_point.acceleration_mps2; // 获取当前加速度
// 生成加速度受限的轨迹
auto cruise_traj_points = getAccelerationLimitedTrajectory(
stop_traj_points, planner_data.ego_pose, v0, a0, target_acc, target_jerk_ratio);
// 如果轨迹中存在速度为0的点，则将后续点的速度设置为0
const auto zero_vel_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(cruise_traj_points);
if (!zero_vel_idx_opt) { // 如果没有速度为0的点
return cruise_traj_points; // 返回生成的轨迹
}
for (size_t i = zero_vel_idx_opt.value(); i < cruise_traj_points.size(); ++i) {
cruise_traj_points.at(i).longitudinal_velocity_mps = 0.0; // 将后续点的速度设置为0
}
return cruise_traj_points; // 返回生成的巡航轨迹
}
// 生成加速度受限的轨迹的函数
std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory(
const std::vector traj_points, const geometry_msgs::msg::Pose & start_pose,
const double v0, const double a0, const double target_acc, const double target_jerk_ratio) const
{
// 计算速度函数
const auto vt = [&](const double v0, const double a0, const double jerk, const double t,
const double target_acc) {
const double t1 = (target_acc - a0) / jerk; // 计算时间
const double v = [&]() {
if (t < t1) { // 如果时间小于t1
return v0 + a0 * t + jerk * t * t / 2.0; // 返回速度
}
const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; // 计算v1
return v1 + target_acc * (t - t1); // 返回目标速度
}();
constexpr double max_vel = 100.0; // 最大速度
return std::clamp(v, 0.0, max_vel); // 限制速度
};
// 计算sv图
const double s_traj = autoware::motion_utils::calcArcLength(traj_points); // 计算轨迹长度
const double s_max = 50.0; // 最大距离
const double max_sampling_num = 100.0; // 最大采样数量
const double t_delta_min = 0.1; // 最小时间步长
const double s_delta_min = 0.1; // 最小距离步长
// 初始化速度和时间
const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0;
const double a1 = a0; // + jerk * 0.1;
const double jerk = target_acc > a1 ? longitudinal_info_.max_jerk * target_jerk_ratio
: longitudinal_info_.min_jerk * target_jerk_ratio;
double t_current = 0.0; // 当前时间
std::vector s_vec{0.0}; // 距离向量
std::vector v_vec{v1}; // 速度向量
for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) {
if (v_vec.back() <= 0.0) { // 如果速度小于等于0
s_vec.push_back(s_vec.back() + s_delta_min); // 添加距离
v_vec.push_back(0.0); // 添加速度
} else {
const double v_current = vt(
v1, a1, jerk, t_current + t_delta_min,
target_acc); // 计算当前速度
const double s_delta = std::max(s_delta_min, v_current * t_delta_min); // 计算距离增量
const double s_current = s_vec.back() + s_delta; // 计算当前距离
s_vec.push_back(s_current); // 添加距离
v_vec.push_back(v_current); // 添加速度
const double t_delta = [&]() {
if (v_current <= 0) { // 如果速度小于等于0
return 0.0;
}
constexpr double t_delta_max = 100.0; // 最大时间步长
return std::min(t_delta_max, s_delta / v_current); // 限制时间步长
}();
t_current += t_delta; // 更新当前时间
}
if (s_traj < s_vec.back() || s_max < s_vec.back()) { // 如果距离超过轨迹长度或最大距离
break;
}
}
// 去重
std::vector unique_s_vec{s_vec.front()}; // 去重后的距离向量
std::vector unique_v_vec{v_vec.front()}; // 去重后的速度向量
for (size_t i = 0; i < s_vec.size(); ++i) {
if (s_vec.at(i) == unique_s_vec.back()) { // 如果距离重复
continue;
}
unique_s_vec.push_back(s_vec.at(i)); // 添加距离
unique_v_vec.push_back(v_vec.at(i)); // 添加速度
}
if (unique_s_vec.size() < 2) { // 如果去重后的向量大小小于2
return traj_points; // 返回原始轨迹点
}
// 生成加速度受限的轨迹
auto acc_limited_traj_points = traj_points; // 复制轨迹点
const size_t ego_seg_idx = findEgoIndex(acc_limited_traj_points, start_pose); // 获取自身所在轨迹段索引
double sum_dist = 0.0; // 累计距离
for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) {
if (i != ego_seg_idx) { // 如果不是自身所在轨迹段
sum_dist += autoware::universe_utils::calcDistance2d(
acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); // 累加距离
}
const double vel = [&]() {
if (unique_s_vec.back() < sum_dist) { // 如果距离超过最大距离
return unique_v_vec.back(); // 返回最后一个速度值
}
return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); // 插值计算速度
}();
acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp(
vel, 0.0,
static_cast(
acc_limited_traj_points.at(i)
.longitudinal_velocity_mps)); // 限制速度
}
return acc_limited_traj_points; // 返回生成的轨迹
}
// 更新巡航参数的函数
void PIDBasedPlanner::updateCruiseParam(const std::vector & parameters)
{
// 更新巡航时的最小加速度
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_);
{ // 更新基于速度限制的规划器参数
auto & p = velocity_limit_based_planner_param_;
double kp = p.pid_vel_controller->getKp(); // 获取PID控制器的Kp值
double ki = p.pid_vel_controller->getKi(); // 获取PID控制器的Ki值
double kd = p.pid_vel_controller->getKd(); // 获取PID控制器的Kd值
// 更新PID控制器参数
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kp", kp);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.ki", ki);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kd", kd);
p.pid_vel_controller->updateParam(kp, ki, kd);
// 更新其他参数
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel",
p.output_ratio_during_accel);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc",
p.enable_jerk_limit_to_output_acc);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration",
p.disable_target_acceleration);
}
{ // 更新基于速度插入的规划器参数
auto & p = velocity_insertion_based_planner_param_;
// 更新加速度PID控制器参数
double kp_acc = p.pid_acc_controller->getKp();
double ki_acc = p.pid_acc_controller->getKi();
double kd_acc = p.pid_acc_controller->getKd();
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc);
p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc);
// 更新加速度变化率PID控制器参数
double kp_jerk = p.pid_jerk_controller->getKp();
double ki_jerk = p.pid_jerk_controller->getKi();
double kd_jerk = p.pid_jerk_controller->getKd();
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk);
p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk);
// 更新其他参数
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel",
p.output_acc_ratio_during_accel);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel",
p.output_jerk_ratio_during_accel);
autoware::universe_utils::updateParam(
parameters,
"cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc",
p.enable_jerk_limit_to_output_acc);
}
// 更新其他参数
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_);
autoware::universe_utils::updateParam(
parameters, "cruise.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_);
}
代码功能概述
PID基础规划器：
用于生成巡航轨迹，支持基于速度限制和基于轨迹的两种巡航方式。
使用PID控制器来控制速度和加速度，确保车辆在接近障碍物时能够平稳减速或保持安全距离。
主要功能模块：
速度限制消息生成：createVelocityLimitMsg函数用于生成速度限制消息。
符号获取：getSign函数用于获取数值的符号。
巡航障碍物信息计算：calcObstacleToCruise函数用于计算需要巡航的障碍物信息。
巡航规划：planCruise函数根据配置选择使用速度限制或轨迹方式进行巡航。
速度限制巡航：doCruiseWithVelocityLimit函数使用PID控制器计算目标速度和加速度。
轨迹方式巡航：doCruiseWithTrajectory函数生成加速度受限的轨迹。
参数更新：updateCruiseParam函数用于动态更新巡航相关参数。
调试信息：
代码中通过cruise_planning_debug_info_记录了大量调试信息，包括车辆速度、加速度、目标速度、目标加速度等，方便开发和调试。
低通滤波器：
使用低通滤波器对误差距离、目标速度、目标加速度等进行平滑处理，减少噪声影响。
polygon_utils.cpp
// Copyright 2022 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "autoware/obstacle_cruise_planner/polygon_utils.hpp"
#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
namespace
{
/**
* @brief 计算最近碰撞点
*
* @param first_within_idx 第一个在范围内的索引
* @param collision_points 碰撞点列表
* @param decimated_traj_points 轨迹点列表（简化后的）
* @param is_driving_forward 是否正向行驶
* @return PointWithStamp 返回最近的碰撞点
*/
PointWithStamp calcNearestCollisionPoint(
const size_t first_within_idx, const std::vector & collision_points,
const std::vector & decimated_traj_points, const bool is_driving_forward)
{
// 获取前一个和后一个索引
const size_t prev_idx = first_within_idx == 0 ? first_within_idx : first_within_idx - 1;
const size_t next_idx = first_within_idx == 0 ? first_within_idx + 1 : first_within_idx;
// 创建轨迹段上的两个点
std::vector segment_points{
decimated_traj_points.at(prev_idx).pose, decimated_traj_points.at(next_idx).pose};
if (!is_driving_forward) {
std::reverse(segment_points.begin(), segment_points.end());
}
// 计算每个碰撞点到轨迹段的距离
std::vector dist_vec;
for (const auto & collision_point : collision_points) {
const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment(
segment_points, 0, collision_point.point);
dist_vec.push_back(dist);
}
// 找到距离最小的碰撞点
const size_t min_idx = std::min_element(dist_vec.begin(), dist_vec.end()) - dist_vec.begin();
return collision_points.at(min_idx);
}
/**
* @brief 获取碰撞索引
*
* @param traj_points 轨迹点列表
* @param traj_polygons 轨迹多边形列表
* @param object_pose 物体姿态
* @param object_time 物体时间戳
* @param object_shape 物体形状
* @param max_dist 最大距离（用于高效计算）
* @return std::optional>> 返回碰撞信息（索引和碰撞点列表）
*/
std::optional>> getCollisionIndex(
const std::vector & traj_points, const std::vector & traj_polygons,
const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time,
const Shape & object_shape, const double max_dist = std::numeric_limits::max())
{
// 将物体转换为多边形
const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape);
// 遍历轨迹多边形，检查是否与物体多边形相交
for (size_t i = 0; i < traj_polygons.size(); ++i) {
const double approximated_dist =
autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose);
if (approximated_dist > max_dist) {
continue;
}
std::vector collision_polygons;
boost::geometry::intersection(traj_polygons.at(i), obj_polygon, collision_polygons);
std::vector collision_geom_points;
bool has_collision = false;
for (const auto & collision_polygon : collision_polygons) {
if (boost::geometry::area(collision_polygon) > 0.0) {
has_collision = true;
// 收集碰撞多边形的所有顶点作为碰撞点
for (const auto & collision_point : collision_polygon.outer()) {
PointWithStamp collision_geom_point;
collision_geom_point.stamp = object_time;
collision_geom_point.point.x = collision_point.x();
collision_geom_point.point.y = collision_point.y();
collision_geom_point.point.z = traj_points.at(i).pose.position.z;
collision_geom_points.push_back(collision_geom_point);
}
}
}
if (has_collision) {
const auto collision_info =
std::pair>{i, collision_geom_points};
return collision_info;
}
}
return std::nullopt;
}
} // namespace
namespace polygon_utils
{
/**
* @brief 获取碰撞点及其距离
*
* @param traj_points 轨迹点列表
* @param traj_polygons 轨迹多边形列表
* @param obstacle 障碍物信息
* @param is_driving_forward 是否正向行驶
* @param vehicle_info 车辆信息
* @return std::optional> 返回碰撞点及其距离
*/
std::optional> getCollisionPoint(
const std::vector & traj_points, const std::vector & traj_polygons,
const Obstacle & obstacle, const bool is_driving_forward,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
{
// 获取碰撞信息
const auto collision_info =
getCollisionIndex(traj_points, traj_polygons, obstacle.pose, obstacle.stamp, obstacle.shape);
if (!collision_info) {
return std::nullopt;
}
// 计算车辆前保险杠位置
const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m
: vehicle_info.min_longitudinal_offset_m;
const auto bumper_pose = autoware::universe_utils::calcOffsetPose(
traj_points.at(collision_info->first).pose, x_diff_to_bumper, 0.0, 0.0);
// 查找离前保险杠最远的碰撞点
std::optional max_collision_length = std::nullopt;
std::optional max_collision_point = std::nullopt;
for (const auto & poly_vertex : collision_info->second) {
const double dist_from_bumper =
std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x);
if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) {
max_collision_length = dist_from_bumper;
max_collision_point = poly_vertex.point;
}
}
return std::make_pair(
*max_collision_point,
autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) -
*max_collision_length);
}
/**
* @brief 获取碰撞点及其距离（基于给定的碰撞索引和碰撞点）
*
* @param traj_points 轨迹点列表
* @param collision_idx 碰撞索引
* @param collision_points 碰撞点列表
* @param is_driving_forward 是否正向行驶
* @param vehicle_info 车辆信息
* @return std::optional> 返回碰撞点及其距离
*/
std::optional> getCollisionPoint(
const std::vector & traj_points, const size_t collision_idx,
const std::vector & collision_points, const bool is_driving_forward,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
{
std::pair> collision_info;
collision_info.first = collision_idx;
collision_info.second = collision_points;
// 计算车辆前保险杠位置
const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m
: vehicle_info.min_longitudinal_offset_m;
const auto bumper_pose = autoware::universe_utils::calcOffsetPose(
traj_points.at(collision_info.first).pose, x_diff_to_bumper, 0.0, 0.0);
// 查找离前保险杠最远的碰撞点
std::optional max_collision_length = std::nullopt;
std::optional max_collision_point = std::nullopt;
for (const auto & poly_vertex : collision_info.second) {
const double dist_from_bumper =
std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x);
if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) {
max_collision_length = dist_from_bumper;
max_collision_point = poly_vertex.point;
}
}
return std::make_pair(
*max_collision_point,
autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info.first) -
*max_collision_length);
}
/**
* @brief 获取预测路径中的碰撞点
*
* @param traj_points 轨迹点列表
* @param traj_polygons 轨迹多边形列表
* @param obstacle_stamp 障碍物时间戳
* @param predicted_path 预测路径
* @param shape 物体形状
* @param current_time 当前时间
* @param is_driving_forward 是否正向行驶
* @param collision_index 碰撞索引列表（输出参数）
* @param max_lat_dist 最大横向距离（用于高效计算）
* @param max_prediction_time_for_collision_check 碰撞检测的最大预测时间
* @return std::vector 返回所有碰撞点
*/
std::vector getCollisionPoints(
const std::vector & traj_points, const std::vector & traj_polygons,
const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape,
const rclcpp::Time & current_time, const bool is_driving_forward,
std::vector & collision_index, const double max_lat_dist,
const double max_prediction_time_for_collision_check)
{
std::vector collision_points;
for (size_t i = 0; i < predicted_path.path.size(); ++i) {
// 检查是否超过最大预测时间
if (
max_prediction_time_for_collision_check <
rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) {
break;
}
// 计算物体的时间戳
const auto object_time =
rclcpp::Time(obstacle_stamp) + rclcpp::Duration(predicted_path.time_step) * i;
// 忽略过去的位置
if ((object_time - current_time).seconds() < 0.0) {
continue;
}
// 获取碰撞信息
const auto collision_info = getCollisionIndex(
traj_points, traj_polygons, predicted_path.path.at(i), object_time, shape, max_lat_dist);
if (collision_info) {
// 计算最近的碰撞点并添加到结果中
const auto nearest_collision_point = calcNearestCollisionPoint(
collision_info->first, collision_info->second, traj_points, is_driving_forward);
collision_points.push_back(nearest_collision_point);
collision_index.push_back(collision_info->first);
}
}
return collision_points;
}
} // namespace polygon_utils
utils.cpp
// Copyright 2022 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "autoware/obstacle_cruise_planner/utils.hpp"
#include "autoware/object_recognition_utils/predicted_path_utils.hpp"
#include "autoware/universe_utils/ros/marker_helper.hpp"
namespace obstacle_cruise_utils
{
namespace
{
/**
* @brief 从预测路径中获取当前物体姿态
*
* @param predicted_path 预测路径
* @param obj_base_time 物体基准时间戳
* @param current_time 当前时间戳
* @return std::optional 返回当前物体姿态（如果存在）
*/
std::optional getCurrentObjectPoseFromPredictedPath(
const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time,
const rclcpp::Time & current_time)
{
// 计算相对时间
const double rel_time = (current_time - obj_base_time).seconds();
if (rel_time < 0.0) {
return std::nullopt;
}
// 计算插值后的姿态
const auto pose =
autoware::object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time);
if (!pose) {
return std::nullopt;
}
return pose.get();
}
/**
* @brief 从多个预测路径中获取当前物体姿态
*
* @param predicted_paths 多个预测路径
* @param obj_base_time 物体基准时间戳
* @param current_time 当前时间戳
* @return std::optional 返回当前物体姿态（如果存在）
*/
std::optional getCurrentObjectPoseFromPredictedPaths(
const std::vector & predicted_paths, const rclcpp::Time & obj_base_time,
const rclcpp::Time & current_time)
{
if (predicted_paths.empty()) {
return std::nullopt;
}
// 获取最可靠的路径（根据置信度）
const auto predicted_path = std::max_element(
predicted_paths.begin(), predicted_paths.end(),
[](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });
return getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time);
}
} // namespace
/**
* @brief 创建物体标记
*
* @param obj_pose 物体姿态
* @param idx 标记索引
* @param ns 命名空间
* @param r 红色分量
* @param g 绿色分量
* @param b 蓝色分量
* @return visualization_msgs::msg::Marker 返回创建的物体标记
*/
visualization_msgs::msg::Marker getObjectMarker(
const geometry_msgs::msg::Pose & obj_pose, size_t idx, const std::string & ns, const double r,
const double g, const double b)
{
const auto current_time = rclcpp::Clock().now();
// 创建默认标记
auto marker = autoware::universe_utils::createDefaultMarker(
"map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE,
autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0),
autoware::universe_utils::createMarkerColor(r, g, b, 0.8));
// 设置物体姿态
marker.pose = obj_pose;
return marker;
}
/**
* @brief 获取当前物体姿态（考虑预测）
*
* @param predicted_object 预测物体信息
* @param obj_base_time 物体基准时间戳
* @param current_time 当前时间戳
* @param use_prediction 是否使用预测
* @return PoseWithStamp 返回当前物体姿态及其时间戳
*/
PoseWithStamp getCurrentObjectPose(
const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time,
const rclcpp::Time & current_time, const bool use_prediction)
{
// 获取初始姿态
const auto & pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
if (!use_prediction) {
// 如果不使用预测，直接返回初始姿态
return PoseWithStamp{obj_base_time, pose};
}
// 收集所有预测路径
std::vector predicted_paths;
for (const auto & path : predicted_object.kinematics.predicted_paths) {
predicted_paths.push_back(path);
}
// 获取插值后的姿态
const auto interpolated_pose =
getCurrentObjectPoseFromPredictedPaths(predicted_paths, obj_base_time, current_time);
if (!interpolated_pose) {
// 如果无法找到插值姿态，记录警告并返回初始姿态
RCLCPP_WARN(
rclcpp::get_logger("ObstacleCruisePlanner"), "未能找到插值障碍物姿态");
return PoseWithStamp{obj_base_time, pose};
}
return PoseWithStamp{obj_base_time, *interpolated_pose};
}
/**
* @brief 获取最近的停止障碍物
*
* @param stop_obstacles 停止障碍物列表
* @return std::vector 返回最近的停止障碍物列表
*/
std::vector getClosestStopObstacles(const std::vector & stop_obstacles)
{
std::vector candidates{};
for (const auto & stop_obstacle : stop_obstacles) {
// 查找相同类别的障碍物
const auto itr =
std::find_if(candidates.begin(), candidates.end(), [&stop_obstacle](const StopObstacle & co) {
return co.classification.label == stop_obstacle.classification.label;
});
if (itr == candidates.end()) {
// 如果没有找到相同类别的障碍物，添加新的
candidates.emplace_back(stop_obstacle);
} else if (
stop_obstacle.dist_to_collide_on_decimated_traj < itr->dist_to_collide_on_decimated_traj) {
// 如果找到更近的障碍物，更新
*itr = stop_obstacle;
}
}
return candidates;
}
} // namespace obstacle_cruise_utils
总结
autoware_obstacle_cruise_planner是一个复杂的规划系统，它通过多种方法处理巡航过程中的障碍物，确保安全和舒适性。