diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 46b47de128bd9..35cc02e867557 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -141,10 +141,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( const Pose & curent_pose, const double abort_return_dist, const LaneChangeParameters & lane_change_parameters, const Direction direction); -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length); - 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_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2021630f1ae34..1e97017b24856 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -843,7 +843,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = utils::lane_change::getBackwardLanelets( + const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 48a5a8722b0e9..bae733610905b 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -674,43 +674,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( return true; } -// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length) -{ - if (target_lanes.empty()) { - return {}; - } - - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); - - if (arc_length.length >= backward_length) { - return {}; - } - - const auto & front_lane = target_lanes.front(); - const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( - front_lane, std::abs(backward_length - arc_length.length), {front_lane}); - - lanelet::ConstLanelets backward_lanes{}; - const auto num_of_lanes = std::invoke([&preceding_lanes]() { - size_t sum{0}; - for (const auto & lanes : preceding_lanes) { - sum += lanes.size(); - } - return sum; - }); - - backward_lanes.reserve(num_of_lanes); - - for (const auto & lanes : preceding_lanes) { - backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); - } - - return backward_lanes; -} - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) { return lateral_buffer + 0.5 * vehicle_width; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index dcee7696933dd..22db593b45586 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane( lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length); + lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 188c50993e4ae..ea9872f268678 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -78,7 +78,7 @@ PredictedObjects filterObjects( const std::shared_ptr & params) { // Guard - if (objects->objects.empty()) { + if (objects->objects.empty() || !params) { return PredictedObjects(); } diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 308cf9c4fed2c..6fcfad2209815 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1514,6 +1514,43 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath( return lanes; } +// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length) +{ + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + + if (arc_length.length >= backward_length) { + return {}; + } + + const auto & front_lane = target_lanes.front(); + const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); + + lanelet::ConstLanelets backward_lanes{}; + const auto num_of_lanes = std::invoke([&preceding_lanes]() { + size_t sum{0}; + for (const auto & lanes : preceding_lanes) { + sum += lanes.size(); + } + return sum; + }); + + backward_lanes.reserve(num_of_lanes); + + for (const auto & lanes : preceding_lanes) { + backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); + } + + return backward_lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4b52aae91d032..ff0550e4d4867 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -222,9 +222,9 @@ This ordering is beneficial when the priority is to minimize the backward distan - **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) -- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring there is no overlap with the road lane's centerline. This is to avoid hindering the progress of following vehicles. +- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it. -- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline. +- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and that the rear vehicle can pass through the gap between the ego vehicle and the lane border. ```plantuml @startuml @@ -235,7 +235,7 @@ if (Collision with dynamic objects detected?) then (yes) if (Before departure?) then (yes) :Deactivate module decision is registered; else (no) - if (Can stop within constraints \n && \n no crossing centerline?) then (yes) + if (Can stop within constraints \n && \n Has sufficient space for rear vehicle to drive?) then (yes) :Stop; else (no) :Continue with caution; @@ -250,7 +250,7 @@ stop #### **example of safety check performed range for shift pull out** -Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint overlaps with the center line of the road lane, the safety check against dynamic objects is disabled. +Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint does not leave enough space for a rear vehicle to drive through, the safety check against dynamic objects is disabled.
![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100} @@ -319,27 +319,28 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | -| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | -| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | -| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | -| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | -| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | -| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | -| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | -| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | +| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | +| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane. | 0.5 | +| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | +| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | +| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | +| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | +| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | +| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | +| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ### **Ego vehicle's velocity planning** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index fad29b84c9c76..b17db7f907471 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 + extra_width_margin_for_rear_obstacle: 0.5 th_moving_object_velocity: 1.0 object_types_to_check_for_path_generation: check_car: true diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 07c81d2edd050..1e32237ffd870 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -64,6 +64,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; + double extra_width_margin_for_rear_obstacle{0.0}; std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 1368124929367..6014e5b8f1fdc 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -207,7 +207,8 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; - bool isOverlapWithCenterLane() const; + bool isPreventingRearVehicleFromPassingThrough() const; + bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index beeb675efd3ae..050d591128a15 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -44,6 +44,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_margin_from_front_object = @@ -371,6 +373,10 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam( + parameters, ns + "extra_width_margin_for_rear_obstacle", + p->extra_width_margin_for_rear_obstacle); + updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 3c08d62f500ae..88a33d05f4ea7 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -39,6 +39,7 @@ #include using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; +using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -220,7 +221,7 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && - !isOverlapWithCenterLane(); + !isPreventingRearVehicleFromPassingThrough(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -287,35 +288,140 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } -bool StartPlannerModule::isOverlapWithCenterLane() const +bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); - for (const auto & current_lane : current_lanes) { - std::vector centerline_points; - for (const auto & point : current_lane.centerline()) { - geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point); - centerline_points.push_back(center_point); - } + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - for (size_t i = 0; i < centerline_points.size() - 1; ++i) { - const auto & p1 = centerline_points.at(i); - const auto & p2 = centerline_points.at(i + 1); - for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { - const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); - const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); - const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + const auto target_lanes = utils::getCurrentLanes(planner_data_); + if (target_lanes.empty()) return false; + + // Define functions to get distance between a point and a lane's boundaries. + auto calc_absolute_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { + std::vector boundary_path; + std::for_each( + boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { + const double x = boundary_point.x(); + const double y = boundary_point.y(); + boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + }); + + return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); + }; - if (intersection.has_value()) { - return true; - } + // Check from what side of the road the ego is merging + const auto centerline_path = + route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + const auto start_pose_nearest_segment_index = + motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); + if (!start_pose_nearest_segment_index) return false; + + const auto start_pose_point_msg = tier4_autoware_utils::createPoint( + start_pose.position.x, start_pose.position.y, start_pose.position.z); + const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset( + centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); + if (std::isnan(starting_pose_lateral_offset)) return false; + + RCLCPP_DEBUG(getLogger(), "starting pose lateral offset: %f", starting_pose_lateral_offset); + const bool ego_is_merging_from_the_left = (starting_pose_lateral_offset > 0.0); + + // Get the ego's overhang point closest to the centerline path and the gap between said point and + // the lane's border. + auto get_gap_between_ego_and_lane_border = + [&]( + geometry_msgs::msg::Pose & ego_overhang_point_as_pose, + const bool ego_is_merging_from_the_left) -> std::optional { + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); + for (const auto & point : vehicle_footprint) { + geometry_msgs::msg::Pose point_pose; + point_pose.position.x = point.x(); + point_pose.position.y = point.y(); + point_pose.position.z = 0.0; + + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + + const lanelet::ConstLineString2d current_lane_bound = (ego_is_merging_from_the_left) + ? closest_lanelet_const.rightBound2d() + : closest_lanelet_const.leftBound2d(); + const double current_point_lateral_gap = + calc_absolute_lateral_offset(current_lane_bound, point_pose); + if (current_point_lateral_gap < smallest_lateral_gap_between_ego_and_border) { + smallest_lateral_gap_between_ego_and_border = current_point_lateral_gap; + ego_overhang_point_as_pose.position.x = point.x(); + ego_overhang_point_as_pose.position.y = point.y(); + ego_overhang_point_as_pose.position.z = 0.0; } } - } - return false; + if (smallest_lateral_gap_between_ego_and_border == std::numeric_limits::max()) { + return std::nullopt; + } + return smallest_lateral_gap_between_ego_and_border; + }; + + geometry_msgs::msg::Pose ego_overhang_point_as_pose; + const auto gap_between_ego_and_lane_border = + get_gap_between_ego_and_lane_border(ego_overhang_point_as_pose, ego_is_merging_from_the_left); + if (!gap_between_ego_and_lane_border) return false; + + // Get the lanelets that will be queried for target objects + const auto relevant_lanelets = std::invoke([&]() -> std::optional { + lanelet::Lanelet closest_lanelet; + const bool is_closest_lanelet = lanelet::utils::query::getClosestLanelet( + target_lanes, ego_overhang_point_as_pose, &closest_lanelet); + if (!is_closest_lanelet) return std::nullopt; + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + // Check backwards just in case the Vehicle behind ego is in a different lanelet + constexpr double backwards_length = 200.0; + const auto prev_lanes = behavior_path_planner::utils::getBackwardLanelets( + *route_handler, target_lanes, current_pose, backwards_length); + // return all the relevant lanelets + lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; + relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); + return relevant_lanelets; + }); + if (!relevant_lanelets) return false; + + // filtering objects with velocity, position and class + const auto filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position, + objects_filtering_params_); + if (filtered_objects.objects.empty()) return false; + + // filtering objects based on the current position's lane + const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + relevant_lanelets.value(), route_handler, filtered_objects, objects_filtering_params_); + if (target_objects_on_lane.on_current_lane.empty()) return false; + + // Get the closest target obj width in the relevant lanes + const auto closest_object_width = std::invoke([&]() -> std::optional { + double arc_length_to_closet_object = std::numeric_limits::max(); + double closest_object_width = -1.0; + std::for_each( + target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), + [&](const auto & o) { + const auto arc_length = motion_utils::calcSignedArcLength( + centerline_path.points, current_pose.position, o.initial_pose.pose.position); + if (arc_length > 0.0) return; + if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; + arc_length_to_closet_object = arc_length; + closest_object_width = o.shape.dimensions.y; + }); + if (closest_object_width < 0.0) return std::nullopt; + return closest_object_width; + }); + if (!closest_object_width) return false; + // Decide if the closest object does not fit in the gap left by the ego vehicle. + return closest_object_width.value() + parameters_->extra_width_margin_for_rear_obstacle > + gap_between_ego_and_lane_border.value(); } bool StartPlannerModule::isCloseToOriginalStartPose() const @@ -1148,14 +1254,14 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); - if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } else if ( - distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; - } + turn_signal.turn_signal.command = std::invoke([&]() { + if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE; + if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset) + return TurnIndicatorsCommand::ENABLE_RIGHT; + if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) + return TurnIndicatorsCommand::ENABLE_LEFT; + return TurnIndicatorsCommand::DISABLE; + }); turn_signal.desired_start_point = start_pose; turn_signal.required_start_point = start_pose; @@ -1375,7 +1481,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; - break; + return; } default: { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( @@ -1389,7 +1495,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons ? utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) : current_drivable_area_info; - break; + return; } } }