Revize behavior planner to work with dynamic lanelets

Signed-off-by: Barış Zeren <bzeren1819@gmail.com>

Author: StepTurtle

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -353,6 +353,7 @@ void BehaviorPathPlannerNode::run()
   // update map
   if (map_ptr) {
     planner_data_->route_handler->setMap(*map_ptr);
+    planner_manager_->resetRootLanelet(planner_data_);
   }
 
   std::unique_lock<std::mutex> lk_manager(mutex_manager_);  // for planner_manager_

planning/behavior_path_planner_common/src/utils/utils.cpp

@@ -503,6 +503,19 @@ bool isEgoOutOfRoute(
     if (utils::isInLanelets(goal_pose, shoulder_lanes)) {
       return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
     }
+    lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr();
+    lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map);
+    lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets();
+    for (auto it = route_lanelets.rbegin(); it != route_lanelets.rend(); ++it) {
+      for (auto it2 = all_lanelets.rbegin(); it2 != all_lanelets.rend(); ++it2) {
+        if ((*it).id() == (*it2).id()) {
+          //          std::cout << "Route lanelet id: " << (*it).id() << std::endl;
+          // select the lanelet goal lanelet
+          goal_lane = *it2;
+          return false;
+        }
+      }
+    }
     return !route_handler->getGoalLanelet(&goal_lane);
   });
   if (is_failed_getting_lanelet) {

planning/route_handler/include/route_handler/route_handler.hpp

@@ -353,6 +353,7 @@ class RouteHandler
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
   lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
   bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getRouteLanelets() const;
 
 private:
   // MUST
@@ -398,7 +399,6 @@ class RouteHandler
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
   bool getLeftLaneletWithinRoute(
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const;
-  lanelet::ConstLanelets getRouteLanelets() const;
   lanelet::ConstLanelets getLaneletSequenceUpTo(
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max(),

planning/route_handler/src/route_handler.cpp

@@ -320,10 +320,10 @@ void RouteHandler::setLaneletsFromRouteMsg()
   }
   route_lanelets_.clear();
   preferred_lanelets_.clear();
-  const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_);
-  if (!is_route_valid) {
-    return;
-  }
+  //  const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_,
+  //  lanelet_map_ptr_); if (!is_route_valid) {
+  //    return;
+  //  }
 
   size_t primitive_size{0};
   for (const auto & route_section : route_ptr_->segments) {
@@ -333,11 +333,19 @@ void RouteHandler::setLaneletsFromRouteMsg()
 
   for (const auto & route_section : route_ptr_->segments) {
     for (const auto & primitive : route_section.primitives) {
-      const auto id = primitive.id;
-      const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
-      route_lanelets_.push_back(llt);
-      if (id == route_section.preferred_primitive.id) {
-        preferred_lanelets_.push_back(llt);
+      try {
+        const auto id = primitive.id;
+        const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
+        route_lanelets_.push_back(llt);
+        if (id == route_section.preferred_primitive.id) {
+          preferred_lanelets_.push_back(llt);
+        }
+      } catch (const std::exception & e) {
+        std::cerr
+          << e.what()
+          << ". Maybe the loaded route was created on a different Map from the current one. "
+             "Try to load the other Route again."
+          << std::endl;
       }
     }
   }
@@ -346,15 +354,31 @@ void RouteHandler::setLaneletsFromRouteMsg()
   if (!route_ptr_->segments.empty()) {
     goal_lanelets_.reserve(route_ptr_->segments.back().primitives.size());
     for (const auto & primitive : route_ptr_->segments.back().primitives) {
-      const auto id = primitive.id;
-      const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
-      goal_lanelets_.push_back(llt);
+      try {
+        const auto id = primitive.id;
+        const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
+        goal_lanelets_.push_back(llt);
+      } catch (const std::exception & e) {
+        std::cerr
+          << e.what()
+          << ". Maybe the loaded route was created on a different Map from the current one. "
+             "Try to load the other Route again."
+          << std::endl;
+      }
     }
     start_lanelets_.reserve(route_ptr_->segments.front().primitives.size());
     for (const auto & primitive : route_ptr_->segments.front().primitives) {
-      const auto id = primitive.id;
-      const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
-      start_lanelets_.push_back(llt);
+      try {
+        const auto id = primitive.id;
+        const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
+        start_lanelets_.push_back(llt);
+      } catch (const std::exception & e) {
+        std::cerr
+          << e.what()
+          << ". Maybe the loaded route was created on a different Map from the current one. "
+             "Try to load the other Route again."
+          << std::endl;
+      }
     }
   }
   is_handler_ready_ = true;