feat(dynamic_avoidance): add a guard of LPF for reference path change…

…d by avoidance/LC (autowarefoundation#6112)

Signed-off-by: Takayuki Murooka <[email protected]>

planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp

@@ -382,8 +382,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     const TimeWhileCollision & time_while_collision) const;
   std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoid(
     const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
-    const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left,
-    const double obj_normal_vel, const std::optional<DynamicAvoidanceObject> & prev_object) const;
+    const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel,
+    const bool is_collision_left, const double obj_normal_vel,
+    const std::optional<DynamicAvoidanceObject> & prev_object) const;
 
   std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
     const double forward_distance, const double backward_distance) const;

planning/behavior_path_dynamic_avoidance_module/src/scene.cpp

@@ -676,8 +676,8 @@ void DynamicAvoidanceModule::updateTargetObjects()
       ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape,
       time_while_collision);
     const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid(
-      ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel,
-      prev_object);
+      ref_path_points_for_obj_poly, obj_points, object.pose.position, object.vel, is_collision_left,
+      object.lat_vel, prev_object);
     if (!lat_offset_to_avoid) {
       RCLCPP_INFO_EXPRESSION(
         getLogger(), parameters_->enable_debug_info,
@@ -808,9 +808,9 @@ TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision(
   const double relative_velocity = getEgoSpeed() - obj_tangent_vel;
 
   const double signed_time_to_start_collision = [&]() {
-    const double lon_offset_ego_front_to_obj_back = lon_offset_ego_to_obj_idx +
-                                                    lat_lon_offset.min_lon_offset -
-                                                    planner_data_->parameters.front_overhang;
+    const double lon_offset_ego_front_to_obj_back =
+      lon_offset_ego_to_obj_idx + lat_lon_offset.min_lon_offset -
+      (planner_data_->parameters.wheel_base + planner_data_->parameters.front_overhang);
     const double lon_offset_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx -
                                                     lat_lon_offset.max_lon_offset -
                                                     planner_data_->parameters.rear_overhang;
@@ -1070,7 +1070,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid(
              std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object;
     }
     // The ego and object are the opposite directional.
-    const double obj_acc = -1.0;
+    const double obj_acc = -0.5;
     const double decel_time = 1.0;
     const double obj_moving_dist =
       (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 /
@@ -1120,9 +1120,10 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid(
   const size_t obj_seg_idx =
     motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position);
 
-  const double dist_threshold_paths = planner_data_->parameters.vehicle_width / 2.0 +
-                                      parameters_->lat_offset_from_obstacle +
-                                      calcObstacleMaxLength(obj_shape);
+  constexpr double dist_threshold_additional_margin = 0.5;
+  const double dist_threshold_paths =
+    planner_data_->parameters.vehicle_width / 2.0 + parameters_->lat_offset_from_obstacle +
+    calcObstacleWidth(obj_shape) / 2.0 + dist_threshold_additional_margin;
 
   // crop the ego's path by object position
   std::vector<PathPointWithLaneId> cropped_ego_path_points;
@@ -1204,28 +1205,31 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid(
 
 std::optional<MinMaxValue> DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(
   const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
-  const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left,
-  const double obj_normal_vel, const std::optional<DynamicAvoidanceObject> & prev_object) const
+  const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel,
+  const bool is_collision_left, const double obj_normal_vel,
+  const std::optional<DynamicAvoidanceObject> & prev_object) const
 {
-  const bool enable_lowpass_filter = true;
-  /*
   const bool enable_lowpass_filter = [&]() {
-    if (!prev_ref_path_points_for_obj_poly_ || prev_ref_path_points_for_obj_poly_->size() < 2) {
+    if (
+      !prev_object || prev_object->ref_path_points_for_obj_poly.size() < 2 ||
+      ref_path_points_for_obj_poly.size() < 2) {
       return true;
     }
-    const size_t prev_front_seg_idx = motion_utils::findNearestSegmentIndex(
-      *prev_ref_path_points_for_obj_poly_,
-  ref_path_points_for_obj_poly.front().point.pose.position); constexpr double
-  min_lane_change_path_lat_offset = 1.0; if ( motion_utils::calcLateralOffset(
-        *prev_ref_path_points_for_obj_poly_,
-  ref_path_points_for_obj_poly.front().point.pose.position, prev_front_seg_idx) <
-  min_lane_change_path_lat_offset) { return true;
+    const size_t obj_point_idx =
+      motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos);
+    const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset(
+      prev_object->ref_path_points_for_obj_poly,
+      ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position));
+
+    std::cerr << paths_lat_diff << std::endl;
+    constexpr double min_paths_lat_diff = 0.3;
+    if (paths_lat_diff < min_paths_lat_diff) {
+      return true;
     }
     // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to
     // shift the obstacle polygon suddenly.
     return false;
   }();
-  */
 
   // calculate min/max lateral offset from object to path
   const auto obj_lat_abs_offset = [&]() {