Skip to content

Commit

Permalink
perf(lane_change): only compute interpolate ego once (#2839)
Browse files Browse the repository at this point in the history 
* fix(lane_change): improvement on isLaneChangePathSafe computation

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* re-add logic to ignore parked vehicle

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix the duration issues and some code editing

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 authored Feb 28, 2023
1 parent 640cb61 commit 7d15fdf
Show file tree
Hide file tree
Showing 3 changed files with 91 additions and 62 deletions.
22 changes: 12 additions & 10 deletions planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include "behavior_path_planner/data_manager.hpp"
#include "behavior_path_planner/marker_util/debug_utilities.hpp"
#include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
#include "behavior_path_planner/util/pull_out/pull_out_path.hpp"

#include <opencv2/opencv.hpp>
Expand Down Expand Up @@ -493,19 +494,20 @@ bool isLateralDistanceEnough(
const double & relative_lateral_distance, const double & lateral_distance_threshold);

bool isSafeInLaneletCollisionCheck(
const Pose & ego_current_pose, const Twist & ego_current_twist,
const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
const double check_start_time, const double check_end_time, const double check_time_resolution,
const PredictedObject & target_object, const PredictedPath & target_object_path,
const BehaviorPathPlannerParameters & common_parameters, const double front_decel,
const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
const Twist & ego_current_twist, const std::vector<double> & check_duration,
const double prepare_duration, const PredictedObject & target_object,
const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters,
const double prepare_phase_ignore_target_speed_thresh, const double front_decel,
const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug);

bool isSafeInFreeSpaceCollisionCheck(
const Pose & ego_current_pose, const Twist & ego_current_twist,
const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
const double check_start_time, const double check_end_time, const double check_time_resolution,
const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters,
const double front_decel, const double rear_decel, CollisionCheckDebug & debug);
const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
const Twist & ego_current_twist, const std::vector<double> & check_duration,
const double prepare_duration, const PredictedObject & target_object,
const BehaviorPathPlannerParameters & common_parameters,
const double prepare_phase_ignore_target_speed_thresh, const double front_decel,
const double rear_decel, CollisionCheckDebug & debug);

bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);

Expand Down
55 changes: 31 additions & 24 deletions planning/behavior_path_planner/src/util/lane_change/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -506,19 +506,16 @@ bool isLaneChangePathSafe(
}

const double time_resolution = lane_change_parameters.prediction_time_resolution;
const auto & lane_change_prepare_duration = lane_change_path.duration.prepare;
const auto & enable_collision_check_at_prepare_phase =
const auto check_at_prepare_phase =
lane_change_parameters.enable_collision_check_at_prepare_phase;
const auto & lane_changing_safety_check_duration = lane_change_path.duration.lane_changing;
const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration;

const double check_start_time = check_at_prepare_phase ? 0.0 : lane_change_path.duration.prepare;
const double check_end_time = lane_change_path.duration.sum();
const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity};

const auto vehicle_predicted_path = util::convertToPredictedPath(
path, current_twist, current_pose, static_cast<double>(current_seg_idx), check_end_time,
time_resolution, acceleration, min_lc_speed);
const auto prepare_phase_ignore_target_speed_thresh =
lane_change_parameters.prepare_phase_ignore_target_speed_thresh;

const auto & vehicle_info = common_parameters.vehicle_info;

auto in_lane_object_indices = dynamic_objects_indices.target_lane;
Expand Down Expand Up @@ -551,26 +548,42 @@ bool isLaneChangePathSafe(
}
};

const auto reserve_size =
static_cast<size_t>((check_end_time - check_start_time) / time_resolution);
std::vector<double> check_durations{};
std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> interpolated_ego{};
check_durations.reserve(reserve_size);
interpolated_ego.reserve(reserve_size);

{
Pose expected_ego_pose = current_pose;
for (double t = check_start_time; t < check_end_time; t += time_resolution) {
std::string failed_reason;
tier4_autoware_utils::Polygon2d ego_polygon;
[[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon(
current_pose, vehicle_predicted_path, ego_polygon, t, vehicle_info, expected_ego_pose,
failed_reason);
check_durations.push_back(t);
interpolated_ego.emplace_back(expected_ego_pose, ego_polygon);
}
}

for (const auto & i : in_lane_object_indices) {
const auto & obj = dynamic_objects->objects.at(i);
const auto object_speed =
util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear);
const double check_start_time = (enable_collision_check_at_prepare_phase &&
(object_speed > prepare_phase_ignore_target_speed_thresh))
? 0.0
: lane_change_prepare_duration;
auto current_debug_data = assignDebugData(obj);
const auto predicted_paths =
util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path);
for (const auto & obj_path : predicted_paths) {
if (!util::isSafeInLaneletCollisionCheck(
current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time,
check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel,
interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare,
obj, obj_path, common_parameters,
lane_change_parameters.prepare_phase_ignore_target_speed_thresh, front_decel,
rear_decel, ego_pose_before_collision, current_debug_data.second)) {
appendDebugInfo(current_debug_data, false);
return false;
}
}
appendDebugInfo(current_debug_data, true);
}

if (!lane_change_parameters.use_predicted_path_outside_lanelet) {
Expand All @@ -585,16 +598,10 @@ bool isLaneChangePathSafe(
const auto predicted_paths =
util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path);

const auto object_speed =
util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear);
const double check_start_time = (enable_collision_check_at_prepare_phase &&
(object_speed > prepare_phase_ignore_target_speed_thresh))
? 0.0
: lane_change_prepare_duration;
if (!util::isSafeInFreeSpaceCollisionCheck(
current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time,
check_end_time, time_resolution, obj, common_parameters, front_decel, rear_decel,
current_debug_data.second)) {
interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, obj,
common_parameters, lane_change_parameters.prepare_phase_ignore_target_speed_thresh,
front_decel, rear_decel, current_debug_data.second)) {
appendDebugInfo(current_debug_data, false);
return false;
}
Expand Down
76 changes: 48 additions & 28 deletions planning/behavior_path_planner/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2290,29 +2290,38 @@ bool isLateralDistanceEnough(
}

bool isSafeInLaneletCollisionCheck(
const Pose & ego_current_pose, const Twist & ego_current_twist,
const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
const double check_start_time, const double check_end_time, const double check_time_resolution,
const PredictedObject & target_object, const PredictedPath & target_object_path,
const BehaviorPathPlannerParameters & common_parameters, const double front_decel,
const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
const Twist & ego_current_twist, const std::vector<double> & check_duration,
const double prepare_duration, const PredictedObject & target_object,
const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters,
const double prepare_phase_ignore_target_speed_thresh, const double front_decel,
const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug)
{
const auto lerp_path_reserve = (check_end_time - check_start_time) / check_time_resolution;
if (lerp_path_reserve > 1e-3) {
debug.lerped_path.reserve(static_cast<size_t>(lerp_path_reserve));
}
debug.lerped_path.reserve(check_duration.size());

Pose expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose;
Pose expected_ego_pose = ego_current_pose;
for (double t = check_start_time; t < check_end_time; t += check_time_resolution) {
const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist;
const auto object_speed = l2Norm(object_twist.linear);
const auto ignore_check_at_time = [&](const double current_time) {
return (
(current_time < prepare_duration) &&
(object_speed < prepare_phase_ignore_target_speed_thresh));
};

for (size_t i = 0; i < check_duration.size(); ++i) {
const auto current_time = check_duration.at(i);

if (ignore_check_at_time(current_time)) {
continue;
}

tier4_autoware_utils::Polygon2d obj_polygon;
[[maybe_unused]] const auto get_obj_info = util::getObjectExpectedPoseAndConvertToPolygon(
target_object_path, target_object, obj_polygon, t, expected_obj_pose, debug.failed_reason);

tier4_autoware_utils::Polygon2d ego_polygon;
[[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon(
ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose,
target_object_path, target_object, obj_polygon, current_time, expected_obj_pose,
debug.failed_reason);
const auto & ego_info = interpolated_ego.at(i);
auto expected_ego_pose = ego_info.first;
const auto & ego_polygon = ego_info.second;

debug.ego_polygon = ego_polygon;
debug.obj_polygon = obj_polygon;
Expand All @@ -2328,7 +2337,6 @@ bool isSafeInLaneletCollisionCheck(
debug.expected_ego_pose = expected_ego_pose;
debug.expected_obj_pose = expected_obj_pose;

const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist;
if (!util::hasEnoughDistance(
expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, common_parameters,
front_decel, rear_decel, debug)) {
Expand All @@ -2341,22 +2349,34 @@ bool isSafeInLaneletCollisionCheck(
}

bool isSafeInFreeSpaceCollisionCheck(
const Pose & ego_current_pose, const Twist & ego_current_twist,
const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
const double check_start_time, const double check_end_time, const double check_time_resolution,
const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters,
const double front_decel, const double rear_decel, CollisionCheckDebug & debug)
const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
const Twist & ego_current_twist, const std::vector<double> & check_duration,
const double prepare_duration, const PredictedObject & target_object,
const BehaviorPathPlannerParameters & common_parameters,
const double prepare_phase_ignore_target_speed_thresh, const double front_decel,
const double rear_decel, CollisionCheckDebug & debug)
{
tier4_autoware_utils::Polygon2d obj_polygon;
if (!util::calcObjectPolygon(target_object, &obj_polygon)) {
return false;
}
Pose expected_ego_pose = ego_current_pose;
for (double t = check_start_time; t < check_end_time; t += check_time_resolution) {
tier4_autoware_utils::Polygon2d ego_polygon;
[[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon(
ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose,
debug.failed_reason);

const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist;
const auto object_speed = l2Norm(object_twist.linear);
const auto ignore_check_at_time = [&](const double current_time) {
return (
(current_time < prepare_duration) &&
(object_speed < prepare_phase_ignore_target_speed_thresh));
};
for (size_t i = 0; i < check_duration.size(); ++i) {
const auto current_time = check_duration.at(i);

if (ignore_check_at_time(current_time)) {
continue;
}
const auto & ego_info = interpolated_ego.at(i);
auto expected_ego_pose = ego_info.first;
const auto & ego_polygon = ego_info.second;

debug.ego_polygon = ego_polygon;
debug.obj_polygon = obj_polygon;
Expand Down

0 comments on commit 7d15fdf

Please sign in to comment.