ttc_calculator Last update: 16.07.2025 1 3 4#include "autonomy/evaluator/ttc_calculator/ttc_calculator.h" 5#include "autonomy/evaluator/ttc_calculator/utils.h" 6#include <optional> 7 8namespace simulation_framework 9{ 10namespace evaluator 11{ 12 13using namespace utils; 14 15std::chrono::milliseconds TimeToCollisionCalculator::Calculate(const osi3::GroundTruth& ground_truth) const 16{ 17 auto host_vehicle_index = std::make_optional<int>(); 18 19 const auto& host_vehicle_id = ground_truth.host_vehicle_id(); 20 const auto moving_objects_count = ground_truth.moving_object_size(); 21 22 for (int i = 0; i < moving_objects_count; ++i) 23 { 24 if (host_vehicle_id.value() == ground_truth.moving_object(i).id().value()) 25 { 26 host_vehicle_index.value() = i; 27 break; 28 } 29 } 30 31 if (!host_vehicle_index.has_value()) 32 { 33 return std::chrono::milliseconds::max(); 34 } 35 36 const auto& host_vehicle_base = ground_truth.moving_object(host_vehicle_index.value()).base(); 37 BoundingBox bounding_box_ego{host_vehicle_base.position().x(), 38 host_vehicle_base.position().y(), 39 host_vehicle_base.dimension().width(), 40 host_vehicle_base.dimension().length(), 41 host_vehicle_base.orientation().yaw(), 42 host_vehicle_base.velocity().x(), 43 host_vehicle_base.velocity().y()}; 44 BoundingBox bounding_box_target{}; 45 std::optional<int> target_vehicle_index = std::make_optional<int>(); 46 47 auto minimum_distance_to_host{std::numeric_limits<double>::infinity()}; 48 49 for (int i = 0; i < moving_objects_count; ++i) 50 { 51 if (host_vehicle_index == i) 52 { 53 continue; 54 } 55 56 const auto& vehicle_base = ground_truth.moving_object(i).base(); 57 BoundingBox bounding_box_vehicle{vehicle_base.position().x(), 58 vehicle_base.position().y(), 59 vehicle_base.dimension().width(), 60 vehicle_base.dimension().length(), 61 vehicle_base.orientation().yaw(), 62 vehicle_base.velocity().x(), 63 vehicle_base.velocity().y()}; 64 65 if (!CanPotentiallyHit(bounding_box_ego, bounding_box_vehicle)) 66 { 67 continue; 68 } 69 70 auto host_to_vehicle_distance = Calculate2dVectorNorm({bounding_box_ego.x, bounding_box_ego.y}, 71 {bounding_box_vehicle.x, bounding_box_vehicle.y}) - 72 (bounding_box_ego.length * 0.5 + bounding_box_vehicle.length * 0.5); 73 74 if (minimum_distance_to_host > host_to_vehicle_distance) 75 { 76 minimum_distance_to_host = host_to_vehicle_distance; 77 target_vehicle_index.value() = i; 78 bounding_box_target = bounding_box_vehicle; 79 } 80 } 81 82 if (!target_vehicle_index.has_value()) 83 { 84 return std::chrono::milliseconds::max(); 85 } 86 87 const double host_to_target_relative_velocity = 88 Calculate2dVectorNorm({bounding_box_ego.velocity_x, bounding_box_ego.velocity_y}, {}) - 89 Calculate2dVectorNorm({bounding_box_target.velocity_x, bounding_box_target.velocity_y}, {}); 90 91 double distance_to_target = minimum_distance_to_host; 92 93 if ((bounding_box_ego.x - bounding_box_target.x) > 0.0) 94 { 95 distance_to_target = -minimum_distance_to_host; 96 } 97 98 std::chrono::milliseconds time_to_collision{std::chrono::milliseconds::max()}; 99 100 if (AreBoundingBoxesOverlapped(bounding_box_ego, bounding_box_target)) 101 { 103 return std::chrono::milliseconds{0}; 104 } 105 106 if (host_to_target_relative_velocity != 0.0 && 107 (std::signbit(distance_to_target) == std::signbit(host_to_target_relative_velocity))) 108 { 109 // Introducing the following factor to get milliseconds instead of seconds 110 uint64_t time_conversion_factor = 1000u; 111 time_to_collision = std::chrono::milliseconds{static_cast<uint64_t>( 112 std::abs(distance_to_target / host_to_target_relative_velocity) * time_conversion_factor)}; 113 } 114 115 return time_to_collision; 116}; 117 118} // namespace evaluator 119} // namespace simulation_framework evaluatorThe namespace containing evaluator implementations. simulation_frameworkThe top namespace for simulation framework.
