From 55a19c006244c7092e40c947c02d1c4f56073cd6 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Tue, 26 Apr 2022 23:18:04 +0900
Subject: [PATCH] feat(behavior_path_planner): make initial ego pose in
 drivable area (#769)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../behavior_path_planner/src/utilities.cpp   | 20 ++++++++--
 .../include/route_handler/route_handler.hpp   |  2 +
 planning/route_handler/src/route_handler.cpp  | 38 ++++++++++++++-----
 3 files changed, 46 insertions(+), 14 deletions(-)

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
index 4a08a0134e92..ef5dfe05c589 100644
--- a/planning/behavior_path_planner/src/utilities.cpp
+++ b/planning/behavior_path_planner/src/utilities.cpp
@@ -1129,10 +1129,22 @@ OccupancyGrid generateDrivableArea(
   const double width = max_x - min_x;
   const double height = max_y - min_y;
 
-  lanelet::ConstLanelets drivable_lanes = lanes;
-  if (containsGoal(lanes, route_handler->getGoalLaneId())) {
-    const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length);
-    drivable_lanes.insert(drivable_lanes.end(), lanes_after_goal.begin(), lanes_after_goal.end());
+  lanelet::ConstLanelets drivable_lanes;
+  {  // add lanes which covers initial and final footprints
+    // 1. add preceding lanes before current pose
+    const auto lanes_before_current_pose =
+      route_handler->getLanesBeforePose(current_pose->pose, vehicle_length);
+    drivable_lanes.insert(
+      drivable_lanes.end(), lanes_before_current_pose.begin(), lanes_before_current_pose.end());
+
+    // 2. add lanes
+    drivable_lanes.insert(drivable_lanes.end(), lanes.begin(), lanes.end());
+
+    // 3. add succeeding lanes after goal
+    if (containsGoal(lanes, route_handler->getGoalLaneId())) {
+      const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length);
+      drivable_lanes.insert(drivable_lanes.end(), lanes_after_goal.begin(), lanes_after_goal.end());
+    }
   }
 
   OccupancyGrid occupancy_grid;
diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
index 7cf38a40d0ee..ea7f2e7c362e 100644
--- a/planning/route_handler/include/route_handler/route_handler.hpp
+++ b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -93,6 +93,8 @@ class RouteHandler
   Pose getPullOverGoalPose() const;
   lanelet::Id getGoalLaneId() const;
   bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const;
+  std::vector<lanelet::ConstLanelet> getLanesBeforePose(
+    const geometry_msgs::msg::Pose & pose, const double vehicle_length) const;
   std::vector<lanelet::ConstLanelet> getLanesAfterGoal(const double vehicle_length) const;
 
   // for lanelet
diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
index ac48a20ea020..5cfcbf511e3e 100644
--- a/planning/route_handler/src/route_handler.cpp
+++ b/planning/route_handler/src/route_handler.cpp
@@ -353,22 +353,40 @@ lanelet::ConstPolygon3d RouteHandler::getIntersectionAreaById(const lanelet::Id
 
 Header RouteHandler::getRouteHeader() const { return route_msg_.header; }
 
+std::vector<lanelet::ConstLanelet> RouteHandler::getLanesBeforePose(
+  const geometry_msgs::msg::Pose & pose, const double vehicle_length) const
+{
+  lanelet::ConstLanelet pose_lanelet;
+  if (!getClosestLaneletWithinRoute(pose, &pose_lanelet)) {
+    return std::vector<lanelet::ConstLanelet>{};
+  }
+
+  const double min_preceding_length = vehicle_length * 2;
+  const auto preceding_lanes_vec = lanelet::utils::query::getPrecedingLaneletSequences(
+    routing_graph_ptr_, pose_lanelet, min_preceding_length);
+  if (preceding_lanes_vec.empty()) {
+    return std::vector<lanelet::ConstLanelet>{};
+  }
+
+  return preceding_lanes_vec.front();
+}
+
 std::vector<lanelet::ConstLanelet> RouteHandler::getLanesAfterGoal(
   const double vehicle_length) const
 {
   lanelet::ConstLanelet goal_lanelet;
-  if (getGoalLanelet(&goal_lanelet)) {
-    const double min_succeeding_length = vehicle_length * 2;
-    const auto succeeding_lanes_vec = lanelet::utils::query::getSucceedingLaneletSequences(
-      routing_graph_ptr_, goal_lanelet, min_succeeding_length);
-    if (succeeding_lanes_vec.empty()) {
-      return std::vector<lanelet::ConstLanelet>{};
-    } else {
-      return succeeding_lanes_vec.front();
-    }
-  } else {
+  if (!getGoalLanelet(&goal_lanelet)) {
     return std::vector<lanelet::ConstLanelet>{};
   }
+
+  const double min_succeeding_length = vehicle_length * 2;
+  const auto succeeding_lanes_vec = lanelet::utils::query::getSucceedingLaneletSequences(
+    routing_graph_ptr_, goal_lanelet, min_succeeding_length);
+  if (succeeding_lanes_vec.empty()) {
+    return std::vector<lanelet::ConstLanelet>{};
+  }
+
+  return succeeding_lanes_vec.front();
 }
 
 lanelet::ConstLanelets RouteHandler::getRouteLanelets() const { return route_lanelets_; }