feat(goal_planner): add lane ids to freespace path (autowarefoundatio…

…n#4668) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Aug 18, 2023 · 5ed7ca3 · 5ed7ca3
1 parent 93478b7
commit 5ed7ca3
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 5 deletions.
diff --git a/...ior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/...ior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp
@@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase
   std::unique_ptr<AbstractPlanningAlgorithm> 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
@@ -75,7 +75,8 @@ std::pair<TurnIndicatorsCommand, double> 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<size_t> 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
@@ -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<PullOverPath> 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<PathWithLaneId> 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
@@ -336,14 +336,19 @@ std::pair<TurnIndicatorsCommand, double> 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);
   }