Skip to content

Commit

Permalink
refactor(lane_change): refactor extended object safety check (#9322)
Browse files Browse the repository at this point in the history 
* refactor LC extended object collision check code

Signed-off-by: mohammad alqudah <[email protected]>

* Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

Co-authored-by: Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: mohammad alqudah <[email protected]>
Co-authored-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@mkquda @zulfaqar-azmi-t4
mkquda and zulfaqar-azmi-t4 authored Nov 18, 2024
1 parent 6f40830 commit a0c4ab4
Show file tree
Hide file tree
Showing 4 changed files with 40 additions and 29 deletions.
7 changes: 4 additions & 3 deletions ...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,12 +169,13 @@ class NormalLaneChange : public LaneChangeBase
bool has_collision_with_decel_patterns(
const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects,
const size_t deceleration_sampling_num, const RSSparams & rss_param,
CollisionCheckDebugMap & debug_data) const;
const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const;

bool is_collided(
const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj,
const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj,
const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const;
const RSSparams & selected_rss_param, const bool check_prepare_phase,
CollisionCheckDebugMap & debug_data) const;

double get_max_velocity_for_safety_check() const;

Expand Down
5 changes: 2 additions & 3 deletions ...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ lanelet::BasicPolygon2d create_polygon(

ExtendedPredictedObject transform(
const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase);
const LaneChangeParameters & lane_change_parameters);

bool is_collided_polygons_in_lanelet(
const std::vector<Polygon2d> & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon);
Expand Down Expand Up @@ -249,8 +249,7 @@ bool is_before_terminal(
double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose);

ExtendedPredictedObjects transform_to_extended_objects(
const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects,
const bool check_prepare_phase);
const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects);

double get_distance_to_next_regulatory_element(
const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,
Expand Down
42 changes: 30 additions & 12 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1006,17 +1006,16 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const
return is_within_vel_th(object) && ahead_of_ego;
});

const auto is_check_prepare_phase = check_prepare_phase();
const auto target_lane_leading_extended_objects =
utils::lane_change::transform_to_extended_objects(
common_data_ptr_, filtered_by_lanes_objects.target_lane_leading, is_check_prepare_phase);
common_data_ptr_, filtered_by_lanes_objects.target_lane_leading);
const auto target_lane_trailing_extended_objects =
utils::lane_change::transform_to_extended_objects(
common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing, is_check_prepare_phase);
common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing);
const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects(
common_data_ptr_, filtered_by_lanes_objects.current_lane, is_check_prepare_phase);
common_data_ptr_, filtered_by_lanes_objects.current_lane);
const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects(
common_data_ptr_, filtered_by_lanes_objects.other_lane, is_check_prepare_phase);
common_data_ptr_, filtered_by_lanes_objects.other_lane);

FilteredByLanesExtendedObjects lane_change_target_objects(
current_lane_extended_objects, target_lane_leading_extended_objects,
Expand Down Expand Up @@ -1856,10 +1855,13 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
return {is_safe, !is_object_behind_ego};
}

const auto is_check_prepare_phase = check_prepare_phase();

const auto all_decel_pattern_has_collision =
[&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool {
return has_collision_with_decel_patterns(
lane_change_path, objects, deceleration_sampling_num, rss_params, debug_data);
lane_change_path, objects, deceleration_sampling_num, rss_params, is_check_prepare_phase,
debug_data);
};

if (all_decel_pattern_has_collision(collision_check_objects.trailing)) {
Expand All @@ -1876,7 +1878,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
bool NormalLaneChange::has_collision_with_decel_patterns(
const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects,
const size_t deceleration_sampling_num, const RSSparams & rss_param,
CollisionCheckDebugMap & debug_data) const
const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const
{
if (objects.empty()) {
return false;
Expand Down Expand Up @@ -1922,21 +1924,23 @@ bool NormalLaneChange::has_collision_with_decel_patterns(
? lane_change_parameters_->rss_params_for_parked
: rss_param;
return is_collided(
lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data);
lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase,
debug_data);
});
});

return all_collided;
}

bool NormalLaneChange::is_collided(
const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj,
const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj,
const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const
const RSSparams & selected_rss_param, const bool check_prepare_phase,
CollisionCheckDebugMap & debug_data) const
{
constexpr auto is_collided{true};

if (lane_change_path.points.empty()) {
if (lane_change_path.path.points.empty()) {
return !is_collided;
}

Expand All @@ -1961,9 +1965,23 @@ bool NormalLaneChange::is_collided(
const auto safety_check_max_vel = get_max_velocity_for_safety_check();
const auto & bpp_param = *common_data_ptr_->bpp_param_ptr;

const double velocity_threshold = lane_change_parameters_->stopped_object_velocity_threshold;
const double prepare_duration = lane_change_path.info.duration.prepare;
const double start_time = check_prepare_phase ? 0.0 : prepare_duration;

for (const auto & obj_path : obj_predicted_paths) {
utils::path_safety_checker::PredictedPathWithPolygon predicted_obj_path;
predicted_obj_path.confidence = obj_path.confidence;
std::copy_if(
obj_path.path.begin(), obj_path.path.end(), std::back_inserter(predicted_obj_path.path),
[&](const auto & entry) {
return !(
entry.time < start_time ||
(entry.time < prepare_duration && entry.velocity < velocity_threshold));
});

const auto collided_polygons = utils::path_safety_checker::get_collided_polygons(
lane_change_path, ego_predicted_path, obj, obj_path, bpp_param.vehicle_info,
lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info,
selected_rss_param, hysteresis_factor, safety_check_max_vel,
collision_check_yaw_diff_threshold, current_debug_data.second);

Expand Down
15 changes: 4 additions & 11 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -914,14 +914,11 @@ lanelet::BasicPolygon2d create_polygon(
ExtendedPredictedObject transform(
const PredictedObject & object,
[[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase)
const LaneChangeParameters & lane_change_parameters)
{
ExtendedPredictedObject extended_object(object);

const auto & time_resolution = lane_change_parameters.prediction_time_resolution;
const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration;
const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold;
const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration;
const double obj_vel_norm =
std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y);

Expand All @@ -933,11 +930,8 @@ ExtendedPredictedObject transform(
extended_object.predicted_paths.at(i).confidence = path.confidence;

// create path
for (double t = start_time; t < end_time + std::numeric_limits<double>::epsilon();
for (double t = 0.0; t < end_time + std::numeric_limits<double>::epsilon();
t += time_resolution) {
if (t < prepare_duration && obj_vel_norm < velocity_threshold) {
continue;
}
const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t);
if (obj_pose) {
const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape);
Expand Down Expand Up @@ -1163,8 +1157,7 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co
}

ExtendedPredictedObjects transform_to_extended_objects(
const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects,
const bool check_prepare_phase)
const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects)
{
ExtendedPredictedObjects extended_objects;
extended_objects.reserve(objects.size());
Expand All @@ -1173,7 +1166,7 @@ ExtendedPredictedObjects transform_to_extended_objects(
const auto & lc_param = *common_data_ptr->lc_param_ptr;
std::transform(
objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) {
return utils::lane_change::transform(object, bpp_param, lc_param, check_prepare_phase);
return utils::lane_change::transform(object, bpp_param, lc_param);
});

return extended_objects;
Expand Down

0 comments on commit a0c4ab4

Please sign in to comment.