feat(behavior_path_planner): update path when object is gone

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -9,6 +9,7 @@
 
       enable_avoidance_over_same_direction: true
       enable_avoidance_over_opposite_direction: true
+      enable_update_path_when_object_is_gone: false
 
       threshold_distance_object_is_on_center: 1.0 # [m]
       threshold_speed_object_is_stopped: 1.0      # [m/s]

planning/behavior_path_planner/behavior_path_planner_avoidance-design.md

@@ -380,6 +380,16 @@ The shift points are modified by a filtering process in order to get the expecte
 - Similar gradient removal: Connect two shift points with a straight line, and remove the shift points in between if their shift amount is in the vicinity of the straight line.
 - Remove momentary returns: For shift points that reduce the avoidance width (for going back to the center line), if there is enough long distance in the longitudinal direction, remove them.
 
+##### Avoidance Cancelling
+
+If `enable_update_path_when_object_is_gone` 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 vehcile 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 vehcile 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.
+
 #### How to keep the consistency of the planning
 
 TODO
@@ -411,6 +421,7 @@ The avoidance specific parameter configuration file can be located at `src/autow
 | avoidance_execution_lateral_threshold      | [m]    | double | The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance. (\*2)                      | 0.499         |
 | enable_avoidance_over_same_direction       | [-]    | bool   | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.                          | true          |
 | enable_avoidance_over_opposite_direction   | [-]    | bool   | Extend avoidance trajectory to adjacent lanes that has opposite direction. `enable_avoidance_over_same_direction` must be `true` to take effects | true          |
+| 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         |
 
 (\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value.
 

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

@@ -9,6 +9,7 @@
 
       enable_avoidance_over_same_direction: true
       enable_avoidance_over_opposite_direction: true
+      enable_update_path_when_object_is_gone: false
 
       threshold_distance_object_is_on_center: 1.0 # [m]
       threshold_speed_object_is_stopped: 1.0      # [m/s]

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

@@ -213,6 +213,8 @@ class AvoidanceModule : public SceneModuleInterface
   void updateRegisteredRawShiftLines();
 
   // -- for state management --
+  bool is_avoidance_maneuver_starts;
+  bool isAvoidanceManeuverRunning();
   bool isAvoidancePlanRunning() const;
 
   // -- for pre-processing --

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

@@ -64,6 +64,9 @@ struct AvoidanceParameters
   // to use this, enable_avoidance_over_same_direction need to be set to true.
   bool enable_avoidance_over_opposite_direction{true};
 
+  // enable update path when if detected objects on planner data is gone.
+  bool enable_update_path_when_object_is_gone{false};
+
   // Vehicles whose distance to the center of the path is
   // less than this will not be considered for avoidance.
   double threshold_distance_object_is_on_center;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -271,6 +271,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.detection_area_left_expand_dist = dp("detection_area_left_expand_dist", 1.0);
   p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true);
   p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true);
+  p.enable_update_path_when_object_is_gone = dp("enable_update_path_when_object_is_gone", false);
 
   p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0);
   p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0);

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

@@ -103,8 +103,14 @@ BT::NodeStatus AvoidanceModule::updateState()
   DebugData debug;
   const auto avoid_data = calcAvoidancePlanningData(debug);
   const bool has_avoidance_target = !avoid_data.target_objects.empty();
+
   if (!is_plan_running && !has_avoidance_target) {
     current_state_ = BT::NodeStatus::SUCCESS;
+  } else 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
+    current_state_ = BT::NodeStatus::SUCCESS;
   } else {
     current_state_ = BT::NodeStatus::RUNNING;
   }
@@ -121,6 +127,18 @@ bool AvoidanceModule::isAvoidancePlanRunning() const
   const bool has_shift_line = (path_shifter_.getShiftLinesSize() > 0);
   return has_base_offset || has_shift_line;
 }
+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;
+}
 
 AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const
 {
@@ -2665,6 +2683,7 @@ void AvoidanceModule::initVariables()
   registered_raw_shift_lines_ = {};
   current_raw_shift_lines_ = {};
   original_unique_id = 0;
+  is_avoidance_maneuver_starts = false;
 }
 
 bool AvoidanceModule::isTargetObjectType(const PredictedObject & object) const