predictive_ttc_calculator

1

3

8{

10{

11

12using namespace utils;

13

14std::chrono::milliseconds PredictiveTimeToCollisionCalculator::Calculate(const osi3::GroundTruth& ground_truth) const

15{

16 auto host_vehicle_validation = std::make_optional<int>();

17

18 const auto& host_vehicle_id = ground_truth.host_vehicle_id();

19 BoundingBox bounding_box_ego;

20 const auto& gt_moving_objects = ground_truth.moving_object();

21 for (auto object : gt_moving_objects)

22 {

23 if (host_vehicle_id.value() == object.id().value())

24 {

25 host_vehicle_validation = object.id().value();

26 bounding_box_ego.x = object.base().position().x();

27 bounding_box_ego.y = object.base().position().y();

28 bounding_box_ego.width = object.base().dimension().width();

29 bounding_box_ego.length = object.base().dimension().length();

30 bounding_box_ego.yaw = object.base().orientation().yaw();

31 bounding_box_ego.velocity_x = object.base().velocity().x();

32 bounding_box_ego.velocity_y = object.base().velocity().y();

33 break;

34 }

35 }

36

37 if (!host_vehicle_validation.has_value())

38 {

39 return std::chrono::milliseconds::max();

40 }

41

42 auto predictive_minimum_ttc{std::chrono::milliseconds::max()};

43

44 for (auto object : gt_moving_objects)

45 {

46 if (host_vehicle_id.value() == object.id().value())

47 {

48 continue;

49 }

50

51 const auto& vehicle_base = object.base();

52 BoundingBox bounding_box_vehicle{vehicle_base.position().x(),

53 vehicle_base.position().y(),

54 vehicle_base.dimension().width(),

55 vehicle_base.dimension().length(),

56 vehicle_base.orientation().yaw(),

57 vehicle_base.velocity().x(),

58 vehicle_base.velocity().y()};

59

60 if (AreBoundingBoxesOverlapped(bounding_box_vehicle, bounding_box_ego))

61 {

62 return std::chrono::milliseconds(0);

63 }

64 predictive_minimum_ttc =

65 std::min(PredictTimeToCollision(

66 bounding_box_vehicle, bounding_box_ego, predictive_ttc_precision_, predictive_ttc_max_),

67 predictive_minimum_ttc);

68 }

69

70 return predictive_minimum_ttc;

71};

72

73} // namespace evaluator

74} // namespace simulation_framework