diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index da37d04550f5e..126e66772920a 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -991,34 +991,51 @@ calcCurvature>( * curvature calculation */ template -std::vector> calcCurvatureAndArcLength(const T & points) +std::vector>> calcCurvatureAndSegmentLength( + const T & points) { - // Note that arclength is for the segment, not the sum. - std::vector> curvature_arc_length_vec; - curvature_arc_length_vec.emplace_back(0.0, 0.0); + // segment length is pair of segment length between {p1, p2} and {p2, p3} + std::vector>> curvature_and_segment_length_vec; + curvature_and_segment_length_vec.reserve(points.size()); + curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0)); for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); const auto p2 = autoware::universe_utils::getPoint(points.at(i)); const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3); - const double arc_length = - autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); - curvature_arc_length_vec.emplace_back(curvature, arc_length); + + // The first point has only the next point, so put the distance to that point. + // In other words, assign the first segment length at the second point to the + // second_segment_length at the first point. + if (i == 1) { + curvature_and_segment_length_vec.at(0).second.second = + autoware::universe_utils::calcDistance2d(p1, p2); + } + + // The second_segment_length of the previous point and the first segment length of the current + // point are equal. + const std::pair arc_length{ + curvature_and_segment_length_vec.back().second.second, + autoware::universe_utils::calcDistance2d(p2, p3)}; + + curvature_and_segment_length_vec.emplace_back(curvature, arc_length); } - curvature_arc_length_vec.emplace_back(0.0, 0.0); - return curvature_arc_length_vec; + // set to the last point + curvature_and_segment_length_vec.emplace_back( + 0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0)); + + return curvature_and_segment_length_vec; } -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); /** diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 5d536c0772fea..ce26952c3d634 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -238,14 +238,14 @@ calcCurvature>( const std::vector & points); // -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); // diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 53e06631a81d5..e6f84e7eece0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -22,6 +22,7 @@ lateral_offset_interval: 0.25 ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 + high_curvature_threshold: 0.1 # occupancy grid map occupancy_grid: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index bdb267ce223d3..6a10c0fb183f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -60,6 +60,7 @@ struct GoalPlannerParameters double lateral_offset_interval{0.0}; double ignore_distance_from_lane_start{0.0}; double margin_from_boundary{0.0}; + double high_curvature_threshold{0.0}; // occupancy grid map bool use_occupancy_grid_for_goal_search{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index d2ba416c7fa90..05385600926f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -52,6 +52,7 @@ struct PullOverPath size_t goal_id{}; size_t id{}; bool decided_velocity{false}; + double max_curvature{0.0}; // max curvature of the parking path PathWithLaneId getFullPath() const { @@ -72,6 +73,7 @@ struct PullOverPath return path; } + // path from the pull over start pose to the end pose PathWithLaneId getParkingPath() const { const PathWithLaneId full_path = getFullPath(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 6caeb411f6680..dc7a76031a3c1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -293,9 +293,18 @@ void GoalPlannerModule::onTimer() planner->setPlannerData(local_planner_data); planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path) { + if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); + + // calculate absolute maximum curvature of parking path(start pose to end pose) for path + // priority + const std::vector curvatures = + autoware::motion_utils::calcCurvature(pull_over_path->getParkingPath().points); + pull_over_path->max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -817,23 +826,37 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto debugPrintPathPriority = [this]( const std::vector & sorted_pull_over_path_candidates, - const std::map & goal_id_to_index, - const std::optional> & path_id_to_margin_map_opt = std::nullopt, - const std::optional> & isSoftMarginOpt = - std::nullopt) { + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature) { std::stringstream ss; - ss << "\n---------------------- path priority ----------------------\n"; - for (const auto & path : sorted_pull_over_path_candidates) { - // clang-format off - ss << "path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id - << ", goal_id: " << path.goal_id - << ", goal_priority:" << goal_id_to_index.at(path.goal_id); - // clang-format on - if (path_id_to_margin_map_opt && isSoftMarginOpt) { - ss << ", margin: " << path_id_to_margin_map_opt->at(path.id) - << ((*isSoftMarginOpt)(path) ? " (soft)" : " (hard)"); + + // unsafe goal and it's priority are not visible as debug marker in rviz, + // so exclude unsafe goal from goal_priority + std::map goal_id_and_priority; + { + int priority = 0; + for (const auto & goal_candidate : goal_candidates) { + goal_id_and_priority[goal_candidate.id] = goal_candidate.is_safe ? priority++ : -1; } + } + + ss << "\n---------------------- path priority ----------------------\n"; + for (size_t i = 0; i < sorted_pull_over_path_candidates.size(); ++i) { + const auto & path = sorted_pull_over_path_candidates[i]; + + // goal_index is same to goal priority including unsafe goal + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const bool is_safe_goal = goal_candidates[goal_index].is_safe; + const int goal_priority = goal_id_and_priority[path.goal_id]; + + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) + << ", path_id: " << path.id << ", goal_id: " << path.goal_id + << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") + << ", margin: " << path_id_to_margin_map.at(path.id) + << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.max_curvature + << (isHighCurvature(path) ? " (high)" : " (low)"); ss << "\n"; } ss << "-----------------------------------------------------------\n"; @@ -844,6 +867,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins; const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins; + // (1) Sort pull_over_path_candidates based on the order in goal_candidates // Create a map of goal_id to its index in goal_candidates std::map goal_id_to_index; for (size_t i = 0; i < goal_candidates.size(); ++i) { @@ -868,6 +892,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri // if object recognition is enabled, sort by collision check margin if (parameters_->use_object_recognition) { + // (2) Sort by collision check margins const std::vector margins = std::invoke([&]() { std::vector combined_margins = soft_margins; combined_margins.insert(combined_margins.end(), hard_margins.begin(), hard_margins.end()); @@ -914,23 +939,45 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; }); - // Sort pull_over_path_candidates based on the order in efficient_path_order - if (parameters_->path_priority == "efficient_path") { - const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_margin_map[path.id]; - return std::any_of( - soft_margins.begin(), soft_margins.end(), - [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); - }; - const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - return !isSoftMargin(a) && !isSoftMargin(b) && - std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; - }; + // (3) Sort by curvature + // If the curvature is less than the threshold, prioritize the path. + const auto isHighCurvature = [&](const PullOverPath & path) -> bool { + return path.max_curvature >= parameters_->high_curvature_threshold; + }; + const auto isSoftMargin = [&](const PullOverPath & path) -> bool { + const double margin = path_id_to_margin_map[path.id]; + return std::any_of( + soft_margins.begin(), soft_margins.end(), + [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); + }; + const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { + return !isSoftMargin(a) && !isSoftMargin(b) && + std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; + }; + + // NOTE: this is just partition sort based on curvature threshold within each sub partitions + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [&](const PullOverPath & a, const PullOverPath & b) { + // if both are soft margin or both are same hard margin, prioritize the path with lower + // curvature. + if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { + return !isHighCurvature(a) && isHighCurvature(b); + } + // otherwise, keep the order based on the margin. + return false; + }); + + // (4) Sort pull_over_path_candidates based on the order in efficient_path_order keeping the + // collision check margin and curvature priority. + if (parameters_->path_priority == "efficient_path") { std::stable_sort( sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&](const auto & a, const auto & b) { - // if both are soft margin or both are same hard margin, sort by planner priority + // if any of following conditions are met, sort by path type priority + // - both are soft margin + // - both are same hard margin if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { return comparePathTypePriority(a, b); } @@ -938,18 +985,24 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return false; }); - // debug print path priority: sorted by efficient_path_order and collision check margin + // debug print path priority sorted by + // - efficient_path_order + // - collision check margin + // - curvature debugPrintPathPriority( - sorted_pull_over_path_candidates, goal_id_to_index, path_id_to_margin_map, isSoftMargin); + sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map, + isSoftMargin, isHighCurvature); } } else { - // Sort pull_over_path_candidates based on the order in efficient_path_order + /** + * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the + * future. sort by curvature is not implemented yet. + * Sort pull_over_path_candidates based on the order in efficient_path_order + */ if (parameters_->path_priority == "efficient_path") { std::stable_sort( sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&](const auto & a, const auto & b) { return comparePathTypePriority(a, b); }); - // debug print path priority: sorted by efficient_path_order and collision check margin - debugPrintPathPriority(sorted_pull_over_path_candidates, goal_id_to_index); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index b079db9babf31..b86bbbca7d98b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -61,6 +61,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.ignore_distance_from_lane_start = node->declare_parameter(ns + "ignore_distance_from_lane_start"); p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); + p.high_curvature_threshold = node->declare_parameter(ns + "high_curvature_threshold"); const std::string parking_policy_name = node->declare_parameter(ns + "parking_policy"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index beb27c67833ea..0690f97900cab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -308,8 +308,19 @@ double ShiftPullOver::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + + const auto curvature_and_segment_length = + autoware::motion_utils::calcCurvatureAndSegmentLength(reversed_path.points); + + for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) { + const auto & [k, segment_length_pair] = curvature_and_segment_length[i]; + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvature_and_segment_length.size() - 1 + ? segment_length_pair.first + : segment_length_pair.first + segment_length_pair.second; + // after shifted segment length const double after_segment_length = k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 6eea3e3576732..ebdb0869545ca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -90,6 +90,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_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index b07d6497463ef..bb70ae2638056 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/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_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 971c9a48e8b40..def7072dc0e71 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -423,8 +423,17 @@ double ShiftPullOut::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(path.points)) { + const auto curvatures_and_segment_lengths = + autoware::motion_utils::calcCurvatureAndSegmentLength(path.points); + for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) { + const auto & [k, segment_length_pair] = curvatures_and_segment_lengths.at(i); + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvatures_and_segment_lengths.size() - 1 + ? segment_length_pair.first + segment_length_pair.second + : segment_length_pair.first; + // after shifted segment length const double after_segment_length = k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 2d9fbf7355415..e318fbedf3200 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -368,7 +368,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(); @@ -414,8 +442,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const 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, autoware::universe_utils::pose2transform(current_pose)); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, autoware::universe_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(); @@ -481,7 +509,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // Check backwards just in case the Vehicle behind ego is in a different lanelet constexpr double backwards_length = 200.0; const auto prev_lanes = autoware::behavior_path_planner::utils::getBackwardLanelets( - *route_handler, target_lanes, current_pose, backwards_length); + *route_handler, target_lanes, ego_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()); @@ -491,7 +519,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; @@ -508,7 +536,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { const auto arc_length = autoware::motion_utils::calcSignedArcLength( - centerline_path.points, current_pose.position, o.initial_pose.pose.position); + centerline_path.points, ego_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; @@ -1723,6 +1751,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;