From d6af4057bd71d170e2fc1e9b7f139f965ac3302e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 28 Oct 2024 18:40:35 +0900 Subject: [PATCH] feat(goal_planner): align vehicle footprint heading parallel to parking side lane boundary (#9159) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 2 +- .../examples/plot_map.cpp | 116 ++++++++++++++++-- .../goal_planner_module.hpp | 1 - .../util.hpp | 12 +- .../src/goal_planner_module.cpp | 65 ++-------- .../src/goal_searcher.cpp | 22 ++++ .../src/util.cpp | 82 +++++++++++++ 7 files changed, 230 insertions(+), 70 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 6446f4aec6851..d3f7f7a4015f0 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 main + GIT_TAG master ) 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 3afdb28f80a6c..af64dc5220010 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 @@ -13,6 +13,7 @@ // limitations under the License. #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" #include #include @@ -23,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -40,6 +42,7 @@ #include #include +#include #include using namespace std::chrono_literals; // NOLINT @@ -54,16 +57,58 @@ using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDec using autoware_planning_msgs::msg::LaneletRoute; using tier4_planning_msgs::msg::PathWithLaneId; +void plot_footprint( + matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint, + const std::string & color = "blue") +{ + std::vector xs, ys; + for (const auto & pt : footprint) { + xs.push_back(pt.x()); + ys.push_back(pt.y()); + } + xs.push_back(xs.front()); + ys.push_back(ys.front()); + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted")); +} + +void plot_goal_candidates( + matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals, + const autoware::universe_utils::LinearRing2d & local_footprint, + const std::string & color = "green") +{ + std::vector xs, ys; + std::vector yaw_cos, yaw_sin; + for (const auto & goal : goals) { + const auto goal_footprint = + transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); + plot_footprint(axes, goal_footprint); + xs.push_back(goal.goal_pose.position.x); + ys.push_back(goal.goal_pose.position.y); + axes.text(Args(xs.back(), ys.back(), std::to_string(goal.id))); + const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; + yaw_cos.push_back(std::cos(yaw)); + yaw_sin.push_back(std::sin(yaw)); + } + axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 1.0)); +} + void plot_path_with_lane_id( matplotlibcpp17::axes::Axes & axes, const PathWithLaneId & path, - const std::string & color = "red") + const std::string & color = "red", const std::string & label = "") { 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 = color, "linewidth"_a = 1.0)); + if (label == "") { + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0)); + } else { + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0, "label"_a = label)); + } } void plot_lanelet( @@ -97,7 +142,7 @@ void plot_lanelet( Kwargs("color"_a = "black", "linewidth"_a = linewidth, "linestyle"_a = "dashed")); } -std::shared_ptr instantiate_planner_data( +std::shared_ptr instantiate_planner_data( rclcpp::Node::SharedPtr node, const std::string & map_path, const LaneletRoute & route_msg) { lanelet::ErrorMessages errors{}; @@ -336,6 +381,51 @@ std::vector selectPullOverPaths( return selected; } +std::optional calculate_centerline_path( + const geometry_msgs::msg::Pose & original_goal_pose, + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + const auto refined_goal_opt = + autoware::behavior_path_planner::goal_planner_utils::calcRefinedGoal( + original_goal_pose, planner_data->route_handler, true, + planner_data->parameters.vehicle_length, planner_data->parameters.base_link2front, + planner_data->parameters.base_link2front, parameters); + if (!refined_goal_opt) { + return std::nullopt; + } + const auto & refined_goal = refined_goal_opt.value(); + + const auto & route_handler = planner_data->route_handler; + const double forward_length = parameters.forward_goal_search_length; + const double backward_length = parameters.backward_goal_search_length; + const bool use_bus_stop_area = parameters.bus_stop_area.use_bus_stop_area; + /* + const double margin_from_boundary = parameters.margin_from_boundary; + const double lateral_offset_interval = use_bus_stop_area + ? parameters.bus_stop_area.lateral_offset_interval + : parameters.lateral_offset_interval; + const double max_lateral_offset = parameters.max_lateral_offset; + const double ignore_distance_from_lane_start = parameters.ignore_distance_from_lane_start; + */ + + const auto pull_over_lanes = + autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes( + *route_handler, true, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + const auto departure_check_lane = + autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *route_handler, true); + const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, refined_goal); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + const double longitudinal_interval = use_bus_stop_area + ? parameters.bus_stop_area.goal_search_interval + : parameters.goal_search_interval; + auto center_line_path = autoware::behavior_path_planner::utils::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval); + return center_line_path; +} + int main(int argc, char ** argv) { using autoware::behavior_path_planner::utils::getReferencePath; @@ -455,8 +545,8 @@ 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); - autoware::behavior_path_planner::GoalSearcher goal_searcher( - goal_planner_parameter, vehicle_info.createFootprint()); + const auto footprint = vehicle_info.createFootprint(); + autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); const auto goal_candidates = goal_searcher.search(planner_data); pybind11::scoped_interpreter guard{}; @@ -473,7 +563,9 @@ int main(int argc, char ** argv) plot_lanelet(ax2, lanelet); } - plot_path_with_lane_id(ax1, reference_path.path, "green"); + plot_goal_candidates(ax1, goal_candidates, footprint); + + plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); std::vector candidates; for (const auto & goal_candidate : goal_candidates) { @@ -488,14 +580,18 @@ int main(int argc, char ** argv) plot_path_with_lane_id(ax1, full_path); } } - const auto filtered_paths = selectPullOverPaths( + [[maybe_unused]] 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"); - } + const auto centerline_path = + calculate_centerline_path(route_msg.goal_pose, planner_data, goal_planner_parameter); + if (centerline_path) { + plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path"); + } ax1.set_aspect(Args("equal")); ax2.set_aspect(Args("equal")); + ax1.legend(); + ax2.legend(); plt.show(); rclcpp::shutdown(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index f3fa2898ce833..6caeff97c2607 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -364,7 +364,6 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr occupancy_grid_map) const; // goal seach - Pose calcRefinedGoal(const Pose & goal_pose) const; GoalCandidates generateGoalCandidates() const; // stop or decelerate diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 52dfbbddc79ff..f00d9d41622e9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -20,16 +20,17 @@ #include -#include "visualization_msgs/msg/detail/marker_array__struct.hpp" #include #include #include #include #include +#include #include #include +#include #include #include @@ -175,6 +176,15 @@ lanelet::Points3d combineLanePoints( lanelet::Lanelet createDepartureCheckLanelet( const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, const bool left_side_parking); + +std::optional calcRefinedGoal( + const Pose & goal_pose, const std::shared_ptr route_handler, + const bool left_side_parking, const double vehicle_width, const double base_link2front, + const double base_link2rear, const GoalPlannerParameters & parameters); + +std::optional calcClosestPose( + const lanelet::ConstLineString3d line, const Point & query_point); + } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ 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 b17a5588835a5..f72c96fedbe34 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 @@ -585,8 +585,14 @@ void GoalPlannerModule::updateData() // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { - goal_searcher_->setReferenceGoal( - calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose())); + const auto refined_goal = goal_planner_utils::calcRefinedGoal( + planner_data_->route_handler->getOriginalGoalPose(), planner_data_->route_handler, + left_side_parking_, planner_data_->parameters.vehicle_width, + planner_data_->parameters.base_link2front, planner_data_->parameters.base_link2rear, + *parameters_); + if (refined_goal) { + goal_searcher_->setReferenceGoal(refined_goal.value()); + } thread_safe_data_.set_goal_candidates(generateGoalCandidates()); } @@ -757,61 +763,6 @@ double GoalPlannerModule::calcModuleRequestLength() const return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } -Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const -{ - const double vehicle_width = planner_data_->parameters.vehicle_width; - const double base_link2front = planner_data_->parameters.base_link2front; - const double base_link2rear = planner_data_->parameters.base_link2rear; - - const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - - lanelet::Lanelet closest_pull_over_lanelet{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); - - // calc closest center line pose - Pose center_pose{}; - { - // find position - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); - const auto segment = lanelet::utils::getClosestSegment( - lanelet::utils::to2D(lanelet_point), closest_pull_over_lanelet.centerline()); - const auto p1 = segment.front().basicPoint(); - const auto p2 = segment.back().basicPoint(); - const auto direction_vector = (p2 - p1).normalized(); - const auto p1_to_goal = lanelet_point.basicPoint() - p1; - const double s = direction_vector.dot(p1_to_goal); - const auto refined_point = p1 + direction_vector * s; - - center_pose.position.x = refined_point.x(); - center_pose.position.y = refined_point.y(); - center_pose.position.z = refined_point.z(); - - // find orientation - const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, yaw); - center_pose.orientation = tf2::toMsg(tf_quat); - } - - const auto distance_from_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, - left_side_parking_); - if (!distance_from_bound) { - RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); - return goal_pose; - } - - const double sign = left_side_parking_ ? -1.0 : 1.0; - const double offset_from_center_line = - sign * (distance_from_bound.value() + parameters_->margin_from_boundary); - - const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); - - return refined_goal_pose; -} - bool GoalPlannerModule::planFreespacePath( std::shared_ptr planner_data, const std::shared_ptr goal_searcher, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index ea0816954d0e1..0bca5ba7a7864 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -20,8 +20,11 @@ #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" #include "autoware_lanelet2_extension/utility/utilities.hpp" +#include + #include #include @@ -193,8 +196,27 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p continue; } + // modify the goal_pose orientation so that vehicle footprint front heading is parallel to the + // lane boundary + const auto vehicle_front_midpoint = + (transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontLeftIndex) + + transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontRightIndex)) / + 2.0; + lanelet::ConstLanelet vehicle_front_closest_lanelet; + lanelet::utils::query::getClosestLanelet( + pull_over_lanes, search_pose, &vehicle_front_closest_lanelet); + const auto vehicle_front_pose_for_bound_opt = goal_planner_utils::calcClosestPose( + left_side_parking_ ? vehicle_front_closest_lanelet.leftBound() + : vehicle_front_closest_lanelet.rightBound(), + autoware::universe_utils::createPoint( + vehicle_front_midpoint.x(), vehicle_front_midpoint.y(), search_pose.position.z)); + if (!vehicle_front_pose_for_bound_opt) { + continue; + } + const auto & vehicle_front_pose_for_bound = vehicle_front_pose_for_bound_opt.value(); GoalCandidate goal_candidate{}; goal_candidate.goal_pose = search_pose; + goal_candidate.goal_pose.orientation = vehicle_front_pose_for_bound.orientation; goal_candidate.lateral_offset = dy; goal_candidate.id = goal_id; goal_id++; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 3815acc4561e7..75cdb2d892510 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -740,4 +740,86 @@ lanelet::Lanelet createDepartureCheckLanelet( left_side_parking ? inner_linestring : outer_linestring); } +std::optional calcRefinedGoal( + const Pose & goal_pose, const std::shared_ptr route_handler, + const bool left_side_parking, const double vehicle_width, const double base_link2front, + const double base_link2rear, const GoalPlannerParameters & parameters) +{ + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + + lanelet::Lanelet closest_pull_over_lanelet{}; + lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); + + // calc closest center line pose + Pose center_pose{}; + { + // find position + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); + const auto segment = lanelet::utils::getClosestSegment( + lanelet::utils::to2D(lanelet_point), closest_pull_over_lanelet.centerline()); + const auto p1 = segment.front().basicPoint(); + const auto p2 = segment.back().basicPoint(); + const auto direction_vector = (p2 - p1).normalized(); + const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const double s = direction_vector.dot(p1_to_goal); + const auto refined_point = p1 + direction_vector * s; + + center_pose.position.x = refined_point.x(); + center_pose.position.y = refined_point.y(); + center_pose.position.z = refined_point.z(); + + // find orientation + const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + center_pose.orientation = tf2::toMsg(tf_quat); + } + + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking); + if (!distance_from_bound) { + return std::nullopt; + } + + const double sign = left_side_parking ? -1.0 : 1.0; + const double offset_from_center_line = + sign * (distance_from_bound.value() + parameters.margin_from_boundary); + + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + + return refined_goal_pose; +} + +std::optional calcClosestPose( + const lanelet::ConstLineString3d line, const Point & query_point) +{ + const auto segment = + lanelet::utils::getClosestSegment(lanelet::BasicPoint2d{query_point.x, query_point.y}, line); + if (segment.empty()) { + return std::nullopt; + } + + const Eigen::Vector2d direction( + (segment.back().basicPoint2d() - segment.front().basicPoint2d()).normalized()); + const Eigen::Vector2d xf(segment.front().basicPoint2d()); + const Eigen::Vector2d x(query_point.x, query_point.y); + const Eigen::Vector2d p = xf + (x - xf).dot(direction) * direction; + + geometry_msgs::msg::Pose closest_pose; + closest_pose.position.x = p.x(); + closest_pose.position.y = p.y(); + closest_pose.position.z = query_point.z; + + const double lane_yaw = + std::atan2(segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); + tf2::Quaternion q; + q.setRPY(0, 0, lane_yaw); + closest_pose.orientation = tf2::toMsg(q); + + return closest_pose; +} + } // namespace autoware::behavior_path_planner::goal_planner_utils