feat(avoidance): enable avoidance cancel (autowarefoundation#4398)

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/docs/behavior_path_planner_avoidance_design.md

@@ -601,13 +601,13 @@ $$
 
 ### Avoidance cancelling maneuver
 
-If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows:
+If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows:
 
 - If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled.
 - If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled.
 - If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled.
 
-If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone.
+If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone.
 
 ## How to keep the consistency of the optimize-base path generation logic
 
@@ -621,15 +621,15 @@ The avoidance specific parameter configuration file can be located at `src/autow
 
 namespace: `avoidance.`
 
-| Name                                   | Unit | Type   | Description                                                                                                                         | Default value |
-| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| resample_interval_for_planning         | [m]  | double | Path resample interval for avoidance planning path.                                                                                 | 0.3           |
-| resample_interval_for_output           | [m]  | double | Path resample interval for output path. Too short interval increases computational cost for latter modules.                         | 4.0           |
-| detection_area_right_expand_dist       | [m]  | double | Lanelet expand length for right side to find avoidance target vehicles.                                                             | 0.0           |
-| detection_area_left_expand_dist        | [m]  | double | Lanelet expand length for left side to find avoidance target vehicles.                                                              | 1.0           |
-| enable_update_path_when_object_is_gone | [-]  | bool   | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false         |
-| enable_yield_maneuver                  | [-]  | bool   | Flag to enable yield maneuver.                                                                                                      | false         |
-| enable_yield_maneuver_during_shifting  | [-]  | bool   | Flag to enable yield maneuver during shifting.                                                                                      | false         |
+| Name                                  | Unit | Type   | Description                                                                                                                         | Default value |
+| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| resample_interval_for_planning        | [m]  | double | Path resample interval for avoidance planning path.                                                                                 | 0.3           |
+| resample_interval_for_output          | [m]  | double | Path resample interval for output path. Too short interval increases computational cost for latter modules.                         | 4.0           |
+| detection_area_right_expand_dist      | [m]  | double | Lanelet expand length for right side to find avoidance target vehicles.                                                             | 0.0           |
+| detection_area_left_expand_dist       | [m]  | double | Lanelet expand length for left side to find avoidance target vehicles.                                                              | 1.0           |
+| enable_cancel_maneuver                | [-]  | bool   | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false         |
+| enable_yield_maneuver                 | [-]  | bool   | Flag to enable yield maneuver.                                                                                                      | false         |
+| enable_yield_maneuver_during_shifting | [-]  | bool   | Flag to enable yield maneuver during shifting.                                                                                      | false         |
 
 | Name                      | Unit | Type | Description                                                                                                             | Default value |
 | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- |

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

@@ -195,19 +195,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

@@ -92,8 +92,8 @@ struct AvoidanceParameters
   // to use this, enable_avoidance_over_same_direction need to be set to true.
   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};
+  // if this param is true, it reverts avoidance path when the path is no longer needed.
+  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

@@ -135,64 +135,53 @@ bool AvoidanceModule::isExecutionReady() const
 bool AvoidanceModule::canTransitSuccessState()
 {
   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 true;
   }
 
-  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;
-  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 true;
-  }
+  helper_.setPreviousDrivingLanes(data.current_lanelets);
 
-  DEBUG_PRINT(
-    "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target);
+  // 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;
+    }
 
-  if (!is_plan_running && !has_avoidance_target) {
-    return 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 true;
+    }
   }
 
-  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 true;
+  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;
+
+  // 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 true;
+    }
   }
 
-  helper_.setPreviousDrivingLanes(data.current_lanelets);
-
-  return false;
-}
-
-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;
+  // 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 true;
     }
   }
-  return false;
+
+  return false;  // Keep current state.
 }
 
 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");
   }