feat(behavior_path_planner): revise behavior planner to work with dynamic lanelets

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -9,6 +9,8 @@
   <arg name="planning_validator_param_path"/>
   <!-- Auto mode setting-->
   <arg name="enable_all_modules_auto_mode"/>
+  <!-- Differential lanelet map loading setting -->
+  <arg name="enable_differential_map_loading"/>
   <arg name="is_simulation"/>
 
   <group>
@@ -26,6 +28,7 @@
       <push-ros-namespace namespace="scenario_planning"/>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
         <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+        <arg name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
         <arg name="is_simulation" value="$(var is_simulation)"/>
       </include>
     </group>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml

@@ -1,5 +1,6 @@
 <launch>
   <arg name="enable_all_modules_auto_mode"/>
+  <arg name="enable_differential_map_loading"/>
   <arg name="is_simulation"/>
 
   <!-- lane_driving scenario -->
@@ -12,6 +13,7 @@
         <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml">
           <arg name="container_type" value="component_container_mt"/>
           <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+          <arg name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
           <arg name="is_simulation" value="$(var is_simulation)"/>
           <!-- This condition should be true if run_out module is enabled and its detection method is Points -->
           <arg name="launch_compare_map_pipeline" value="false"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -1,8 +1,12 @@
 <launch>
+  <arg name="enable_all_modules_auto_mode"/>
+  <arg name="enable_differential_map_loading"/>
+
   <arg name="input_route_topic_name" default="/planning/mission_planning/route"/>
   <arg name="input_traffic_light_topic_name" default="/perception/traffic_light_recognition/traffic_signals"/>
   <arg name="input_virtual_traffic_light_topic_name" default="/awapi/tmp/virtual_traffic_light_states"/>
-  <arg name="input_vector_map_topic_name" default="/map/vector_map"/>
+  <arg name="input_vector_map_topic_name" default="/map/vector_map" unless="$(var enable_differential_map_loading)"/>
+  <arg name="input_vector_map_topic_name" default="/map/dynamic_vector_map" if="$(var enable_differential_map_loading)"/>
   <arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
   <arg name="enable_all_modules_auto_mode"/>
   <arg name="is_simulation"/>
@@ -192,6 +196,7 @@
       <param from="$(var nearest_search_param_path)"/>
       <param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
       <param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+      <param name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
       <param name="is_simulation" value="$(var is_simulation)"/>
       <param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
       <param from="$(var behavior_path_planner_static_obstacle_avoidance_module_param_path)"/>

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -1,5 +1,6 @@
 <launch>
   <arg name="enable_all_modules_auto_mode"/>
+  <arg name="enable_differential_map_loading"/>
   <arg name="is_simulation"/>
   <!-- scenario selector -->
   <group>
@@ -55,6 +56,7 @@
     <group>
       <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml">
         <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+        <arg name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
         <arg name="is_simulation" value="$(var is_simulation)"/>
       </include>
     </group>

planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp

@@ -72,8 +72,9 @@ class RouteHandler
   explicit RouteHandler(const LaneletMapBin & map_msg);
 
   // non-const methods
-  void setMap(const LaneletMapBin & map_msg);
-  void setRoute(const LaneletRoute & route_msg);
+  void setMap(const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet = false);
+  void setRoute(
+    const LaneletRoute & route_msg, const bool & is_enable_differential_lanelet = false);
   void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets);
   void clearRoute();
 
@@ -325,6 +326,7 @@ class RouteHandler
    * @return vector of shoulder lanelets intersecting with given pose.
    */
   lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const;
+  lanelet::ConstLanelets getRouteLanelets() const;
 
 private:
   // MUST
@@ -348,14 +350,24 @@ class RouteHandler
   Pose original_goal_pose_;
 
   // non-const methods
-  void setLaneletsFromRouteMsg();
+  void setLaneletsFromRouteMsg(const bool & is_enable_differential_lanelet = false);
 
   // const methods
   // for routing
   lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const;
 
   // for lanelet
-  lanelet::ConstLanelets getRouteLanelets() const;
+  bool isBijectiveConnection(
+    const lanelet::ConstLanelets & lanelet_section1,
+    const lanelet::ConstLanelets & lanelet_section2) const;
+  bool getPreviousLaneletWithinRouteExceptGoal(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
+  bool getNextLaneletWithinRouteExceptStart(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
+  bool getRightLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
+  bool getLeftLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const;
   lanelet::ConstLanelets getLaneletSequenceUpTo(
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max(),

planning/autoware_route_handler/src/route_handler.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -182,7 +182,8 @@
   route_ptr_ = nullptr;
 }
 
-void RouteHandler::setMap(const LaneletMapBin & map_msg)
+void RouteHandler::setMap(
+  const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet)
 {
   lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(
@@ -214,7 +215,7 @@
   is_map_msg_ready_ = true;
   is_handler_ready_ = false;
 
-  setLaneletsFromRouteMsg();
+  setLaneletsFromRouteMsg(is_enable_differential_lanelet);
 }
 
 bool RouteHandler::isRouteLooped(const RouteSections & route_sections)
@@ -231,7 +232,8 @@
   return false;
 }
 
-void RouteHandler::setRoute(const LaneletRoute & route_msg)
+void RouteHandler::setRoute(
+  const LaneletRoute & route_msg, const bool & is_enable_differential_lanelet)
 {
   if (!isRouteLooped(route_msg.segments)) {
     // if get not modified route but new route, reset original start pose
@@ -241,7 +243,7 @@
     }
     route_ptr_ = std::make_shared<LaneletRoute>(route_msg);
     is_handler_ready_ = false;
-    setLaneletsFromRouteMsg();
+    setLaneletsFromRouteMsg(is_enable_differential_lanelet);
   } else {
     RCLCPP_ERROR(
       logger_,
@@ -364,48 +366,95 @@
   is_handler_ready_ = false;
 }
 
-void RouteHandler::setLaneletsFromRouteMsg()
+void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differential_lanelet)
 {
   if (!route_ptr_ || !is_map_msg_ready_) {
     return;
   }
   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;
+  if (!is_enable_differential_lanelet) {
+    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) {
     primitive_size += route_section.primitives.size();
   }
   route_lanelets_.reserve(primitive_size);
 
   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) {
+        if (!is_enable_differential_lanelet) {
+          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;
+          return;
+        } else {
+          RCLCPP_DEBUG(
+            logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id,
+            e.what());
+        }
       }
     }
   }
   goal_lanelets_.clear();
   start_lanelets_.clear();
   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) {
+        if (!is_enable_differential_lanelet) {
+          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;
+          return;
+        } else {
+          RCLCPP_DEBUG(
+            logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id,
+            e.what());
+        }
+      }
     }
     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) {
+        if (!is_enable_differential_lanelet) {
+          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;
+          return;
+        } else {
+          RCLCPP_DEBUG(
+            logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id,
+            e.what());
+        }
+      }
     }
   }
   is_handler_ready_ = true;

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -219,6 +219,8 @@
   p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
+  p.enable_differential_map_loading = declare_parameter<bool>("enable_differential_map_loading");
+
   return p;
 }
 
@@ -402,15 +404,20 @@
 
   // update map
   if (map_ptr) {
-    planner_data_->route_handler->setMap(*map_ptr);
+    planner_data_->route_handler->setMap(
+      *map_ptr, planner_data_->parameters.enable_differential_map_loading);
+    if (planner_data_->parameters.enable_differential_map_loading) {
+      planner_manager_->resetCurrentRouteLanelet(planner_data_);
+    }
   }
 
   std::unique_lock<std::mutex> lk_manager(mutex_manager_);  // for planner_manager_
 
   // update route
   const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());
   if (route_ptr) {
-    planner_data_->route_handler->setRoute(*route_ptr);
+    planner_data_->route_handler->setRoute(
+      *route_ptr, planner_data_->parameters.enable_differential_map_loading);
     // uuid is not changed when rerouting with modified goal,
     // in this case do not need to reset modules.
     const bool has_same_route_id =

planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -139,7 +139,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
 
   const bool is_out_of_route = utils::isEgoOutOfRoute(
     data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal,
-    data->route_handler);
+    data->route_handler, data->parameters);
 
   if (!is_any_module_running && is_out_of_route) {
     BehaviorModuleOutput result_output = utils::createGoalAroundPath(data);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp

@@ -73,6 +73,9 @@ struct BehaviorPathPlannerParameters
   double right_over_hang;
   double base_link2front;
   double base_link2rear;
+
+  // enable/disable differential map loading
+  bool enable_differential_map_loading;
 };
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp

@@ -237,7 +237,8 @@ bool isInLaneletWithYawThreshold(
 bool isEgoOutOfRoute(
   const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
   const std::optional<PoseWithUuidStamped> & modified_goal,
-  const std::shared_ptr<RouteHandler> & route_handler);
+  const std::shared_ptr<RouteHandler> & route_handler,
+  const BehaviorPathPlannerParameters & common_param);
 
 bool isEgoWithinOriginalLane(
   const lanelet::ConstLanelets & current_lanes, const Pose & current_pose,

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -493,17 +493,45 @@
 bool isEgoOutOfRoute(
   const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
   const std::optional<PoseWithUuidStamped> & modified_goal,
-  const std::shared_ptr<RouteHandler> & route_handler)
+  const std::shared_ptr<RouteHandler> & route_handler,
+  const BehaviorPathPlannerParameters & common_param)
 {
   const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
                              ? modified_goal->pose
                              : route_handler->getGoalPose();
 
   lanelet::ConstLanelet goal_lane;
   const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose);
   if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front();
-  const auto is_failed_getting_lanelet =
-    shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);
+
+  bool is_failed_getting_lanelet;
+  if (!common_param.enable_differential_map_loading) {
+    is_failed_getting_lanelet =
+      shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);
+  } else {
+    // If dynamic map is enabled, get the goal lanelet from the current lanelet map
+    //
+    // Check the route lanelets and last route element in the current lanelet map
+    // will be the goal lanelet
+
+    lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr();
+    lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map);
+    lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets();
+
+    auto goalLaneletIt =
+      std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) {
+        return currentLanelet.id() == route_lanelets.back().id();
+      });
+
+    if (goalLaneletIt != all_lanelets.rend()) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("behavior_path_planner").get_child("util"),
+        "Found goal lanelet in current lanelet map");
+      goal_lane = *goalLaneletIt;
+      is_failed_getting_lanelet = false;
+    }
+  }
+
   if (is_failed_getting_lanelet) {
     RCLCPP_WARN_STREAM(
       rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet");