From a939962526b9655bd64f84466c73d23639be77e5 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 19 Aug 2023 01:32:05 +0900 Subject: [PATCH] feat(goal_planner): add lane ids to freespace path (#4668) Signed-off-by: kosuke55 --- .../utils/goal_planner/freespace_pull_over.hpp | 1 + .../behavior_path_planner/utils/path_utils.hpp | 3 ++- .../utils/goal_planner/freespace_pull_over.cpp | 16 ++++++++++++++-- .../src/utils/path_utils.cpp | 9 +++++++-- 4 files changed, 24 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index 1378be5461a64..b698892907789 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase std::unique_ptr planner_; double velocity_; bool use_back_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index bdca6a2fdf085..191cd3e10def5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -75,7 +75,8 @@ std::pair getPathTurnSignal( const BehaviorPathPlannerParameters & common_parameter); PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets); std::vector getReversingIndices(const PathWithLaneId & path); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index ebfac8a347134..f4aabb13b8916 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -25,7 +25,9 @@ namespace behavior_path_planner FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) -: PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} +: PullOverPlannerBase{node, parameters}, + velocity_{parameters.freespace_parking_velocity}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); @@ -58,8 +60,18 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) return {}; } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); + PathWithLaneId path = - utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); const auto reverse_indices = utils::getReversingIndices(path); std::vector partial_paths = utils::dividePath(path, reverse_indices); diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index d2d70b0a46dfc..d455cd66ae769 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -336,14 +336,19 @@ std::pair getPathTurnSignal( } PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets) { PathWithLaneId path; path.header = waypoints.header; for (const auto & waypoint : waypoints.waypoints) { PathPointWithLaneId point{}; point.point.pose = waypoint.pose.pose; - // point.lane_id = // todo + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + } + } point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; path.points.push_back(point); }