feat(avoidance): enable avoidance cancel

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -11,10 +11,10 @@
 
       # avoidance module common setting
       enable_bound_clipping: false
-      enable_update_path_when_object_is_gone: false
       enable_force_avoidance_for_stopped_vehicle: false
       enable_yield_maneuver: true
       enable_yield_maneuver_during_shifting: false
+      enable_cancel_maneuver: false
       disable_path_update: false
 
       # drivable area setting

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -185,19 +185,6 @@ class AvoidanceModule : public SceneModuleInterface
    */
   AvoidanceState updateEgoState(const AvoidancePlanningData & data) const;
 
-  /**
-   * @brief check whether the ego is shifted based on shift line.
-   * @return result.
-   */
-  bool isAvoidanceManeuverRunning();
-
-  /**
-   * @brief check whether the ego is in avoidance maneuver based on shift line and target object
-   * existence.
-   * @return result.
-   */
-  bool isAvoidancePlanRunning() const;
-
   // ego behavior update
 
   /**

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -93,7 +93,7 @@ struct AvoidanceParameters
   bool use_opposite_lane{true};
 
   // enable update path when if detected objects on planner data is gone.
-  bool enable_update_path_when_object_is_gone{false};
+  bool enable_cancel_maneuver{false};
 
   // enable avoidance for all parking vehicle
   bool enable_force_avoidance_for_stopped_vehicle{false};

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -139,67 +139,59 @@ bool AvoidanceModule::isExecutionReady() const
 ModuleStatus AvoidanceModule::updateState()
 {
   const auto & data = avoidance_data_;
-  const auto is_plan_running = isAvoidancePlanRunning();
-  const bool has_avoidance_target = !data.target_objects.empty();
 
+  // Change input lane. -> EXIT.
   if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) {
-    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated.");
+    RCLCPP_WARN(getLogger(), "Previous module lane is updated. Exit.");
     return ModuleStatus::SUCCESS;
   }
 
-  const auto idx = planner_data_->findEgoIndex(data.reference_path.points);
-  if (idx == data.reference_path.points.size() - 1) {
-    arrived_path_end_ = true;
-  }
+  helper_.setPreviousDrivingLanes(data.current_lanelets);
 
-  constexpr double THRESHOLD = 1.0;
-  if (
-    calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD &&
-    arrived_path_end_) {
-    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "reach path end point. exit avoidance module.");
-    return ModuleStatus::SUCCESS;
+  // Reach input path end point. -> EXIT.
+  {
+    const auto idx = planner_data_->findEgoIndex(data.reference_path.points);
+    if (idx == data.reference_path.points.size() - 1) {
+      arrived_path_end_ = true;
+    }
+
+    constexpr double THRESHOLD = 1.0;
+    const auto is_further_than_threshold =
+      calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD;
+    if (is_further_than_threshold && arrived_path_end_) {
+      RCLCPP_WARN(getLogger(), "Reach path end point. Exit.");
+      return ModuleStatus::SUCCESS;
+    }
   }
 
-  DEBUG_PRINT(
-    "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target);
+  const bool has_avoidance_target = !data.target_objects.empty();
+  const bool has_shift_point = !path_shifter_.getShiftLines().empty();
+  const bool has_base_offset =
+    std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold;
 
-  if (!is_plan_running && !has_avoidance_target) {
-    return ModuleStatus::SUCCESS;
+  // Nothing to do. -> EXIT.
+  if (!has_avoidance_target) {
+    if (!has_shift_point && !has_base_offset) {
+      RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit.");
+      return ModuleStatus::SUCCESS;
+    }
   }
 
-  if (
-    !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone &&
-    !isAvoidanceManeuverRunning()) {
-    // if dynamic objects are removed on path, change current state to reset path
-    return ModuleStatus::SUCCESS;
+  // Be able to canceling avoidance path. -> EXIT.
+  if (!has_avoidance_target) {
+    if (!helper_.isShifted() && parameters_->enable_cancel_maneuver) {
+      RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit.");
+      return ModuleStatus::SUCCESS;
+    }
   }
 
-  helper_.setPreviousDrivingLanes(data.current_lanelets);
-
-  if (is_plan_running || current_state_ == ModuleStatus::RUNNING) {
+  // Exist approved shift line. -> KEEP RUNNING state.
+  if (has_shift_point || has_base_offset || current_state_ == ModuleStatus::RUNNING) {
     return ModuleStatus::RUNNING;
   }
-  return ModuleStatus::IDLE;
-}
-
-bool AvoidanceModule::isAvoidancePlanRunning() const
-{
-  constexpr double AVOIDING_SHIFT_THR = 0.1;
-  const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > AVOIDING_SHIFT_THR;
-  const bool has_shift_point = (path_shifter_.getShiftLinesSize() > 0);
-  return has_base_offset || has_shift_point;
-}
-bool AvoidanceModule::isAvoidanceManeuverRunning()
-{
-  const auto path_idx = avoidance_data_.ego_closest_path_index;
 
-  for (const auto & al : registered_raw_shift_lines_) {
-    if (path_idx > al.start_idx || is_avoidance_maneuver_starts) {
-      is_avoidance_maneuver_starts = true;
-      return true;
-    }
-  }
-  return false;
+  // Don't exist approved shift line but there are avoidance targets. -> KEEP IDLE state.
+  return ModuleStatus::IDLE;
 }
 
 AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -55,8 +55,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.detection_area_left_expand_dist =
       get_parameter<double>(node, ns + "detection_area_left_expand_dist");
     p.enable_bound_clipping = get_parameter<bool>(node, ns + "enable_bound_clipping");
-    p.enable_update_path_when_object_is_gone =
-      get_parameter<bool>(node, ns + "enable_update_path_when_object_is_gone");
+    p.enable_cancel_maneuver = get_parameter<bool>(node, ns + "enable_cancel_maneuver");
     p.enable_force_avoidance_for_stopped_vehicle =
       get_parameter<bool>(node, ns + "enable_force_avoidance_for_stopped_vehicle");
     p.enable_yield_maneuver = get_parameter<bool>(node, ns + "enable_yield_maneuver");

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -197,8 +197,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
       get_parameter<double>(node, ns + "detection_area_right_expand_dist");
     p.detection_area_left_expand_dist =
       get_parameter<double>(node, ns + "detection_area_left_expand_dist");
-    p.enable_update_path_when_object_is_gone =
-      get_parameter<bool>(node, ns + "enable_update_path_when_object_is_gone");
     p.enable_force_avoidance_for_stopped_vehicle =
       get_parameter<bool>(node, ns + "enable_force_avoidance_for_stopped_vehicle");
   }