feat(start_planner): allow lane departure check override (autowarefoundation#6512)

* small refactor

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

* another refactor

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

* further refactoring

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

* add param to override lane_departure_check when starting outside lane

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

* update documentation

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

---------

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

Author: danielsanchezaran

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -12,7 +12,9 @@
       center_line_path_interval: 1.0
       # shift pull out
       enable_shift_pull_out: true
-      check_shift_path_lane_departure: false
+      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
       lateral_jerk: 0.5

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -154,6 +154,21 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool requiresDynamicObjectsCollisionDetection() const;
 
+  uint16_t getSteeringFactorDirection(
+    const behavior_path_planner::BehaviorModuleOutput & output) const
+  {
+    switch (output.turn_signal_info.turn_signal.command) {
+      case TurnIndicatorsCommand::ENABLE_LEFT:
+        return SteeringFactor::LEFT;
+
+      case TurnIndicatorsCommand::ENABLE_RIGHT:
+        return SteeringFactor::RIGHT;
+
+      default:
+        return SteeringFactor::STRAIGHT;
+    }
+  };
+
   /**
    * @brief Check if there are no moving objects around within a certain radius.
    *

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp

@@ -52,6 +52,8 @@ 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};
   double lateral_jerk{0.0};

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -53,6 +53,10 @@ 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 =
     node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
   p.lateral_acceleration_sampling_num =

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -97,13 +97,27 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
         shift_path.points.begin() + collision_check_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;
     }

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -176,18 +176,14 @@ void StartPlannerModule::updateData()
     status_.first_engaged_and_driving_forward_time = clock_->now();
   }
 
-  if (hasFinishedBackwardDriving()) {
+  status_.backward_driving_complete = hasFinishedBackwardDriving();
+  if (status_.backward_driving_complete) {
     updateStatusAfterBackwardDriving();
     DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving");
-  } else {
-    status_.backward_driving_complete = false;
   }
 
-  if (requiresDynamicObjectsCollisionDetection()) {
-    status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects();
-  } else {
-    status_.is_safe_dynamic_objects = true;
-  }
+  status_.is_safe_dynamic_objects =
+    (!requiresDynamicObjectsCollisionDetection()) ? true : !hasCollisionWithDynamicObjects();
 }
 
 bool StartPlannerModule::hasFinishedBackwardDriving() const
@@ -326,20 +322,17 @@ bool StartPlannerModule::isStopped()
 
 bool StartPlannerModule::isExecutionReady() const
 {
-  bool is_safe = true;
   // Evaluate safety. The situation is not safe if any of the following conditions are met:
   // 1. pull out path has not been found
   // 2. there is a moving objects around ego
   // 3. waiting for approval and there is a collision with dynamic objects
-  if (!status_.found_pull_out_path) {
-    is_safe = false;
-  } else if (isWaitingApproval()) {
-    if (!noMovingObjectsAround()) {
-      is_safe = false;
-    } else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) {
-      is_safe = false;
-    }
-  }
+
+  const bool is_safe = [&]() -> bool {
+    if (!status_.found_pull_out_path) return false;
+    if (!isWaitingApproval()) return true;
+    if (!noMovingObjectsAround()) return false;
+    return !(requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects());
+  }();
 
   if (!is_safe) {
     stop_pose_ = planner_data_->self_odometry->pose.pose;
@@ -420,14 +413,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
 
   setDrivableAreaInfo(output);
 
-  const uint16_t steering_factor_direction = std::invoke([&output]() {
-    if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
-      return SteeringFactor::LEFT;
-    } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
-      return SteeringFactor::RIGHT;
-    }
-    return SteeringFactor::STRAIGHT;
-  });
+  const auto steering_factor_direction = getSteeringFactorDirection(output);
 
   if (status_.driving_forward) {
     const double start_distance = motion_utils::calcSignedArcLength(
@@ -441,15 +427,16 @@ BehaviorModuleOutput StartPlannerModule::plan()
       {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
       {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
       SteeringFactor::TURNING, "");
-  } else {
-    const double distance = motion_utils::calcSignedArcLength(
-      path.points, planner_data_->self_odometry->pose.pose.position,
-      status_.pull_out_path.start_pose.position);
-    updateRTCStatus(0.0, distance);
-    steering_factor_interface_ptr_->updateSteeringFactor(
-      {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
-      PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
+    setDebugData();
+    return output;
   }
+  const double distance = motion_utils::calcSignedArcLength(
+    path.points, planner_data_->self_odometry->pose.pose.position,
+    status_.pull_out_path.start_pose.position);
+  updateRTCStatus(0.0, distance);
+  steering_factor_interface_ptr_->updateSteeringFactor(
+    {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
+    PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
 
   setDebugData();
 
@@ -530,14 +517,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 
   setDrivableAreaInfo(output);
 
-  const uint16_t steering_factor_direction = std::invoke([&output]() {
-    if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
-      return SteeringFactor::LEFT;
-    } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
-      return SteeringFactor::RIGHT;
-    }
-    return SteeringFactor::STRAIGHT;
-  });
+  const auto steering_factor_direction = getSteeringFactorDirection(output);
 
   if (status_.driving_forward) {
     const double start_distance = motion_utils::calcSignedArcLength(
@@ -551,15 +531,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
       {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
       {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
       SteeringFactor::APPROACHING, "");
-  } else {
-    const double distance = motion_utils::calcSignedArcLength(
-      stop_path.points, planner_data_->self_odometry->pose.pose.position,
-      status_.pull_out_path.start_pose.position);
-    updateRTCStatus(0.0, distance);
-    steering_factor_interface_ptr_->updateSteeringFactor(
-      {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
-      PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
+    setDebugData();
+
+    return output;
   }
+  const double distance = motion_utils::calcSignedArcLength(
+    stop_path.points, planner_data_->self_odometry->pose.pose.position,
+    status_.pull_out_path.start_pose.position);
+  updateRTCStatus(0.0, distance);
+  steering_factor_interface_ptr_->updateSteeringFactor(
+    {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
+    PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
 
   setDebugData();
 
@@ -618,10 +600,11 @@ PriorityOrder StartPlannerModule::determinePriorityOrder(
         order_priority.emplace_back(i, planner);
       }
     }
-  } else {
-    RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str());
-    throw std::domain_error("[start_planner] invalid search_priority");
+    return order_priority;
   }
+
+  RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str());
+  throw std::domain_error("[start_planner] invalid search_priority");
   return order_priority;
 }
 
@@ -990,12 +973,7 @@ bool StartPlannerModule::isStuck()
     return false;
   }
 
-  if (status_.planner_type == PlannerType::STOP) {
-    return true;
-  }
-
-  // not found safe path
-  if (!status_.found_pull_out_path) {
+  if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) {
     return true;
   }
 
@@ -1088,30 +1066,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
     return false;
   });
 
-  if (is_near_intersection) {
-    // offset required end pose with ration to activate turn signal for intersection
-    turn_signal.required_end_point = std::invoke([&]() {
-      const double length_start_to_end =
-        motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
-      const auto ratio = std::clamp(
-        parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
-
-      const double required_end_length = length_start_to_end * ratio;
-      double accumulated_length = 0.0;
-      const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
-      for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
-        accumulated_length +=
-          tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
-        if (accumulated_length > required_end_length) {
-          return path.points.at(i).point.pose;
-        }
+  turn_signal.required_end_point = std::invoke([&]() {
+    if (is_near_intersection) return end_pose;
+    const double length_start_to_end =
+      motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
+    const auto ratio = std::clamp(
+      parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
+
+    const double required_end_length = length_start_to_end * ratio;
+    double accumulated_length = 0.0;
+    const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
+    for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
+      accumulated_length +=
+        tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
+      if (accumulated_length > required_end_length) {
+        return path.points.at(i).point.pose;
       }
-      // not found required end point
-      return end_pose;
-    });
-  } else {
-    turn_signal.required_end_point = end_pose;
-  }
+    }
+    // not found required end point
+    return end_pose;
+  });
 
   return turn_signal;
 }
@@ -1279,23 +1253,27 @@ bool StartPlannerModule::planFreespacePath()
 
 void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 {
-  if (status_.planner_type == PlannerType::FREESPACE) {
-    std::cerr << "Freespace planner updated drivable area." << std::endl;
-    const double drivable_area_margin = planner_data_->parameters.vehicle_width;
-    output.drivable_area_info.drivable_margin =
-      planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
-  } else {
-    const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
-      output.path, generateDrivableLanes(output.path),
-      planner_data_->drivable_area_expansion_parameters);
-
-    DrivableAreaInfo current_drivable_area_info;
-    current_drivable_area_info.drivable_lanes = target_drivable_lanes;
-    output.drivable_area_info =
-      status_.driving_forward
-        ? utils::combineDrivableAreaInfo(
-            current_drivable_area_info, getPreviousModuleOutput().drivable_area_info)
-        : current_drivable_area_info;
+  switch (status_.planner_type) {
+    case PlannerType::FREESPACE: {
+      const double drivable_area_margin = planner_data_->parameters.vehicle_width;
+      output.drivable_area_info.drivable_margin =
+        planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
+      break;
+    }
+    default: {
+      const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
+        output.path, generateDrivableLanes(output.path),
+        planner_data_->drivable_area_expansion_parameters);
+
+      DrivableAreaInfo current_drivable_area_info;
+      current_drivable_area_info.drivable_lanes = target_drivable_lanes;
+      output.drivable_area_info =
+        status_.driving_forward
+          ? utils::combineDrivableAreaInfo(
+              current_drivable_area_info, getPreviousModuleOutput().drivable_area_info)
+          : current_drivable_area_info;
+      break;
+    }
   }
 }