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 0e85c1d0e76b3..ace78c318175d 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 @@ -52,6 +52,9 @@ struct StartPlannerDebugData std::vector start_pose_candidates; size_t selected_start_pose_candidate_index; double margin_for_start_pose_candidate; + + // for isPreventingRearVehicleFromPassingThrough + std::optional estimated_stop_pose; }; struct StartPlannerParameters 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 591d8adb60819..795080b1b5d99 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 @@ -229,8 +229,31 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; + + /** + * @brief Check if the ego vehicle is preventing the rear vehicle from passing through. + * + * This function just call isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) with + * two poses. If rear vehicle is obstructed by ego vehicle at either of the two poses, it returns + * true. + * + * @return true if the ego vehicle is preventing the rear vehicle from passing through with the + * current pose or the pose if it stops. + */ bool isPreventingRearVehicleFromPassingThrough() const; + /** + * @brief Check if the ego vehicle is preventing the rear vehicle from passing through. + * + * This function measures the distance to the lane boundary from the current pose and the pose if +it stops, and determines whether there is enough space for the rear vehicle to pass through. If + * it is obstructing at either of the two poses, it returns true. + * + * @return true if the ego vehicle is preventing the rear vehicle from passing through with given +ego pose. + */ + bool isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const; + bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; 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 b62b4c707039e..6105abacc8e62 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 @@ -359,7 +359,35 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { + // prepare poses for preventing check + // - current_pose + // - estimated_stop_pose (The position assumed when stopped with the minimum stop distance, + // although it is NOT actually stopped) const auto & current_pose = planner_data_->self_odometry->pose.pose; + std::vector preventing_check_pose{current_pose}; + const auto min_stop_distance = + autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop, + 0.0); + debug_data_.estimated_stop_pose.reset(); // debug + if (min_stop_distance.has_value()) { + const auto current_path = getCurrentPath(); + const auto estimated_stop_pose = calcLongitudinalOffsetPose( + current_path.points, current_pose.position, min_stop_distance.value()); + if (estimated_stop_pose.has_value()) { + preventing_check_pose.push_back(estimated_stop_pose.value()); + debug_data_.estimated_stop_pose = estimated_stop_pose.value(); // debug + } + } + + // check if any of the preventing check poses are preventing rear vehicle from passing through + return std::any_of( + preventing_check_pose.begin(), preventing_check_pose.end(), + [&](const auto & pose) { return isPreventingRearVehicleFromPassingThrough(pose); }); +} + +bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const +{ 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(); @@ -406,7 +434,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const 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)); + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits::max(); @@ -482,7 +510,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // 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, + dynamic_object, route_handler, relevant_lanelets.value(), ego_pose.position, objects_filtering_params_); if (filtered_objects.objects.empty()) return false; @@ -1651,6 +1679,16 @@ void StartPlannerModule::setDebugData() info_marker_); add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_); + // visualize estimated_stop_pose for isPreventingRearVehicleFromPassingThrough() + if (debug_data_.estimated_stop_pose.has_value()) { + auto footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "estimated_stop_pose", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), purple_color); + footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + addFootprintMarker(footprint_marker, debug_data_.estimated_stop_pose.value(), vehicle_info_); + debug_marker_.markers.push_back(footprint_marker); + } + // set objects of interest for (const auto & [uuid, data] : debug_data_.collision_check) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;