## On this page

  

 

 # distance\_calculator

 Last update: 16.07.2025 

<a id="l00001" name="l00001"></a> 1 

<a id="l00003" name="l00003"></a> 3 

<a id="l00004" name="l00004"></a> 4\#include "autonomy/evaluator/distance\_calculator/distance\_calculator.h"

<a id="l00005" name="l00005"></a> 5\#include &lt;cmath&gt;

<a id="l00006" name="l00006"></a> 6 

<a id="l00007" name="l00007"></a> 7namespace [simulation\_framework](namespacesimulation__framework.xhtml)

<a id="l00008" name="l00008"></a> 8{

<a id="l00009" name="l00009"></a> 9namespace [evaluator](namespaceevaluator.xhtml)

<a id="l00010" name="l00010"></a> 10{

<a id="l00011" name="l00011"></a> 11 

<a id="l00012" name="l00012"></a> 12double DistanceDrivenCalculator::Calculate(const osi3::GroundTruth&amp; ground_truth)

<a id="l00013" name="l00013"></a> 13{

<a id="l00014" name="l00014"></a> 14 double distance = 0.0;

<a id="l00015" name="l00015"></a> 15 auto host_vehicle_index = std::make_optional&lt;int&gt;();

<a id="l00016" name="l00016"></a> 16 

<a id="l00017" name="l00017"></a> 17 const auto&amp; host_vehicle_id = ground_truth.host_vehicle_id();

<a id="l00018" name="l00018"></a> 18 const auto moving_objects_count = ground_truth.moving_object_size();

<a id="l00019" name="l00019"></a> 19 

<a id="l00020" name="l00020"></a> 20 for (int i = 0; i &lt; moving_objects_count; ++i)

<a id="l00021" name="l00021"></a> 21 {

<a id="l00022" name="l00022"></a> 22 if (host_vehicle_id.value() == ground_truth.moving_object(i).id().value())

<a id="l00023" name="l00023"></a> 23 {

<a id="l00024" name="l00024"></a> 24 host_vehicle_index.value() = i;

<a id="l00025" name="l00025"></a> 25 break;

<a id="l00026" name="l00026"></a> 26 }

<a id="l00027" name="l00027"></a> 27 }

<a id="l00028" name="l00028"></a> 28 

<a id="l00029" name="l00029"></a> 29 if (!host_vehicle_index.has_value())

<a id="l00030" name="l00030"></a> 30 {

<a id="l00031" name="l00031"></a> 31 return distance;

<a id="l00032" name="l00032"></a> 32 }

<a id="l00033" name="l00033"></a> 33 

<a id="l00034" name="l00034"></a> 34 const auto&amp; host_vehicle_base = ground_truth.moving_object(host_vehicle_index.value()).base();

<a id="l00035" name="l00035"></a> 35 

<a id="l00036" name="l00036"></a> 36 if (!is_initialized_)

<a id="l00037" name="l00037"></a> 37 {

<a id="l00038" name="l00038"></a> 38 is_initialized_ = true;

<a id="l00039" name="l00039"></a> 39 last_xpos_ = host_vehicle_base.position().x();

<a id="l00040" name="l00040"></a> 40 last_ypos_ = host_vehicle_base.position().y();

<a id="l00041" name="l00041"></a> 41 return 0;

<a id="l00042" name="l00042"></a> 42 }

<a id="l00043" name="l00043"></a> 43 

<a id="l00044" name="l00044"></a> 44 double current_xpos = host_vehicle_base.position().x();

<a id="l00045" name="l00045"></a> 45 double current_ypos = host_vehicle_base.position().y();

<a id="l00046" name="l00046"></a> 46 

<a id="l00047" name="l00047"></a> 47 // Values to be substituted in the standard Euclidean distance formula

<a id="l00048" name="l00048"></a> 48 const auto delta_xpos = last_xpos_.value() - current_xpos;

<a id="l00049" name="l00049"></a> 49 const auto delta_ypos = last_ypos_.value() - current_ypos;

<a id="l00050" name="l00050"></a> 50 

<a id="l00051" name="l00051"></a> 51 // Euclidean distance formula to calculate distance between two points/positions.

<a id="l00052" name="l00052"></a> 52 distance = std::sqrt((delta_xpos * delta_xpos) + (delta_ypos * delta_ypos));

<a id="l00053" name="l00053"></a> 53 

<a id="l00054" name="l00054"></a> 54 // Update value of last position values with the recent current position value.

<a id="l00055" name="l00055"></a> 55 last_xpos_ = current_xpos;

<a id="l00056" name="l00056"></a> 56 last_ypos_ = current_ypos;

<a id="l00057" name="l00057"></a> 57 

<a id="l00058" name="l00058"></a> 58 return distance;

<a id="l00059" name="l00059"></a> 59}

<a id="l00060" name="l00060"></a> 60} // namespace evaluator

<a id="l00061" name="l00061"></a> 61 

<a id="l00062" name="l00062"></a> 62} // namespace simulation\_framework

[evaluator](namespaceevaluator.xhtml)

The namespace containing evaluator implementations.



[simulation\_framework](namespacesimulation__framework.xhtml)

The top namespace for simulation framework.