feat(dynamic_avoidance): change the logic of longitudinal distance to…

planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml

@@ -46,6 +46,9 @@
           min_object_vel: -0.5               # [m/s] The value is negative considering the noisy velocity of the stopped vehicle.
           max_ego_path_lat_cover_ratio: 0.3  # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value.
 
+        stopped_object:
+          max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value.
+
       drivable_area_generation:
         polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base"
         object_path_base:

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

@@ -87,6 +87,7 @@ struct DynamicAvoidanceParameters
   double max_overtaking_crossing_object_angle{0.0};
   double min_oncoming_crossing_object_vel{0.0};
   double max_oncoming_crossing_object_angle{0.0};
+  double max_stopped_object_vel{0.0};
 
   // drivable area generation
   PolygonGenerationMethod polygon_generation_method{};
@@ -106,6 +107,12 @@ struct DynamicAvoidanceParameters
   double end_duration_to_avoid_oncoming_object{0.0};
 };
 
+struct TimeWhileCollision
+{
+  double time_to_start_collision;
+  double time_to_end_collision;
+};
+
 class DynamicAvoidanceModule : public SceneModuleInterface
 {
 public:
@@ -324,22 +331,23 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const;
   bool isObjectFarFromPath(
     const PredictedObject & predicted_object, const double obj_dist_to_path) const;
-  double calcTimeToCollision(
-    const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
-    const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const;
+  TimeWhileCollision calcTimeWhileCollision(
+    const std::vector<PathPointWithLaneId> & ego_path, const double obj_tangent_vel,
+    const LatLonOffset & lat_lon_offset) const;
   std::optional<std::pair<size_t, size_t>> calcCollisionSection(
     const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & obj_path) const;
   LatLonOffset getLateralLongitudinalOffset(
     const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
     const autoware_auto_perception_msgs::msg::Shape & obj_shape) const;
   double calcValidLengthToAvoid(
     const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
-    const autoware_auto_perception_msgs::msg::Shape & obj_shape) const;
+    const autoware_auto_perception_msgs::msg::Shape & obj_shape,
+    const bool is_object_same_direction) const;
   MinMaxValue calcMinMaxLongitudinalOffsetToAvoid(
     const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
     const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
     const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape,
-    const double time_to_collision) const;
+    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,

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

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -570,25 +570,30 @@
     // }
 
     // 2.e. check time to collision
-    const double time_to_collision =
-      calcTimeToCollision(input_path.points, object.pose, object.vel, lat_lon_offset);
-    if (
-      (0 <= object.vel &&
-       parameters_->max_time_to_collision_overtaking_object < time_to_collision) ||
-      (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) {
-      RCLCPP_INFO_EXPRESSION(
-        getLogger(), parameters_->enable_debug_info,
-        "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.",
-        obj_uuid.c_str(), time_to_collision);
-      continue;
-    }
-    if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) {
-      RCLCPP_INFO_EXPRESSION(
-        getLogger(), parameters_->enable_debug_info,
-        "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative "
-        "value.",
-        obj_uuid.c_str(), time_to_collision);
-      continue;
+    const auto time_while_collision =
+      calcTimeWhileCollision(input_path.points, object.vel, lat_lon_offset);
+    const double time_to_collision = time_while_collision.time_to_start_collision;
+    if (parameters_->max_stopped_object_vel < std::hypot(object.vel, object.lat_vel)) {
+      // NOTE: Only not stopped object is filtered by time to collision.
+      if (
+        (0 <= object.vel &&
+         parameters_->max_time_to_collision_overtaking_object < time_to_collision) ||
+        (object.vel <= 0 &&
+         parameters_->max_time_to_collision_oncoming_object < time_to_collision)) {
+        RCLCPP_INFO_EXPRESSION(
+          getLogger(), parameters_->enable_debug_info,
+          "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.",
+          obj_uuid.c_str(), time_to_collision);
+        continue;
+      }
+      if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) {
+        RCLCPP_INFO_EXPRESSION(
+          getLogger(), parameters_->enable_debug_info,
+          "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small "
+          "negative value.",
+          obj_uuid.c_str(), time_to_collision);
+        continue;
+      }
     }
 
     // 2.f. calculate which side object will be against ego's path
@@ -625,7 +630,7 @@
     const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
     const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid(
       ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape,
-      time_to_collision);
+      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);
@@ -711,26 +716,60 @@
   return std::make_pair(*collision_start_idx, ego_path.size() - 1);
 }
 
-double DynamicAvoidanceModule::calcTimeToCollision(
-  const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
-  const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const
+TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision(
+  const std::vector<PathPointWithLaneId> & ego_path, const double obj_tangent_vel,
+  const LatLonOffset & lat_lon_offset) const
 {
   // Set maximum time-to-collision 0 if the object longitudinally overlaps ego.
   // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative.
   const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path);
-  const double lon_offset_ego_to_obj =
-    motion_utils::calcSignedArcLength(
-      ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) +
-    lat_lon_offset.max_lon_offset;
-  const double maximum_time_to_collision =
-    (0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits<double>::max();
-
+  const double lon_offset_ego_to_obj_idx = motion_utils::calcSignedArcLength(
+    ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx);
   const double relative_velocity = getEgoSpeed() - obj_tangent_vel;
-  const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position);
-  const double signed_lon_length = motion_utils::calcSignedArcLength(
-    ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx);
-  const double positive_relative_velocity = std::max(relative_velocity, 1.0);
-  return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity);
+
+  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_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx -
+                                                    lat_lon_offset.max_lon_offset -
+                                                    planner_data_->parameters.rear_overhang;
+    if (0.0 < lon_offset_ego_front_to_obj_back) {  // The object is ahead of the ego.
+      return lon_offset_ego_front_to_obj_back / relative_velocity;
+    } else if (0.0 < lon_offset_obj_front_to_ego_back) {  // The ego is ahead of the object.
+      return lon_offset_obj_front_to_ego_back / -relative_velocity;
+    }
+    // The ego and object are colliding.
+    return 0.0;
+  }();
+  const double signed_time_to_end_collision = [&]() {
+    const double lon_offset_ego_back_to_obj_front = lon_offset_ego_to_obj_idx +
+                                                    lat_lon_offset.max_lon_offset +
+                                                    planner_data_->parameters.rear_overhang;
+    const double lon_offset_obj_back_to_ego_front = -lon_offset_ego_to_obj_idx -
+                                                    lat_lon_offset.min_lon_offset +
+                                                    planner_data_->parameters.front_overhang;
+    if (0.0 < relative_velocity) {
+      return lon_offset_ego_back_to_obj_front / relative_velocity;
+    }
+    return lon_offset_obj_back_to_ego_front / -relative_velocity;
+  }();
+
+  // NOTE: In order to make time_to_start_collision continuous around the relative_velocity is zero.
+  const double time_to_start_collision = [&]() {
+    if (signed_time_to_start_collision < 0.0) {
+      return std::numeric_limits<double>::max();
+    }
+    return signed_time_to_start_collision;
+  }();
+  const double time_to_end_collision = [&]() {
+    if (signed_time_to_end_collision < 0.0) {
+      return std::numeric_limits<double>::max();
+    }
+    return signed_time_to_end_collision;
+  }();
+
+  return TimeWhileCollision{time_to_start_collision, time_to_end_collision};
 }
 
 bool DynamicAvoidanceModule::isObjectFarFromPath(
@@ -923,7 +962,7 @@
   const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
   const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
   const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape,
-  const double time_to_collision) const
+  const TimeWhileCollision & time_while_collision) const
 {
   const size_t obj_seg_idx =
     motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position);
@@ -938,40 +977,54 @@
       obj_lon_offset_vec.push_back(lon_offset);
     }
 
-    const auto raw_obj_lon_offset = getMinMaxValues(obj_lon_offset_vec);
+    return getMinMaxValues(obj_lon_offset_vec);
+  }();
 
-    if (obj_vel < 0) {
-      return MinMaxValue{
-        raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value};
-    }
+  const double relative_velocity = getEgoSpeed() - obj_vel;
 
-    // NOTE: If time to collision is considered here, the ego is close to the object when starting
-    // avoidance.
-    //       The ego should start avoidance before overtaking.
-    return raw_obj_lon_offset;
+  // calculate bound start and end length
+  const double start_length_to_avoid = [&]() {
+    if (obj_vel < parameters_->max_stopped_object_vel) {
+      // The ego and object are the same directional or the object is parked.
+      return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) +
+             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 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 /
+      obj_acc;
+    const double ego_moving_dist = getEgoSpeed() * decel_time;
+    return std::max(0.0, ego_moving_dist - obj_moving_dist) +
+           std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object;
+  }();
+  const double end_length_to_avoid = [&]() {
+    if (obj_vel < parameters_->max_stopped_object_vel) {
+      // The ego and object are the same directional or the object is parked.
+      return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object;
+    }
+    // The ego and object are the opposite directional.
+    return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel +
+           std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object;
   }();
 
-  // calculate bound start and end index
-  const bool is_object_overtaking = (0.0 <= obj_vel);
-  // TODO(murooka) use getEgoSpeed() instead of obj_vel
-  const double start_length_to_avoid =
-    std::abs(obj_vel) * (is_object_overtaking
-                           ? parameters_->start_duration_to_avoid_overtaking_object
-                           : parameters_->start_duration_to_avoid_oncoming_object);
-  const double end_length_to_avoid =
-    std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object
-                                              : parameters_->end_duration_to_avoid_oncoming_object);
-
-  const double valid_length_to_avoid = calcValidLengthToAvoid(obj_path, obj_pose, obj_shape);
-  if (obj_vel < -0.5) {
+  // calculate valid path for the forked object's path from the ego's path
+  if (obj_vel < -parameters_->max_stopped_object_vel) {
+    const bool is_object_same_direction = false;
+    const double valid_length_to_avoid =
+      calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction);
     return MinMaxValue{
-      std::max(obj_lon_offset.min_value - start_length_to_avoid, -valid_length_to_avoid),
+      obj_lon_offset.min_value + std::max(-start_length_to_avoid, -valid_length_to_avoid),
       obj_lon_offset.max_value + end_length_to_avoid};
   }
-  if (0.5 < obj_vel) {
+  if (parameters_->max_stopped_object_vel < obj_vel) {
+    const bool is_object_same_direction = true;
+    const double valid_length_to_avoid =
+      calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction);
     return MinMaxValue{
       obj_lon_offset.min_value - start_length_to_avoid,
-      std::min(obj_lon_offset.max_value + end_length_to_avoid, valid_length_to_avoid)};
+      obj_lon_offset.max_value + std::min(end_length_to_avoid, valid_length_to_avoid)};
   }
   return MinMaxValue{
     obj_lon_offset.min_value - start_length_to_avoid,
@@ -980,25 +1033,44 @@
 
 double DynamicAvoidanceModule::calcValidLengthToAvoid(
   const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
-  const autoware_auto_perception_msgs::msg::Shape & obj_shape) const
+  const autoware_auto_perception_msgs::msg::Shape & obj_shape,
+  const bool is_object_same_direction) const
 {
-  const auto input_path_points = getPreviousModuleOutput().path.points;
+  const auto & input_path_points = getPreviousModuleOutput().path.points;
   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);
+
+  // crop the ego's path by object position
+  std::vector<PathPointWithLaneId> cropped_ego_path_points;
+  if (is_object_same_direction) {
+    cropped_ego_path_points = std::vector<PathPointWithLaneId>{
+      input_path_points.begin() + obj_seg_idx, input_path_points.end()};
+  } else {
+    cropped_ego_path_points = std::vector<PathPointWithLaneId>{
+      input_path_points.begin(), input_path_points.begin() + obj_seg_idx + 1 + 1};
+    std::reverse(cropped_ego_path_points.begin(), cropped_ego_path_points.end());
+  }
+  if (cropped_ego_path_points.size() < 2) {
+    return motion_utils::calcArcLength(obj_path.path);
+  }
+
+  // calculate where the object's path will be forked from (= far from) the ego's path.
+  std::optional<size_t> last_nearest_ego_path_seg_idx{std::nullopt};
   const size_t valid_obj_path_end_idx = [&]() {
-    int ego_path_idx = obj_seg_idx + 1;
+    size_t ego_path_seg_idx = 0;
     for (size_t obj_path_idx = 0; obj_path_idx < obj_path.path.size(); ++obj_path_idx) {
       bool are_paths_close{false};
-      for (; 0 < ego_path_idx; --ego_path_idx) {
+      for (; ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) {
         const double dist_to_segment = calcDistanceToSegment(
           obj_path.path.at(obj_path_idx).position,
-          input_path_points.at(ego_path_idx).point.pose.position,
-          input_path_points.at(ego_path_idx - 1).point.pose.position);
-        if (
-          dist_to_segment < planner_data_->parameters.vehicle_width / 2.0 +
-                              parameters_->lat_offset_from_obstacle +
-                              calcObstacleMaxLength(obj_shape)) {
+          cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position,
+          cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position);
+        if (dist_to_segment < dist_threshold_paths) {
+          last_nearest_ego_path_seg_idx = ego_path_seg_idx;
           are_paths_close = true;
           break;
         }
@@ -1010,6 +1082,43 @@
     }
     return obj_path.path.size() - 1;
   }();
+
+  // calculate valid length to avoid
+  if (last_nearest_ego_path_seg_idx && valid_obj_path_end_idx != obj_path.path.size() - 1) {
+    const auto calc_min_dist = [&](const size_t arg_obj_path_idx) -> std::optional<double> {
+      std::optional<double> min_dist{std::nullopt};
+      for (size_t ego_path_seg_idx = *last_nearest_ego_path_seg_idx;
+           ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) {
+        const double dist_to_segment = calcDistanceToSegment(
+          obj_path.path.at(arg_obj_path_idx).position,
+          cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position,
+          cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position);
+        if (!min_dist || dist_to_segment < *min_dist) {
+          min_dist = dist_to_segment;
+        }
+        if (min_dist && *min_dist < dist_to_segment) {
+          return *min_dist;
+        }
+      }
+      return min_dist;
+    };
+    const size_t prev_valid_obj_path_end_idx =
+      (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx : valid_obj_path_end_idx - 1;
+    const size_t next_valid_obj_path_end_idx =
+      (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx + 1 : valid_obj_path_end_idx;
+    const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx);
+    const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx);
+    if (prev_min_dist && next_min_dist) {
+      const double segment_length = tier4_autoware_utils::calcDistance2d(
+        obj_path.path.at(prev_valid_obj_path_end_idx),
+        obj_path.path.at(next_valid_obj_path_end_idx));
+      const double partial_segment_length = segment_length *
+                                            (dist_threshold_paths - *prev_min_dist) /
+                                            (*next_min_dist - *prev_min_dist);
+      return motion_utils::calcSignedArcLength(obj_path.path, 0, prev_valid_obj_path_end_idx) +
+             partial_segment_length;
+    }
+  }
   return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx);
 }