feat(behavior_path_planner): make initial ego pose in drivable area (#…

…769) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · May 26, 2022 · f65d6d5 · f65d6d5
1 parent 8a80db0
commit f65d6d5
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 14 deletions.
diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
@@ -1136,10 +1136,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
@@ -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
@@ -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_; }