diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4db2390e5d91c..888730ca400e8 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -319,20 +319,21 @@ 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 | -| 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 | +| objects_collision_check_margins | [m] | [double] | obstacle collision check margins against static objects from the footprint on the trajectory in pull_out_lanes | [2.0, 1.0, 0.5, 0.1] | +| back_objects_collision_check_margin | [m] | double | obstacle collision check margin against back static objects from the footprint on the start pose in pull_out_lanes | 3.0 | +| 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 | obstacle collision check margin against front static objects from the footprint on the start pose in pull_out_lanes | 5.0 | +| 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 cfa630b24b394..2ff013ec95906 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 @@ -5,7 +5,8 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.0, 0.5, 0.1] + objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1] + back_objects_collision_check_margin: 0.5 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 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 ecd99393eae2b..45d363737822d 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 @@ -63,7 +63,8 @@ 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}; - std::vector collision_check_margins{}; + std::vector objects_collision_check_margins{}; + double back_objects_collision_check_margin{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 147632329d926..cd4dd23b6567b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -46,10 +46,34 @@ using route_handler::RouteHandler; PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity); + +/** + * @brief Get a sequence of lanelets for pulling out from the current position. + * + * If the ego vehicle's current position is on a shoulder lane, the function retrieves a sequence of + * shoulder lanelets. If it is on a road lane, the function returns a sequence of road lanelets. + * + */ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); + +/** + * @brief Calculate the minimum arc length distance from the ego vehicle to static objects within + the same lanelets. + * + * Calculates the minimum arc length distance between the ego vehicle and static objects in given + * lanelets. It compares each corner of the vehicle's transformed footprint with every corner of + * object polygons to find the shortest distance within the lanelet system. + */ +double calcMinArcLengthDistanceFromEgoToObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects); + +double getArcLengthForPoint( + const lanelet::ConstLanelets & lanelets, const tier4_autoware_utils::Point2d & point); + } // namespace behavior_path_planner::start_planner_utils #endif // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 7fb52d59c418b..4060ef8ddb5e8 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,8 +43,10 @@ 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.collision_check_margins = - node->declare_parameter>(ns + "collision_check_margins"); + p.objects_collision_check_margins = + node->declare_parameter>(ns + "objects_collision_check_margins"); + p.back_objects_collision_check_margin = + node->declare_parameter(ns + "back_objects_collision_check_margin"); p.collision_check_margin_from_front_object = node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); 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 d2b469c55bf33..71432053095b1 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 @@ -628,7 +628,7 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & collision_check_margin : parameters_->objects_collision_check_margins) { for (const auto & [index, planner] : order_priority) { if (findPullOutPath( start_pose_candidates[index], planner, refined_start_pose, goal_pose, @@ -974,13 +974,13 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( - pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, - backward_path_length, std::numeric_limits::max()); + const auto back_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, + backward_path_length); const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( - pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, - std::numeric_limits::max()); + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, + std::numeric_limits::max(), 0); // Set the maximum backward distance less than the distance from the vehicle's base_link to // the lane's rearmost point to prevent lane departure. @@ -1014,10 +1014,14 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( continue; } - if (utils::checkCollisionBetweenFootprintAndObjects( - local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, - parameters_->collision_check_margins.back())) { - break; // poses behind this has a collision, so break. + if ( + (start_planner_utils::calcMinArcLengthDistanceFromEgoToObjects( + local_vehicle_footprint, *backed_pose, pull_out_lanes, + back_stop_objects_in_pull_out_lanes) < parameters_->back_objects_collision_check_margin) || + (utils::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes, + parameters_->objects_collision_check_margins.back()))) { + break; // poses behind this is too close to back static object, so break. } pull_out_start_pose_candidates.push_back(*backed_pose); @@ -1027,8 +1031,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, - const double velocity_threshold, const double object_check_forward_distance, - const double object_check_backward_distance) const + const double velocity_threshold, const double object_check_backward_distance, + const double object_check_forward_distance) const { const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); @@ -1042,8 +1046,8 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( pull_out_lanes, object_check_backward_distance, object_check_forward_distance); utils::path_safety_checker::filterObjectsByPosition( - stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance, - object_check_backward_distance); + stop_objects_in_pull_out_lanes, path.points, current_point, object_check_backward_distance, + object_check_forward_distance); return stop_objects_in_pull_out_lanes; } diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index b34398083a95c..83838165b54d1 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -104,4 +105,35 @@ lanelet::ConstLanelets getPullOutLanes( /*forward_only_in_route*/ true); } +double calcMinArcLengthDistanceFromEgoToObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects) +{ + double min_distance = std::numeric_limits::max(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); + for (const auto & obj : static_objects.objects) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + for (const auto & obj_outer_point : obj_polygon.outer()) { + const auto obj_pose_arc_length = getArcLengthForPoint(lanelets, obj_outer_point); + for (const auto & vehicle_corner_point : vehicle_footprint) { + const auto vehicle_pose_arc_length = getArcLengthForPoint(lanelets, vehicle_corner_point); + const double distance = std::abs(obj_pose_arc_length - vehicle_pose_arc_length); + min_distance = std::min(min_distance, distance); + } + } + } + + return min_distance; +} + +double getArcLengthForPoint( + const lanelet::ConstLanelets & lanelets, const tier4_autoware_utils::Point2d & point) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = point.x(); + pose.position.y = point.y(); + return lanelet::utils::getArcCoordinates(lanelets, pose).length; +} + } // namespace behavior_path_planner::start_planner_utils