add param to override lane_departure_check when starting outside lane

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -13,6 +13,7 @@
       # shift pull out
       enable_shift_pull_out: true
       check_shift_path_lane_departure: true
+      allow_check_shift_path_lane_departure_override: false
       shift_collision_check_distance_from_end: -10.0
       minimum_shift_pull_out_distance: 0.0
       deceleration_interval: 15.0

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

@@ -72,6 +72,7 @@ struct StartPlannerParameters
   // shift pull out
   bool enable_shift_pull_out{false};
   bool check_shift_path_lane_departure{false};
+  bool allow_check_shift_path_lane_departure_override{false};
   double shift_collision_check_distance_from_end{0.0};
   double minimum_shift_pull_out_distance{0.0};
   int lateral_acceleration_sampling_num{0};

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -54,6 +54,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
   p.check_shift_path_lane_departure =
     node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
+  p.allow_check_shift_path_lane_departure_override =
+    node->declare_parameter<bool>(ns + "allow_check_shift_path_lane_departure_override");
   p.shift_collision_check_distance_from_end =
     node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
   p.minimum_shift_pull_out_distance =
@@ -390,6 +392,9 @@ void StartPlannerModuleManager::updateModuleParams(
       p->geometric_collision_check_distance_from_end);
     updateParam<bool>(
       parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure);
+    updateParam<bool>(
+      parameters, ns + "allow_check_shift_path_lane_departure_override",
+      p->allow_check_shift_path_lane_departure_override);
     updateParam<std::string>(parameters, ns + "search_priority", p->search_priority);
     updateParam<double>(parameters, ns + "max_back_distance", p->max_back_distance);
     updateParam<double>(

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -74,13 +74,27 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
         shift_path.points.begin() + pull_out_end_idx + 1);
     }
 
-    // check lane departure
-    // The method for lane departure checking verifies if the footprint of each point on the path is
-    // contained within a lanelet using `boost::geometry::within`, which incurs a high computational
-    // cost.
     const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
+
+    // if lane departure check override is true, and if the initial pose is not fully within a lane,
+    // cancel lane departure check
+    const bool is_lane_departure_check_required = std::invoke([&]() -> bool {
+      if (!parameters_.allow_check_shift_path_lane_departure_override)
+        return parameters_.check_shift_path_lane_departure;
+
+      PathWithLaneId path_with_only_first_pose{};
+      path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.at(0));
+      return !lane_departure_checker_->checkPathWillLeaveLane(
+        lanelet_map_ptr, path_with_only_first_pose);
+    });
+
+    // check lane departure
+    // The method for lane departure checking verifies if the footprint of each point on the path
+    // is contained within a lanelet using `boost::geometry::within`, which incurs a high
+    // computational cost.
+
     if (
-      parameters_.check_shift_path_lane_departure &&
+      is_lane_departure_check_required &&
       lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) {
       continue;
     }