包名:planning-obstacle_stop_planner
核心代码和逻辑
void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg)
{
// 加锁,防止多线程访问
mutex_.lock();
// NOTE: these variables must not be referenced for multithreading
const auto vehicle_info = vehicle_info_;
const auto stop_param = stop_param_;
const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_;
const auto object_ptr = object_ptr_;
const auto current_odometry_ptr = current_odometry_ptr_;
const auto current_acceleration_ptr = current_acceleration_ptr_;
mutex_.unlock();
// 等待 necessary data
{
const auto waiting = [this](const auto & str) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), "waiting for %s ...",
str);
};
// 等待 perception object
if (!object_ptr && node_param_.use_predicted_objects) {
waiting("perception object");
return;
}
// 等待 obstacle pointcloud
if (!obstacle_ros_pointcloud_ptr && !node_param_.use_predicted_objects) {
waiting("obstacle pointcloud");
return;
}
// 等待 current velocity
if (!current_odometry_ptr) {
waiting("current velocity");
return;
}
// 等待 current acceleration
if (!current_acceleration_ptr) {
waiting("current acceleration");
return;
}
// 检查输入消息是否为空
if (input_msg->points.empty()) {
return;
}
}
// 获取当前速度和加速度
const auto current_vel = current_odometry_ptr->twist.twist.linear.x;
const auto current_acc = current_acceleration_ptr->accel.accel.linear.x;
// TODO(someone): support backward path
// 检查是否为forward path
const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(input_msg->points);
is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_;
if (!is_driving_forward_) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 3000, "Backward path is NOT supported. publish input as it is.");
pub_trajectory_->publish(*input_msg);
return;
}
// 初始化PlannerData
PlannerData planner_data{};
// 获取当前pose
planner_data.current_pose = current_odometry_ptr_->pose.pose;
// 将输入消息转换为Trajectory和TrajectoryPoints
Trajectory output_trajectory = *input_msg;
TrajectoryPoints output_trajectory_points =
motion_utils::convertToTrajectoryPointArray(*input_msg);
// trim trajectory from self pose
// 去除自车自身
TrajectoryPoints base_trajectory = trimTrajectoryWithIndexFromSelfPose(
motion_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose,
planner_data.trajectory_trim_index);
// extend trajectory to consider obstacles after the goal
// 扩展轨迹,以考虑障碍物
if (stop_param.enable_stop_behind_goal_for_obstacle) {
base_trajectory = extendTrajectory(base_trajectory, stop_param.max_longitudinal_margin);
}
// decimate trajectory for calculation cost
// 降采样轨迹,以减少计算成本
const auto decimate_trajectory = decimateTrajectory(
base_trajectory, stop_param.step_length, planner_data.decimate_trajectory_index_map);
// 使用预测对象搜索障碍物
if (node_param_.use_predicted_objects) {
searchPredictedObject(
decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info,
stop_param);
} else {
// search obstacles within slow-down/collision area
// 搜索障碍物,以考虑慢速/碰撞区域
searchObstacle(
decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info,
stop_param, obstacle_ros_pointcloud_ptr);
}
// insert slow-down-section/stop-point
// 插入慢速/停止点
insertVelocity(
output_trajectory_points, planner_data, input_msg->header, vehicle_info, current_acc,
current_vel, stop_param);
// 检查是否需要慢速
const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_;
if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) {
// 重置外部速度限制
resetExternalVelocityLimit(current_acc, current_vel);
}
// 将输出轨迹转换为Trajectory
auto trajectory = motion_utils::convertToTrajectory(output_trajectory_points);
// 发布调试数据
publishDebugData(planner_data, current_acc, current_vel);
// 添加头部
trajectory.header = input_msg->header;
// 发布轨迹
pub_trajectory_->publish(trajectory);
}