From 0ab87682c3535d69017d1527002f1d5be32c3af9 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 20 Sep 2023 11:24:03 +0900 Subject: [PATCH 1/2] refactor(behavior_path_planner): make object type filter method common (#5035) Signed-off-by: Zulfaqar Azmi --- .../lane_change/lane_change_module_data.hpp | 11 ++----- .../utils/lane_change/utils.hpp | 2 -- .../path_safety_checker/objects_filtering.hpp | 14 +++++++++ .../src/scene_module/lane_change/manager.cpp | 18 ++++++----- .../src/scene_module/lane_change/normal.cpp | 14 ++++----- .../src/utils/lane_change/utils.cpp | 16 ---------- .../path_safety_checker/objects_filtering.cpp | 30 ++++++++++--------- 7 files changed, 49 insertions(+), 56 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 0556a780467c0..b36ec67f52472 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -66,15 +66,8 @@ struct LaneChangeParameters bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; - // true by default - bool check_car{true}; // check object car - bool check_truck{true}; // check object truck - bool check_bus{true}; // check object bus - bool check_trailer{true}; // check object trailer - bool check_unknown{true}; // check object unknown - bool check_bicycle{true}; // check object bicycle - bool check_motorcycle{true}; // check object motorbike - bool check_pedestrian{true}; // check object pedestrian + // true by default for all objects + utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check utils::path_safety_checker::RSSparams rss_params; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index f59e0170bdf14..18f53b49d21d2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -142,8 +142,6 @@ lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters); - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 8a3b7094c69d1..8c5ca6f60a9b2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -187,6 +187,20 @@ TargetObjectsOnLane createTargetObjectsOnLane( const PredictedObjects & filtered_objects, const std::shared_ptr & params); +/** + * @brief Determines whether the predicted object type matches any of the target object types + * specified by the user. + * + * @param object The predicted object whose type is to be checked. + * @param target_object_types A structure containing boolean flags for each object type that the + * user is interested in checking. + * + * @return Returns true if the predicted object's highest probability label matches any of the + * specified target object types. + */ +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types); + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 27f44433c6d4e..41c73f12f0199 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -108,14 +108,16 @@ LaneChangeModuleManager::LaneChangeModuleManager( // target object { std::string ns = "lane_change.target_object."; - p.check_car = getOrDeclareParameter(*node, ns + "car"); - p.check_truck = getOrDeclareParameter(*node, ns + "truck"); - p.check_bus = getOrDeclareParameter(*node, ns + "bus"); - p.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); - p.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); - p.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); - p.check_motorcycle = getOrDeclareParameter(*node, ns + "motorcycle"); - p.check_pedestrian = getOrDeclareParameter(*node, ns + "pedestrian"); + p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); + p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.object_types_to_check.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.object_types_to_check.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.object_types_to_check.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.object_types_to_check.check_motorcycle = + getOrDeclareParameter(*node, ns + "motorcycle"); + p.object_types_to_check.check_pedestrian = + getOrDeclareParameter(*node, ns + "pedestrian"); } // lane change cancel diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index bf0175ccd92e4..b96f9f43572a9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -671,20 +671,20 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const double min_dist_to_lane_changing_start = std::max( current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); + auto filtered_objects = objects; + + utils::path_safety_checker::filterObjectsByClass( + filtered_objects, lane_change_parameters_->object_types_to_check); + LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & object = objects.objects.at(i); + for (size_t i = 0; i < filtered_objects.objects.size(); ++i) { + const auto & object = filtered_objects.objects.at(i); const auto & obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto extended_object = utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); - // ignore specific object types - if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) { - continue; - } - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); // calc distance from the current ego position diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index f3a1260eecadc..1395f07968c2d 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -652,22 +652,6 @@ lanelet::ConstLanelets getBackwardLanelets( return backward_lanes; } -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters) -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - const auto t = utils::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && parameters.check_car) || - (t == ObjectClassification::TRUCK && parameters.check_truck) || - (t == ObjectClassification::BUS && parameters.check_bus) || - (t == ObjectClassification::TRAILER && parameters.check_trailer) || - (t == ObjectClassification::UNKNOWN && parameters.check_unknown) || - (t == ObjectClassification::BICYCLE && parameters.check_bicycle) || - (t == ObjectClassification::MOTORCYCLE && parameters.check_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && parameters.check_pedestrian)); - return is_object_type; -} - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) { return lateral_buffer + 0.5 * vehicle_width; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index a775e7c16efed..b5db5f80aa74a 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -107,21 +107,10 @@ void filterObjectsByPosition( void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) { - using autoware_auto_perception_msgs::msg::ObjectClassification; - PredictedObjects filtered_objects; for (auto & object : objects.objects) { - const auto t = utils::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && target_object_types.check_car) || - (t == ObjectClassification::TRUCK && target_object_types.check_truck) || - (t == ObjectClassification::BUS && target_object_types.check_bus) || - (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || - (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || - (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || - (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); + const auto is_object_type = isTargetObjectType(object, target_object_types); // If the object type matches any of the target types, add it to the filtered list if (is_object_type) { @@ -131,8 +120,6 @@ void filterObjectsByClass( // Replace the original objects with the filtered list objects = std::move(filtered_objects); - - return; } std::pair, std::vector> separateObjectIndicesByLanelets( @@ -356,4 +343,19 @@ TargetObjectsOnLane createTargetObjectsOnLane( return target_objects_on_lane; } +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + const auto t = utils::getHighestProbLabel(object.classification); + return ( + (t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); +} } // namespace behavior_path_planner::utils::path_safety_checker From 76087bcf36cf5a016f320fb0e0438c63e26fe6fb Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 22 Sep 2023 16:39:45 +0900 Subject: [PATCH 2/2] feat(lane_change): regulate lane change on crosswalk and intersection (#5068) * fix(lane_change): fix hasEnoughDistance() Signed-off-by: Fumiya Watanabe * feat(lane_change): regulate lane change on crosswalk and intersection Signed-off-by: Fumiya Watanabe * fix(lane_change): remove unnecessary value Signed-off-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Kyoichi Sugahara * style(pre-commit): autofix * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * style(pre-commit): autofix * fix(lane_change): separate functions and fix Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe Co-authored-by: Kyoichi Sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../config/lane_change/lane_change.param.yaml | 5 + ...ehavior_path_planner_lane_change_design.md | 12 + .../scene_module/lane_change/normal.hpp | 8 +- .../lane_change/lane_change_module_data.hpp | 4 + .../src/scene_module/lane_change/manager.cpp | 5 + .../src/scene_module/lane_change/normal.cpp | 397 +++++++++++------- .../behavior_path_planner/src/utils/utils.cpp | 5 +- 7 files changed, 276 insertions(+), 160 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 925e70d9084d6..d7179cd384008 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -55,6 +55,11 @@ motorcycle: true pedestrian: true + # lane change regulations + regulation: + crosswalk: false + intersection: false + # collision check enable_prepare_segment_collision_check: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index fddd535931d94..24b9672c1260e 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -186,6 +186,11 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +### Lane change regulations + +If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. +To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`. + ### Aborting lane change The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. @@ -295,6 +300,13 @@ The following parameters are configurable in `lane_change.param.yaml`. | `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | | `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | +### Lane change regulations + +| Name | Unit | Type | Description | Default value | +| :------------------------ | ---- | ------- | ------------------------------------- | ------------- | +| `regulation.crosswalk` | [-] | boolean | Regulate lane change on crosswalks | false | +| `regulation.intersection` | [-] | boolean | Regulate lane change on intersections | false | + ### Collision checks during lane change The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index d1476abb63e27..664f547edb92d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -108,7 +108,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - double calcPrepareDuration( + std::vector calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; @@ -129,6 +129,12 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + bool hasEnoughLengthToCrosswalk( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + + bool hasEnoughLengthToIntersection( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index b36ec67f52472..398b60e426d00 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -66,6 +66,10 @@ struct LaneChangeParameters bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; + // regulatory elements + bool regulate_on_crosswalk{false}; + bool regulate_on_intersection{false}; + // true by default for all objects utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 41c73f12f0199..ac62460713f29 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -75,6 +75,11 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.use_all_predicted_path = getOrDeclareParameter(*node, parameter("use_all_predicted_path")); + // lane change regulations + p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); + p.regulate_on_intersection = + getOrDeclareParameter(*node, parameter("regulation.intersection")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index b96f9f43572a9..c82532c6be8ef 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -561,19 +561,26 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -double NormalLaneChange::calcPrepareDuration( +std::vector NormalLaneChange::calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { const auto & common_parameters = planner_data_->parameters; - const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation; - const auto current_vel = getEgoVelocity(); + const auto base_link2front = planner_data_->parameters.base_link2front; + const auto threshold = + lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + + std::vector prepare_durations; + constexpr double step = 0.5; - // if the ego vehicle is close to the end of the lane at a low speed - if (isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold) && current_vel < 1.0) { - return 0.0; + for (double duration = common_parameters.lane_change_prepare_duration; duration >= 0.0; + duration -= step) { + prepare_durations.push_back(duration); + if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { + break; + } } - return common_parameters.lane_change_prepare_duration; + return prepare_durations; } PathWithLaneId NormalLaneChange::getPrepareSegment( @@ -793,6 +800,7 @@ bool NormalLaneChange::hasEnoughLength( { const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); const auto & common_parameters = planner_data_->parameters; const double lane_change_length = path.info.length.sum(); const auto shift_intervals = @@ -823,6 +831,68 @@ bool NormalLaneChange::hasEnoughLength( return false; } + if (lane_change_parameters_->regulate_on_crosswalk) { + const double dist_to_crosswalk_from_lane_change_start_pose = + utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - + path.info.length.prepare; + // Check lane changing section includes crosswalk + if ( + dist_to_crosswalk_from_lane_change_start_pose > 0.0 && + dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + } + + if (lane_change_parameters_->regulate_on_intersection) { + const double dist_to_intersection_from_lane_change_start_pose = + utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; + // Check lane changing section includes intersection + if ( + dist_to_intersection_from_lane_change_start_pose > 0.0 && + dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + } + + return true; +} + +bool NormalLaneChange::hasEnoughLengthToCrosswalk( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + const double dist_to_crosswalk_from_lane_change_start_pose = + utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - + path.info.length.prepare; + // Check lane changing section includes crosswalk + if ( + dist_to_crosswalk_from_lane_change_start_pose > 0.0 && + dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + + return true; +} + +bool NormalLaneChange::hasEnoughLengthToIntersection( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + const double dist_to_intersection_from_lane_change_start_pose = + utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; + // Check lane changing section includes intersection + if ( + dist_to_intersection_from_lane_change_start_pose > 0.0 && + dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + return true; } @@ -869,182 +939,199 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_objects = getTargetObjects(current_lanes, target_lanes); - candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); + const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - const auto prepare_duration = calcPrepareDuration(current_lanes, target_lanes); + candidate_paths->reserve( + longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); - for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - // get path on original lanes - const auto prepare_velocity = std::max( - current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity); + for (const auto & prepare_duration : prepare_durations) { + for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { + // get path on original lanes + const auto prepare_velocity = std::max( + current_velocity + sampled_longitudinal_acc * prepare_duration, + minimum_lane_changing_velocity); - // compute actual longitudinal acceleration - const double longitudinal_acc_on_prepare = - (prepare_duration < 1e-3) ? 0.0 : ((prepare_velocity - current_velocity) / prepare_duration); + // compute actual longitudinal acceleration + const double longitudinal_acc_on_prepare = + (prepare_duration < 1e-3) ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); - const double prepare_length = current_velocity * prepare_duration + - 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); + const double prepare_length = + current_velocity * prepare_duration + + 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); - if (prepare_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); - continue; - } + if (prepare_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); + continue; + } - // lane changing start getEgoPose() is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); + // lane changing start getEgoPose() is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose); - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); - break; - } - - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - - const auto initial_lane_changing_velocity = prepare_velocity; - const auto & max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - - // get lateral acceleration range - const auto [min_lateral_acc, max_lateral_acc] = - common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); - const auto lateral_acc_resolution = - std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - constexpr double lateral_acc_epsilon = 0.01; - - for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon; - lateral_acc += lateral_acc_resolution) { - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); - const double longitudinal_acc_on_lane_changing = - utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, - sampled_longitudinal_acc); - const auto lane_changing_length = - initial_lane_changing_velocity * lane_changing_time + - 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; - const auto terminal_lane_changing_velocity = - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); - continue; + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_DEBUG( + logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + break; } - if (is_goal_in_route) { - const double s_start = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; - const auto num = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const double backward_buffer = - num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; - if ( - s_start + lane_changing_length + finish_judge_buffer + backward_buffer + - next_lane_change_buffer > - s_goal) { + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto initial_lane_changing_velocity = prepare_velocity; + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); + const auto lateral_acc_resolution = + std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; + constexpr double lateral_acc_epsilon = 0.01; + + for (double lateral_acc = min_lateral_acc; + lateral_acc < max_lateral_acc + lateral_acc_epsilon; + lateral_acc += lateral_acc_resolution) { + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); + const double longitudinal_acc_on_lane_changing = + utils::lane_change::calcLaneChangingAcceleration( + initial_lane_changing_velocity, max_path_velocity, lane_changing_time, + sampled_longitudinal_acc); + const auto lane_changing_length = + initial_lane_changing_velocity * lane_changing_time + + 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; + const auto terminal_lane_changing_velocity = + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; + utils::lane_change::setPrepareVelocity( + prepare_segment, current_velocity, terminal_lane_changing_velocity); + + if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); continue; } - } - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, - initial_lane_changing_velocity, next_lane_change_buffer); + if (is_goal_in_route) { + const double s_start = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + const auto num = + std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); + const double backward_buffer = + num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; + if ( + s_start + lane_changing_length + finish_judge_buffer + backward_buffer + + next_lane_change_buffer > + s_goal) { + RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + continue; + } + } - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); - continue; - } + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, + initial_lane_changing_velocity, next_lane_change_buffer); - const lanelet::BasicPoint2d lc_start_point( - prepare_segment.points.back().point.pose.position.x, - prepare_segment.points.back().point.pose.position.y); + if (target_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); + continue; + } - const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( - target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const lanelet::BasicPoint2d lc_start_point( + prepare_segment.points.back().point.pose.position.x, + prepare_segment.points.back().point.pose.position.y); - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( + target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); - if (!is_valid_start_point) { - // lane changing points are not inside of the target preferred lanes or its neighbors - continue; - } + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, initial_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose, target_lane_length, - lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); + if (!is_valid_start_point) { + // lane changing points are not inside of the target preferred lanes or its neighbors + continue; + } - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); - continue; - } + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_changing_length, initial_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose, target_lane_length, + lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto candidate_path = utils::lane_change::constructCandidatePath( - lane_change_info, prepare_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); - - if (!candidate_path) { - RCLCPP_DEBUG(logger_, "no candidate path!!"); - continue; - } + if (target_lane_reference_path.points.empty()) { + RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); + continue; + } - const auto is_valid = - hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction); + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = + LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; + lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + prepare_segment, target_segment, target_lane_reference_path, shift_length); + lane_change_info.lateral_acceleration = lateral_acc; + lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; + + const auto candidate_path = utils::lane_change::constructCandidatePath( + lane_change_info, prepare_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + if (!candidate_path) { + RCLCPP_DEBUG(logger_, "no candidate path!!"); + continue; + } - if (!is_valid) { - RCLCPP_DEBUG(logger_, "invalid candidate path!!"); - continue; - } + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { + RCLCPP_DEBUG(logger_, "invalid candidate path!!"); + continue; + } - if (utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_)) { - return false; - } - candidate_paths->push_back(*candidate_path); + if ( + lane_change_parameters_->regulate_on_crosswalk && + !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { + RCLCPP_DEBUG(logger_, "Including crosswalk!!"); + continue; + } - if (!check_safety) { - return false; - } + if ( + lane_change_parameters_->regulate_on_intersection && + !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { + RCLCPP_DEBUG(logger_, "Including intersection!!"); + continue; + } - const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); + if (utils::lane_change::passParkedObject( + route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_)) { + return false; + } + candidate_paths->push_back(*candidate_path); + + if (!check_safety) { + return false; + } - if (is_safe) { - return true; + const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( + *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); + + if (is_safe) { + return true; + } } } } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index c7fde9dada4ec..bae591ca16d40 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3076,15 +3076,12 @@ double calcMinimumLaneChangeLength( const double & max_lateral_acc = lat_acc.second; const double & lateral_jerk = common_param.lane_changing_lateral_jerk; const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - const auto prepare_length = 0.5 * common_param.max_acc * - common_param.lane_change_prepare_duration * - common_param.lane_change_prepare_duration; double accumulated_length = length_to_intersection; for (const auto & shift_interval : shift_intervals) { const double t = PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); - accumulated_length += vel * t + prepare_length + finish_judge_buffer; + accumulated_length += vel * t + finish_judge_buffer; } accumulated_length += common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0);