fix time to collision

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Aug 7, 2023 · 54200d7 · 54200d7
1 parent 28ecd45
commit 54200d7
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 7 deletions.
diff --git a/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
@@ -312,7 +312,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     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;
+    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(

diff --git a/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -466,9 +466,9 @@ void DynamicAvoidanceModule::updateTargetObjects()
       continue;
     }
 
-    // 2.e. check if time to collision
+    // 2.e. check time to collision
     const double time_to_collision =
-      calcTimeToCollision(prev_module_path->points, object.pose, object.vel);
+      calcTimeToCollision(prev_module_path->points, object.pose, object.vel, lat_lon_offset);
     if (
       (0 <= object.vel &&
        parameters_->max_time_to_collision_overtaking_object < time_to_collision) ||
@@ -494,7 +494,6 @@ void DynamicAvoidanceModule::updateTargetObjects()
     const bool is_collision_left = future_obj_pose
                                      ? isLeft(prev_module_path->points, future_obj_pose->position)
                                      : is_object_left;
-    std::cerr << obj_uuid << " " << time_to_collision << " " << is_collision_left << std::endl;
 
     // 2.g. calculate longitudinal and lateral offset to avoid
     const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
@@ -573,15 +572,24 @@ DynamicAvoidanceModule::calcCollisionSection(
 
 double DynamicAvoidanceModule::calcTimeToCollision(
   const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
-  const double obj_tangent_vel) const
+  const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const
 {
-  const double relative_velocity = getEgoSpeed() - obj_tangent_vel;
+  // 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 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 signed_lon_length / positive_relative_velocity;
+  return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity);
 }
 
 bool DynamicAvoidanceModule::isObjectFarFromPath(