update planner, add map parameter

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

Author: StepTurtle

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" default="true"/>
 
   <group>
     <push-ros-namespace namespace="planning"/>
@@ -25,6 +27,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)"/>
       </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"/>
 
   <!-- lane_driving scenario -->
   <group>
@@ -11,6 +12,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)"/>
           <!-- 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"/>
         </include>

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

@@ -1,10 +1,13 @@
 <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="launch_avoidance_module" default="true"/>
   <arg name="launch_avoidance_by_lane_change_module" default="true"/>
@@ -203,6 +206,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 from="$(var behavior_path_planner_side_shift_module_param_path)"/>
       <param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
       <param from="$(var behavior_path_planner_avoidance_by_lc_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"/>
   <!-- scenario selector -->
   <group>
     <include file="$(find-pkg-share scenario_selector)/launch/scenario_selector.launch.xml">
@@ -53,6 +54,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)"/>
       </include>
     </group>
     <!-- parking -->

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -263,6 +263,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   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;
 }
 
@@ -352,7 +354,8 @@ void BehaviorPathPlannerNode::run()
 
   // 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);
     planner_manager_->resetRootLanelet(planner_data_);
   }
 
@@ -361,7 +364,8 @@ void BehaviorPathPlannerNode::run()
   // 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);
     planner_manager_->resetRootLanelet(planner_data_);
 
     // uuid is not changed when rerouting with modified goal,

planning/behavior_path_planner/src/planner_manager.cpp

@@ -116,7 +116,8 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
       is_any_approved_module_running || is_any_candidate_module_running_or_idle;
 
     const bool is_out_of_route = utils::isEgoOutOfRoute(
-      data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler);
+      data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler,
+      data->parameters);
 
     if (!is_any_module_running && is_out_of_route) {
       BehaviorModuleOutput output = utils::createGoalAroundPath(data);

planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp

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

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp

@@ -232,7 +232,8 @@ bool isInLaneletWithYawThreshold(
 
 bool isEgoOutOfRoute(
   const Pose & self_pose, 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_common/src/utils/utils.cpp

@@ -491,7 +491,8 @@ bool isInLaneletWithYawThreshold(
 
 bool isEgoOutOfRoute(
   const Pose & self_pose, 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
@@ -503,17 +504,27 @@ 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;
-        }
+    if (common_param.enable_differential_map_loading) {
+      // 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;
+        return false;
       }
     }
     return !route_handler->getGoalLanelet(&goal_lane);

planning/route_handler/include/route_handler/route_handler.hpp

@@ -57,8 +57,8 @@ class RouteHandler
   explicit RouteHandler(const HADMapBin & map_msg);
 
   // non-const methods
-  void setMap(const HADMapBin & map_msg);
-  void setRoute(const LaneletRoute & route_msg);
+  void setMap(const HADMapBin & map_msg, const bool & is_enable_differantial_lanelet = false);
+  void setRoute(const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet = false);
   void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets);
   void clearRoute();
 
@@ -380,7 +380,7 @@ class RouteHandler
   Pose original_goal_pose_;
 
   // non-const methods
-  void setLaneletsFromRouteMsg();
+  void setLaneletsFromRouteMsg(const bool & is_enable_differantial_lanelet = false);
 
   // const methods
   // for routing

planning/route_handler/src/route_handler.cpp

@@ -139,7 +139,7 @@ RouteHandler::RouteHandler(const HADMapBin & map_msg)
   route_ptr_ = nullptr;
 }
 
-void RouteHandler::setMap(const HADMapBin & map_msg)
+void RouteHandler::setMap(const HADMapBin & map_msg, const bool & is_enable_differantial_lanelet)
 {
   lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(
@@ -163,7 +163,7 @@ void RouteHandler::setMap(const HADMapBin & map_msg)
   is_map_msg_ready_ = true;
   is_handler_ready_ = false;
 
-  setLaneletsFromRouteMsg();
+  setLaneletsFromRouteMsg(is_enable_differantial_lanelet);
 }
 
 bool RouteHandler::isRouteLooped(const RouteSections & route_sections)
@@ -180,7 +180,8 @@ bool RouteHandler::isRouteLooped(const RouteSections & route_sections)
   return false;
 }
 
-void RouteHandler::setRoute(const LaneletRoute & route_msg)
+void RouteHandler::setRoute(
+  const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet)
 {
   if (!isRouteLooped(route_msg.segments)) {
     // if get not modified route but new route, reset original start pose
@@ -190,7 +191,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg)
     }
     route_ptr_ = std::make_shared<LaneletRoute>(route_msg);
     is_handler_ready_ = false;
-    setLaneletsFromRouteMsg();
+    setLaneletsFromRouteMsg(is_enable_differantial_lanelet);
   } else {
     RCLCPP_ERROR(
       logger_,
@@ -313,17 +314,19 @@ void RouteHandler::clearRoute()
   is_handler_ready_ = false;
 }
 
-void RouteHandler::setLaneletsFromRouteMsg()
+void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differantial_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_differantial_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) {
@@ -341,11 +344,18 @@ void RouteHandler::setLaneletsFromRouteMsg()
           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;
+        if (!is_enable_differantial_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());
+        }
       }
     }
   }
@@ -359,11 +369,18 @@ void RouteHandler::setLaneletsFromRouteMsg()
         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;
+        if (!is_enable_differantial_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());
@@ -373,11 +390,18 @@ void RouteHandler::setLaneletsFromRouteMsg()
         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;
+        if (!is_enable_differantial_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());
+        }
       }
     }
   }