chore: remove conflicts

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

Author: StepTurtle

planning/autoware_behavior_path_planner_common/src/utils/utils.cpp

@@ -500,8 +500,36 @@ bool isEgoOutOfRoute(
   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);
+
+    const bool is_failed_getting_lanelet = std::invoke([&]() {
+        if (utils::isInLanelets(goal_pose, shoulder_goal_lanes)) {
+            return !lanelet::utils::query::getClosestLanelet(shoulder_goal_lanes, goal_pose, &goal_lane);
+        }
+        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);
+    });
   if (is_failed_getting_lanelet) {
     RCLCPP_WARN_STREAM(
       rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet");

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -368,9 +368,6 @@ void BehaviorPathPlannerNode::run()
   if (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_->resetRootLanelet(planner_data_);
-    }
   }
 
   std::unique_lock<std::mutex> lk_manager(mutex_manager_);  // for planner_manager_
@@ -380,7 +377,6 @@ void BehaviorPathPlannerNode::run()
   if (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,
     // in this case do not need to reset modules.
     const bool has_same_route_id =

planning/route_handler/include/route_handler/route_handler.hpp

@@ -58,7 +58,7 @@ class RouteHandler
   explicit RouteHandler(const LaneletMapBin & map_msg);
 
   // non-const methods
-  void setMap(const HADMapBin & map_msg, const bool & is_enable_differential_lanelet = false);
+  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);

planning/route_handler/src/route_handler.cpp

@@ -139,7 +139,7 @@ RouteHandler::RouteHandler(const LaneletMapBin & map_msg)
   route_ptr_ = nullptr;
 }
 
-void RouteHandler::setMap(const HADMapBin & map_msg, const bool & is_enable_differential_lanelet)
+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(