From b76d195f71ac758aaa07841c20b04e0a67b4d69f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 23 Oct 2024 15:39:39 +0900 Subject: [PATCH] chore(goal_planner): compare sampled/filtered candidate paths on plot (#9140) Signed-off-by: Mamoru Sobue Co-authored-by: Kosuke Takeuchi --- .../CMakeLists.txt | 2 +- .../examples/plot_map.cpp | 430 +++++++++++++----- 2 files changed, 321 insertions(+), 111 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index d3f7f7a4015f0..6446f4aec6851 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -27,7 +27,7 @@ if(BUILD_EXAMPLES) FetchContent_Declare( matplotlibcpp17 GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git - GIT_TAG master + GIT_TAG main ) FetchContent_MakeAvailable(matplotlibcpp17) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp index 7e9ccea9ac0c2..3afdb28f80a6c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp @@ -12,11 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" + #include #include #include #include #include +#include +#include #include #include #include @@ -40,15 +44,26 @@ using namespace std::chrono_literals; // NOLINT +using autoware::behavior_path_planner::BehaviorModuleOutput; +using autoware::behavior_path_planner::GoalCandidate; +using autoware::behavior_path_planner::GoalCandidates; +using autoware::behavior_path_planner::GoalPlannerParameters; +using autoware::behavior_path_planner::PlannerData; +using autoware::behavior_path_planner::PullOverPath; +using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_planning_msgs::msg::PathWithLaneId; + void plot_path_with_lane_id( - matplotlibcpp17::axes::Axes & axes, const tier4_planning_msgs::msg::PathWithLaneId path) + matplotlibcpp17::axes::Axes & axes, const PathWithLaneId & path, + const std::string & color = "red") { std::vector xs, ys; for (const auto & point : path.points) { xs.push_back(point.point.pose.position.x); ys.push_back(point.point.pose.position.y); } - axes.plot(Args(xs, ys), Kwargs("color"_a = "red", "linewidth"_a = 1.0)); + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0)); } void plot_lanelet( @@ -82,135 +97,317 @@ void plot_lanelet( Kwargs("color"_a = "black", "linewidth"_a = linewidth, "linestyle"_a = "dashed")); } -int main(int argc, char ** argv) +std::shared_ptr instantiate_planner_data( + rclcpp::Node::SharedPtr node, const std::string & map_path, const LaneletRoute & route_msg) { - rclcpp::init(argc, argv); - - const auto road_shoulder_test_map_path = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner_common") + - "/test_map/road_shoulder/lanelet2_map.osm"; - lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr lanelet_map_ptr = - lanelet::load(road_shoulder_test_map_path, projector, &errors); + const lanelet::LaneletMapPtr lanelet_map_ptr = lanelet::load(map_path, projector, &errors); if (!errors.empty()) { for (const auto & error : errors) { std::cout << error << std::endl; } - return 1; + return nullptr; + } + autoware_map_msgs::msg::LaneletMapBin map_bin; + lanelet::utils::conversion::toBinMsg( + lanelet_map_ptr, &map_bin); // TODO(soblin): pass lanelet_map_ptr to RouteHandler + + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node); + planner_data->route_handler->setMap(map_bin); + planner_data->route_handler->setRoute(route_msg); + + nav_msgs::msg::Odometry odom; + odom.pose.pose = route_msg.start_pose; + auto odometry = std::make_shared(odom); + planner_data->self_odometry = odometry; + + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.accel.accel.linear.x = 0.537018510955429; + accel.accel.accel.linear.y = -0.2435352815388478; + auto accel_ptr = std::make_shared(accel); + planner_data->self_acceleration = accel_ptr; + return planner_data; +} + +bool hasEnoughDistance( + const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path, + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + using autoware::motion_utils::calcSignedArcLength; + + const Pose & current_pose = planner_data->self_odometry->pose.pose; + const double current_vel = planner_data->self_odometry->twist.twist.linear.x; + + // when the path is separated and start_pose is close, + // once stopped, the vehicle cannot start again. + // so need enough distance to restart. + // distance to restart should be less than decide_path_distance. + // otherwise, the goal would change immediately after departure. + const bool is_separated_path = pull_over_path.partial_paths().size() > 1; + const double distance_to_start = calcSignedArcLength( + long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); + const double distance_to_restart = parameters.decide_path_distance / 2; + const double eps_vel = 0.01; + const bool is_stopped = std::abs(current_vel) < eps_vel; + if (is_separated_path && is_stopped && distance_to_start < distance_to_restart) { + return false; + } + + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data, parameters.maximum_deceleration, parameters.maximum_jerk, 0.0); + if (!current_to_stop_distance) { + return false; } - autoware_planning_msgs::msg::LaneletRoute route_msg; - route_msg.start_pose = geometry_msgs::build() - .position(geometry_msgs::build() - .x(530.707103865218) - .y(436.8758309382301) - .z(100.0)) - .orientation(geometry_msgs::build() - .x(0.0) - .y(0.0) - .z(0.2187392230193602) - .w(0.9757833531644647)); - route_msg.goal_pose = geometry_msgs::build() - .position(geometry_msgs::build() - .x(571.8018955394537) - .y(435.6819504507543) - .z(100.0)) - .orientation(geometry_msgs::build() - .x(0.0) - .y(0.0) - .z(-0.3361155457734493) - .w(0.9418207578352774)); /* + // If the stop line is subtly exceeded, it is assumed that there is not enough distance to the + // starting point of parking, so to prevent this, once the vehicle has stopped, it also has a + // stop_distance_buffer to allow for the amount exceeded. + const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; + if (distance_to_start + buffer < *current_to_stop_distance) { + return false; + }*/ + + return true; +} + +std::vector selectPullOverPaths( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) +{ + using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; + using autoware::motion_utils::calcSignedArcLength; + + const auto & goal_pose = planner_data->route_handler->getOriginalGoalPose(); + const double backward_length = + parameters.backward_goal_search_length + parameters.decide_path_distance; + + std::vector sorted_path_indices; + sorted_path_indices.reserve(pull_over_path_candidates.size()); + + std::unordered_map goal_candidate_map; + for (const auto & goal_candidate : goal_candidates) { + goal_candidate_map[goal_candidate.id] = goal_candidate; + } + for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { + const auto & path = pull_over_path_candidates[i]; + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); + if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { + sorted_path_indices.push_back(i); + } + } + + const double prev_path_front_to_goal_dist = calcSignedArcLength( + previous_module_output.path.points, + previous_module_output.path.points.front().point.pose.position, goal_pose.position); + const auto & long_tail_reference_path = [&]() { + if (prev_path_front_to_goal_dist > backward_length) { + return previous_module_output.path; + } + // get road lanes which is at least backward_length[m] behind the goal + const auto road_lanes = getExtendedCurrentLanesFromPath( + previous_module_output.path, planner_data, backward_length, 0.0, false); + const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + return planner_data->route_handler->getCenterLinePath( + road_lanes, std::max(0.0, goal_pose_length - backward_length), + goal_pose_length + parameters.forward_goal_search_length); + }(); + + sorted_path_indices.erase( + std::remove_if( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t i) { + return !hasEnoughDistance( + pull_over_path_candidates[i], long_tail_reference_path, planner_data, parameters); + }), + sorted_path_indices.end()); + + // compare to sort pull_over_path_candidates based on the order in efficient_path_order + const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { + const auto & order = parameters.efficient_path_order; + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); + return a_pos < b_pos; + }; + + const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins; + const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins; + + const auto [margins, margins_with_zero] = + std::invoke([&]() -> std::tuple, std::vector> { + std::vector margins = soft_margins; + margins.insert(margins.end(), hard_margins.begin(), hard_margins.end()); + std::vector margins_with_zero = margins; + margins_with_zero.push_back(0.0); + return std::make_tuple(margins, margins_with_zero); + }); + + // Create a map of PullOverPath pointer to largest collision check margin + std::map path_id_to_rough_margin_map; + const auto & target_objects = autoware_perception_msgs::msg::PredictedObjects{}; + for (const size_t i : sorted_path_indices) { + const auto & path = pull_over_path_candidates[i]; + const double distance = + autoware::behavior_path_planner::utils::path_safety_checker::calculateRoughDistanceToObjects( + path.parking_path(), target_objects, planner_data->parameters, false, "max"); + auto it = std::lower_bound( + margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); + if (it == margins_with_zero.end()) { + path_id_to_rough_margin_map[path.id()] = margins_with_zero.back(); + } else { + path_id_to_rough_margin_map[path.id()] = *it; + } + } + + // sorts in descending order so the item with larger margin comes first + std::stable_sort( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; + if ( + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01) { + return false; + } + return path_id_to_rough_margin_map[a.id()] > path_id_to_rough_margin_map[b.id()]; + }); + + // STEP2-3: Sort by curvature + // If the curvature is less than the threshold, prioritize the path. + const auto isHighCurvature = [&](const PullOverPath & path) -> bool { + return path.parking_path_max_curvature() >= parameters.high_curvature_threshold; + }; + + const auto isSoftMargin = [&](const PullOverPath & path) -> bool { + const double margin = path_id_to_rough_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_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01; + }; + + // NOTE: this is just partition sort based on curvature threshold within each sub partitions + std::stable_sort( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; + // 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; + }); + + // STEP2-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_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + // if any of following conditions are met, sort by path type priority + // - both are soft margin + // - both are same hard margin + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; + if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { + return comparePathTypePriority(a, b); + } + // otherwise, keep the order. + return false; + }); + } + + std::vector selected; + for (const auto & sorted_index : sorted_path_indices) { + selected.push_back(pull_over_path_candidates.at(sorted_index)); + } + + return selected; +} + +int main(int argc, char ** argv) +{ + using autoware::behavior_path_planner::utils::getReferencePath; + rclcpp::init(argc, argv); + + autoware_planning_msgs::msg::LaneletRoute route_msg; + route_msg.start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(729.944).y(695.124).z(381.18)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.437138).w( + 0.899395)); route_msg.goal_pose = geometry_msgs::build() - .position( - geometry_msgs::build().x(568.836731).y(437.871904).z(100.0)) + .position(geometry_msgs::build().x(797.526).y(694.105).z(381.18)) .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(-0.292125).w( - 0.956380)); - */ + geometry_msgs::build().x(0.0).y(0.0).z(-0.658723).w( + 0.752386)); route_msg.segments = std::vector{ autoware_planning_msgs::build() .preferred_primitive( autoware_planning_msgs::build() - .id(17757) + .id(15214) .primitive_type("")) .primitives(std::vector{ autoware_planning_msgs::build() - .id(17756) + .id(15214) .primitive_type("lane"), autoware_planning_msgs::build() - .id(17757) + .id(15213) .primitive_type("lane"), }), autoware_planning_msgs::build() .preferred_primitive( autoware_planning_msgs::build() - .id(18494) + .id(15226) .primitive_type("")) .primitives(std::vector{ autoware_planning_msgs::build() - .id(18494) + .id(15226) .primitive_type("lane"), autoware_planning_msgs::build() - .id(18493) + .id(15225) .primitive_type("lane"), }), autoware_planning_msgs::build() .preferred_primitive( autoware_planning_msgs::build() - .id(18496) + .id(15228) .primitive_type("")) .primitives(std::vector{ autoware_planning_msgs::build() - .id(18496) + .id(15228) .primitive_type("lane"), autoware_planning_msgs::build() - .id(18497) + .id(15229) + .primitive_type("lane"), + }), + autoware_planning_msgs::build() + .preferred_primitive( + autoware_planning_msgs::build() + .id(15231) + .primitive_type("")) + .primitives(std::vector{ + autoware_planning_msgs::build() + .id(15231) .primitive_type("lane"), }), }; route_msg.allow_modification = false; - autoware_map_msgs::msg::LaneletMapBin map_bin; - lanelet::utils::conversion::toBinMsg( - lanelet_map_ptr, &map_bin); // TODO(soblin): pass lanelet_map_ptr to RouteHandler - /* - reference: - https://github.com/ros2/rclcpp/blob/ee94bc63e4ce47a502891480a2796b53d54fcdfc/rclcpp/test/rclcpp/test_parameter_client.cpp#L927 - */ - /* - auto node = rclcpp::Node::make_shared( - "node", "namespace", rclcpp::NodeOptions().allow_undeclared_parameters(true)); - auto param_node = std::make_shared(node, ""); - { - auto future = param_node->load_parameters( - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + - "/config/behavior_path_planner.param.yaml"); - if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(node->get_logger(), "failed to load behavior_path_planner.param.yaml"); - } - } - { - auto future = param_node->load_parameters( - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + - "/config/drivable_area_expansion.param.yaml"); - if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(node->get_logger(), "failed to load drivable_area_expansion.param.yaml"); - } - } - { - auto future = param_node->load_parameters( - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + - "/config/scene_module_manager.param.yaml"); - if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO(node->get_logger(), "failed to load scene_module_manager.param.yaml"); - } - } - */ auto node_options = rclcpp::NodeOptions{}; node_options.parameter_overrides( std::vector{{"launch_modules", std::vector{}}}); @@ -238,20 +435,16 @@ int main(int argc, char ** argv) "/config/goal_planner.param.yaml"}); auto node = rclcpp::Node::make_shared("plot_map", node_options); - auto planner_data = std::make_shared(); - planner_data->init_parameters(*node); - planner_data->route_handler->setMap(map_bin); - planner_data->route_handler->setRoute(route_msg); - nav_msgs::msg::Odometry odom; - odom.pose.pose = route_msg.start_pose; - auto odometry = std::make_shared(odom); - planner_data->self_odometry = odometry; + auto planner_data = instantiate_planner_data( + node, + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/test_map/road_shoulder/lanelet2_map.osm", + route_msg); + lanelet::ConstLanelet current_route_lanelet; planner_data->route_handler->getClosestLaneletWithinRoute( route_msg.start_pose, ¤t_route_lanelet); - std::cout << "current_route_lanelet is " << current_route_lanelet.id() << std::endl; - const auto reference_path = - autoware::behavior_path_planner::utils::getReferencePath(current_route_lanelet, planner_data); + const auto reference_path = getReferencePath(current_route_lanelet, planner_data); auto goal_planner_parameter = autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters( node.get(), "goal_planner."); @@ -262,30 +455,47 @@ int main(int argc, char ** argv) lane_departure_checker_params.footprint_extra_margin = goal_planner_parameter.lane_departure_check_expansion_margin; lane_departure_checker.setParam(lane_departure_checker_params); - auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( - *node, goal_planner_parameter, lane_departure_checker); - const auto pull_over_path_opt = - shift_pull_over_planner.plan(0, 0, planner_data, reference_path, route_msg.goal_pose); + autoware::behavior_path_planner::GoalSearcher goal_searcher( + goal_planner_parameter, vehicle_info.createFootprint()); + const auto goal_candidates = goal_searcher.search(planner_data); pybind11::scoped_interpreter guard{}; auto plt = matplotlibcpp17::pyplot::import(); - auto [fig, ax] = plt.subplots(); + auto [fig, axes] = plt.subplots(1, 2); - const std::vector ids{17756, 17757, 18493, 18494, 18496, 18497, - 18492, 18495, 18498, 18499, 18500}; + auto & ax1 = axes[0]; + auto & ax2 = axes[1]; + const std::vector ids{15213, 15214, 15225, 15226, 15224, 15227, + 15228, 15229, 15230, 15231, 15232}; for (const auto & id : ids) { - const auto lanelet = lanelet_map_ptr->laneletLayer.get(id); - plot_lanelet(ax, lanelet); + const auto lanelet = planner_data->route_handler->getLaneletMapPtr()->laneletLayer.get(id); + plot_lanelet(ax1, lanelet); + plot_lanelet(ax2, lanelet); } - plot_path_with_lane_id(ax, reference_path.path); - std::cout << pull_over_path_opt.has_value() << std::endl; - if (pull_over_path_opt) { - const auto & pull_over_path = pull_over_path_opt.value(); - const auto & full_path = pull_over_path.full_path; - plot_path_with_lane_id(ax, full_path); + plot_path_with_lane_id(ax1, reference_path.path, "green"); + + std::vector candidates; + for (const auto & goal_candidate : goal_candidates) { + auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( + *node, goal_planner_parameter, lane_departure_checker); + const auto pull_over_path_opt = + shift_pull_over_planner.plan(goal_candidate, 0, planner_data, reference_path); + if (pull_over_path_opt) { + const auto & pull_over_path = pull_over_path_opt.value(); + const auto & full_path = pull_over_path.full_path(); + candidates.push_back(pull_over_path); + plot_path_with_lane_id(ax1, full_path); + } + } + const auto filtered_paths = selectPullOverPaths( + candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path); + for (const auto & filtered_path : filtered_paths) { + plot_path_with_lane_id(ax2, filtered_path.full_path(), "blue"); } - ax.set_aspect(Args("equal")); + + ax1.set_aspect(Args("equal")); + ax2.set_aspect(Args("equal")); plt.show(); rclcpp::shutdown();