feat(dynamic_avoidance): apply LPF to shift length, and positive relative velocity

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

@@ -67,9 +67,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 public:
   struct DynamicAvoidanceObject
   {
-    explicit DynamicAvoidanceObject(
+    DynamicAvoidanceObject(
       const PredictedObject & predicted_object, const double arg_path_projected_vel)
-    : pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
+    : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
+      pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
       path_projected_vel(arg_path_projected_vel),
       shape(predicted_object.shape)
     {
@@ -78,9 +79,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface
       }
     }
 
-    const geometry_msgs::msg::Pose pose;
-    const double path_projected_vel;
-    const autoware_auto_perception_msgs::msg::Shape shape;
+    std::string uuid;
+    geometry_msgs::msg::Pose pose;
+    double path_projected_vel;
+    autoware_auto_perception_msgs::msg::Shape shape;
     std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};
 
     bool is_left;
@@ -124,7 +126,45 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     const DynamicAvoidanceObject & object) const;
 
   std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> target_objects_;
+  // std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> prev_target_objects_;
   std::shared_ptr<DynamicAvoidanceParameters> parameters_;
+
+  struct ObjectsVariable
+  {
+    void resetCurrentUuids() { current_uuids_.clear(); }
+    void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); }
+    void removeCounterUnlessUpdated()
+    {
+      std::vector<std::string> obsolete_uuids;
+      for (const auto & key_and_value : variable_) {
+        if (
+          std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) ==
+          current_uuids_.end()) {
+          obsolete_uuids.push_back(key_and_value.first);
+        }
+      }
+
+      for (const auto & obsolete_uuid : obsolete_uuids) {
+        variable_.erase(obsolete_uuid);
+      }
+    }
+
+    std::optional<double> get(const std::string & uuid) const
+    {
+      if (variable_.count(uuid) != 0) {
+        return variable_.at(uuid);
+      }
+      return std::nullopt;
+    }
+    void update(const std::string & uuid, const double new_variable)
+    {
+      variable_.emplace(uuid, new_variable);
+    }
+
+    std::unordered_map<std::string, double> variable_;
+    std::vector<std::string> current_uuids_;
+  };
+  mutable ObjectsVariable prev_objects_min_bound_lat_offset_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/package.xml

@@ -62,6 +62,7 @@
   <depend>route_handler</depend>
   <depend>rtc_interface</depend>
   <depend>sensor_msgs</depend>
+  <depend>signal_processing</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>

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

@@ -16,6 +16,7 @@
 
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
+#include "signal_processing/lowpass_filter_1d.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -128,6 +129,19 @@ void appendExtractedPolygonMarker(
 
   marker_array.markers.push_back(marker);
 }
+
+template <typename T>
+std::optional<T> getObjectFromUuid(const std::vector<T> & objects, const std::string & target_uuid)
+{
+  const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) {
+    return object.uuid == target_uuid;
+  });
+
+  if (itr == objects.end()) {
+    return std::nullopt;
+  }
+  return *itr;
+}
 }  // namespace
 
 #ifdef USE_OLD_ARCHITECTURE
@@ -211,15 +225,19 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
 
   // 3. create obstacles to avoid (= extract from the drivable area)
   std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
+  prev_objects_min_bound_lat_offset_.resetCurrentUuids();
   for (const auto & object : target_objects_) {
     const auto obstacle_poly = calcDynamicObstaclePolygon(object);
     if (obstacle_poly) {
       obstacles_for_drivable_area.push_back({object.pose, obstacle_poly.value(), object.is_left});
 
       appendObjectMarker(info_marker_, object.pose);
       appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value());
+
+      prev_objects_min_bound_lat_offset_.addCurrentUuid(object.uuid);
     }
   }
+  prev_objects_min_bound_lat_offset_.removeCounterUnlessUpdated();
 
   BehaviorModuleOutput output;
   output.path = prev_module_path;
@@ -389,6 +407,7 @@ std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> DynamicAvoidanceModule
   return std::make_pair(right_lanes, left_lanes);
 }
 
+// NOTE: object does not have const only to update min_bound_lat_offset.
 std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynamicObstaclePolygon(
   const DynamicAvoidanceObject & object) const
 {
@@ -455,10 +474,8 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
         motion_utils::findNearestSegmentIndex(prev_module_path->points, object.pose.position);
       const double signed_lon_length = motion_utils::calcSignedArcLength(
         prev_module_path->points, getEgoPosition(), ego_seg_idx, object.pose.position, obj_seg_idx);
-      if (relative_velocity == 0.0) {
-        return std::numeric_limits<double>::max();
-      }
-      return signed_lon_length / relative_velocity;
+      const double positive_relative_velocity = std::max(relative_velocity, 1.0);
+      return signed_lon_length / positive_relative_velocity;
     }();
 
     if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) {
@@ -535,13 +552,21 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
   const double max_bound_lat_offset =
     max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0);
 
+  // filter min_bound_lat_offset
+  const auto prev_min_bound_lat_offset = prev_objects_min_bound_lat_offset_.get(object.uuid);
+  const double filtered_min_bound_lat_offset =
+    prev_min_bound_lat_offset
+      ? signal_processing::lowpassFilter(min_bound_lat_offset, *prev_min_bound_lat_offset, 0.3)
+      : min_bound_lat_offset;
+  prev_objects_min_bound_lat_offset_.update(object.uuid, filtered_min_bound_lat_offset);
+
   // create inner/outer bound points
   std::vector<geometry_msgs::msg::Point> obj_inner_bound_points;
   std::vector<geometry_msgs::msg::Point> obj_outer_bound_points;
   for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) {
     obj_inner_bound_points.push_back(
       tier4_autoware_utils::calcOffsetPose(
-        path_with_backward_margin.points.at(i).point.pose, 0.0, min_bound_lat_offset, 0.0)
+        path_with_backward_margin.points.at(i).point.pose, 0.0, filtered_min_bound_lat_offset, 0.0)
         .position);
     obj_outer_bound_points.push_back(
       tier4_autoware_utils::calcOffsetPose(