feat(dynamic_avoidance): suppress flickering of dynamic avoidance lau…

…nching

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -47,6 +47,7 @@ struct DynamicAvoidanceParameters
   bool avoid_motorcycle{false};
   bool avoid_pedestrian{false};
   double min_obstacle_vel{0.0};
+  int successive_num_to_entry_dynamic_avoidance_condition{0};
 
   // drivable area generation
   double lat_offset_from_obstacle{0.0};
@@ -87,6 +88,24 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
     bool is_left;
   };
+  struct DynamicAvoidanceObjectCandidate
+  {
+    DynamicAvoidanceObject object;
+    int alive_counter;
+
+    static std::optional<DynamicAvoidanceObjectCandidate> getObjectFromUuid(
+      const std::vector<DynamicAvoidanceObjectCandidate> & objects, const std::string & target_uuid)
+    {
+      const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) {
+        return object.object.uuid == target_uuid;
+      });
+
+      if (itr == objects.end()) {
+        return std::nullopt;
+      }
+      return *itr;
+    }
+  };
 
 #ifdef USE_OLD_ARCHITECTURE
   DynamicAvoidanceModule(
@@ -119,12 +138,14 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
 private:
   bool isLabelTargetObstacle(const uint8_t label) const;
-  std::vector<DynamicAvoidanceObject> calcTargetObjects() const;
+  std::vector<DynamicAvoidanceObjectCandidate> calcTargetObjectsCandidate() const;
   std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
     const double forward_distance, const double backward_distance) const;
   std::optional<tier4_autoware_utils::Polygon2d> calcDynamicObstaclePolygon(
     const DynamicAvoidanceObject & object) const;
 
+  std::vector<DynamicAvoidanceModule::DynamicAvoidanceObjectCandidate>
+    prev_target_objects_candidate_;
   std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> target_objects_;
   // std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> prev_target_objects_;
   std::shared_ptr<DynamicAvoidanceParameters> parameters_;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -743,6 +743,8 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
     p.avoid_motorcycle = declare_parameter<bool>(ns + "motorcycle");
     p.avoid_pedestrian = declare_parameter<bool>(ns + "pedestrian");
     p.min_obstacle_vel = declare_parameter<double>(ns + "min_obstacle_vel");
+    p.successive_num_to_entry_dynamic_avoidance_condition =
+      declare_parameter<int>(ns + "successive_num_to_entry_dynamic_avoidance_condition");
   }
 
   {  // drivable_area_generation

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -192,7 +192,19 @@ bool DynamicAvoidanceModule::isExecutionReady() const
 
 void DynamicAvoidanceModule::updateData()
 {
-  target_objects_ = calcTargetObjects();
+  // calculate target objects candidate
+  const auto target_objects_candidate = calcTargetObjectsCandidate();
+  prev_target_objects_candidate_ = target_objects_candidate;
+
+  // calculate target objects considering flickering suppress
+  target_objects_.clear();
+  for (const auto & target_object_candidate : target_objects_candidate) {
+    if (
+      parameters_->successive_num_to_entry_dynamic_avoidance_condition <=
+      target_object_candidate.alive_counter) {
+      target_objects_.push_back(target_object_candidate.object);
+    }
+  }
 }
 
 ModuleStatus DynamicAvoidanceModule::updateState()
@@ -293,8 +305,8 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const
   return false;
 }
 
-std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject>
-DynamicAvoidanceModule::calcTargetObjects() const
+std::vector<DynamicAvoidanceModule::DynamicAvoidanceObjectCandidate>
+DynamicAvoidanceModule::calcTargetObjectsCandidate() const
 {
   const auto prev_module_path = getPreviousModuleOutput().path;
   const auto & predicted_objects = planner_data_->dynamic_object->objects;
@@ -329,7 +341,7 @@ DynamicAvoidanceModule::calcTargetObjects() const
   // 4. check if object will cut into the ego lane.
   // NOTE: The oncoming object will be ignored.
   constexpr double epsilon_path_lat_diff = 0.3;
-  std::vector<DynamicAvoidanceObject> output_objects;
+  std::vector<DynamicAvoidanceObjectCandidate> output_objects_candidate;
   for (const bool is_left : {true, false}) {
     for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) {
       const auto reliable_predicted_path = std::max_element(
@@ -358,13 +370,24 @@ DynamicAvoidanceModule::calcTargetObjects() const
         continue;
       }
 
+      // get previous object if it exists
+      const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid(
+        prev_target_objects_candidate_, object.uuid);
+      const int alive_counter =
+        prev_target_object_candidate
+          ? std::min(
+              parameters_->successive_num_to_entry_dynamic_avoidance_condition,
+              prev_target_object_candidate->alive_counter + 1)
+          : 0;
+
       auto target_object = object;
       target_object.is_left = is_left;
-      output_objects.push_back(target_object);
+      output_objects_candidate.push_back(
+        DynamicAvoidanceObjectCandidate{target_object, alive_counter});
     }
   }
 
-  return output_objects;
+  return output_objects_candidate;
 }
 
 std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> DynamicAvoidanceModule::getAdjacentLanes(

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

@@ -49,6 +49,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
     updateParam<bool>(parameters, ns + "pedestrian", p->avoid_pedestrian);
 
     updateParam<double>(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel);
+
+    updateParam<int>(
+      parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition",
+      p->successive_num_to_entry_dynamic_avoidance_condition);
   }
 
   {  // drivable_area_generation