Keep the colleague's microbu-esp32c5 tree in this repository
obu-firmware builds against vanetza-idf from microbu-esp32c5/external, but that tree was gitignored, so a clone of this repository could not build the firmware it ships. It is now committed here as ordinary files in its own folder, microbu-esp32c5/: the colleague's commit cf4b99f plus the V2X2MAP bridge's signature verification (--trust) used on the bench. Nothing is fetched from or pushed to the colleague's repository; this repository and its remotes carry everything. The folder's own .gitignore keeps build output, downloaded components and private key material out, as it did there; the committed file set is identical to that repository's tracked files. The ESP32-C5 is still flashed from obu-firmware/, which only takes vanetza-idf from microbu-esp32c5/, so the two stay separate folders. FLASHING.md says how to take a newer version of the colleague's tree (copy it over the folder, rebuild, test, commit).
This commit is contained in:
Ashin Walpola
@@ -0,0 +1,6 @@
|
||||
include(UseGTest)
|
||||
configure_gtest_directory(LINK_LIBRARIES facilities)
|
||||
|
||||
add_gtest(CamFunctions cam_functions.cpp LINK_LIBRARIES asn1_its_r2)
|
||||
add_gtest(PathHistory path_history.cpp)
|
||||
add_gtest(PathPoint path_point.cpp)
|
||||
+340
@@ -0,0 +1,340 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <vanetza/facilities/path_history.hpp>
|
||||
#include <vanetza/asn1/type_traits.hpp>
|
||||
#include <vanetza/asn1/its/Heading.h>
|
||||
#include <vanetza/asn1/its/PathHistory.h>
|
||||
#include <vanetza/asn1/its/ReferencePosition.h>
|
||||
#include <vanetza/asn1/its/r2/Heading.h>
|
||||
#include <vanetza/asn1/its/r2/PathHistory.h>
|
||||
#include <vanetza/asn1/its/r2/ReferencePosition.h>
|
||||
#include <vanetza/facilities/cam_functions.hpp>
|
||||
#include <boost/units/cmath.hpp>
|
||||
#include <boost/units/io.hpp>
|
||||
#include <cmath>
|
||||
|
||||
namespace vanetza
|
||||
{
|
||||
namespace asn1
|
||||
{
|
||||
|
||||
template<> struct asn1_type_traits<::PathHistory>
|
||||
{
|
||||
static asn_TYPE_descriptor_t& descriptor() { return ::asn_DEF_PathHistory; }
|
||||
};
|
||||
|
||||
template<> struct asn1_type_traits<::Vanetza_ITS2_PathHistory>
|
||||
{
|
||||
static asn_TYPE_descriptor_t& descriptor() { return ::asn_DEF_Vanetza_ITS2_PathHistory; }
|
||||
};
|
||||
|
||||
} // namespace asn1
|
||||
} // namespace vanetza
|
||||
|
||||
using namespace vanetza;
|
||||
using namespace vanetza::facilities;
|
||||
using namespace vanetza::units;
|
||||
|
||||
constexpr long latitude(double degree, double arc_minute)
|
||||
{
|
||||
return std::round(1e7 * (degree + arc_minute / 60.0));
|
||||
}
|
||||
|
||||
constexpr long longitude(double degree, double arc_minute)
|
||||
{
|
||||
return std::round(1e7 * (degree + arc_minute / 60.0));
|
||||
}
|
||||
|
||||
::testing::AssertionResult NearDistance(const Length& a, const Length& b, Length delta)
|
||||
{
|
||||
using namespace boost::units;
|
||||
const auto diff = abs(a - b);
|
||||
if (diff < delta) {
|
||||
return ::testing::AssertionSuccess();
|
||||
} else {
|
||||
return ::testing::AssertionFailure() << "actual difference " << diff << " exceeds delta of " << delta;
|
||||
}
|
||||
}
|
||||
|
||||
using HeadingTypes = ::testing::Types<Heading, Vanetza_ITS2_Heading>;
|
||||
template<typename T>
|
||||
class CamFunctionsHeading : public ::testing::Test
|
||||
{
|
||||
};
|
||||
TYPED_TEST_SUITE(CamFunctionsHeading, HeadingTypes);
|
||||
|
||||
TEST(CamFunctionsHeading, similar_heading)
|
||||
{
|
||||
Angle a = 3 * si::radian;
|
||||
Angle b = 2 * si::radian;
|
||||
Angle limit = 0.5 * si::radian;
|
||||
EXPECT_FALSE(similar_heading(a, b, limit));
|
||||
EXPECT_FALSE(similar_heading(b, a, limit));
|
||||
|
||||
limit = 1.0 * si::radian;
|
||||
EXPECT_TRUE(similar_heading(a, b, limit));
|
||||
EXPECT_TRUE(similar_heading(b, a, limit));
|
||||
|
||||
a = 6.1 * si::radian;
|
||||
b = 0.2 * si::radian;
|
||||
limit = 0.4 * si::radian;
|
||||
EXPECT_TRUE(similar_heading(a, b, limit));
|
||||
EXPECT_TRUE(similar_heading(b, a, limit));
|
||||
|
||||
limit = 0.3 * si::radian;
|
||||
EXPECT_FALSE(similar_heading(a, b, limit));
|
||||
EXPECT_FALSE(similar_heading(b, a, limit));
|
||||
|
||||
limit = -1.0 * si::radian;
|
||||
EXPECT_FALSE(similar_heading(a, a, limit));
|
||||
}
|
||||
|
||||
TYPED_TEST(CamFunctionsHeading, similar_heading_unavailable1)
|
||||
{
|
||||
using SomeHeading = TypeParam;
|
||||
|
||||
SomeHeading a;
|
||||
a.headingValue = HeadingValue_unavailable;
|
||||
Angle b = 2.0 * si::radian;
|
||||
Angle limit = 10 * si::radian;
|
||||
EXPECT_FALSE(is_available(a));
|
||||
EXPECT_FALSE(similar_heading(a, b, limit));
|
||||
|
||||
a.headingValue = 2 * HeadingValue_wgs84East;
|
||||
EXPECT_TRUE(is_available(a));
|
||||
EXPECT_TRUE(similar_heading(a, b, limit));
|
||||
|
||||
b = 0.0 * si::radian;
|
||||
limit = 3.14 * si::radian;
|
||||
EXPECT_FALSE(similar_heading(a, b, limit));
|
||||
|
||||
limit = 3.15 * si::radian;
|
||||
EXPECT_TRUE(similar_heading(a, b, limit));
|
||||
}
|
||||
|
||||
TYPED_TEST(CamFunctionsHeading, similar_heading_unavailable2)
|
||||
{
|
||||
using SomeHeading = TypeParam;
|
||||
|
||||
SomeHeading a;
|
||||
a.headingValue = HeadingValue_unavailable;
|
||||
SomeHeading b;
|
||||
b.headingValue = HeadingValue_unavailable;
|
||||
Angle limit = 10 * si::radian;
|
||||
EXPECT_FALSE(is_available(a));
|
||||
EXPECT_FALSE(is_available(b));
|
||||
EXPECT_FALSE(similar_heading(a, b, limit));
|
||||
|
||||
b.headingValue = 200;
|
||||
EXPECT_TRUE(is_available(b));
|
||||
EXPECT_FALSE(similar_heading(a, b, limit));
|
||||
EXPECT_FALSE(similar_heading(b, a, limit));
|
||||
|
||||
a.headingValue = 300;
|
||||
EXPECT_TRUE(is_available(a));
|
||||
EXPECT_TRUE(similar_heading(a, b, limit));
|
||||
EXPECT_TRUE(similar_heading(b, a, limit));
|
||||
}
|
||||
|
||||
using ReferencePositionTypes = ::testing::Types<ReferencePosition_t, Vanetza_ITS2_ReferencePosition_t>;
|
||||
template<typename T>
|
||||
class CamFunctionsReferencePosition : public ::testing::Test
|
||||
{
|
||||
};
|
||||
TYPED_TEST_SUITE(CamFunctionsReferencePosition, ReferencePositionTypes);
|
||||
|
||||
TYPED_TEST(CamFunctionsReferencePosition, distance_reference_positions)
|
||||
{
|
||||
using SomeReferencePosition = TypeParam;
|
||||
|
||||
SomeReferencePosition pos1;
|
||||
pos1.latitude = -latitude(6, 21.23);
|
||||
pos1.longitude = -longitude(33, 22.12);
|
||||
SomeReferencePosition pos2;
|
||||
pos2.latitude = -latitude(6, 22.48);
|
||||
pos2.longitude = -longitude(33, 22.55);
|
||||
EXPECT_TRUE(NearDistance(distance(pos1, pos2), 2440.0 * si::meter , 10.0 * si::meter));
|
||||
|
||||
SomeReferencePosition pos3;
|
||||
pos3.latitude = latitude(37, 17.3);
|
||||
pos3.longitude = -longitude(0, 13.14);
|
||||
SomeReferencePosition pos4;
|
||||
pos4.latitude = latitude(37, 17.19);
|
||||
pos4.longitude = longitude(0, 9.45);
|
||||
EXPECT_TRUE(NearDistance(distance(pos3, pos4), 33390.0 * si::meter , 100.0 * si::meter));
|
||||
|
||||
SomeReferencePosition pos5;
|
||||
pos5.latitude = -latitude(0, 19.24);
|
||||
pos5.longitude = longitude(83, 37.32);
|
||||
SomeReferencePosition pos6;
|
||||
pos6.latitude = latitude(0, 27.15);
|
||||
pos6.longitude = longitude(83, 04.45);
|
||||
EXPECT_TRUE(NearDistance(distance(pos5, pos6), 105010.0 * si::meter , 300.0 * si::meter));
|
||||
|
||||
SomeReferencePosition pos7;
|
||||
pos7.latitude = latitude(48, 45.56);
|
||||
pos7.longitude = longitude(11, 26.01);
|
||||
SomeReferencePosition pos8;
|
||||
pos8.latitude = latitude(48, 45.566);
|
||||
pos8.longitude = longitude(11, 26.04);
|
||||
EXPECT_TRUE(NearDistance(distance(pos7, pos8), 38.0 * si::meter , 0.5 * si::meter));
|
||||
}
|
||||
|
||||
TYPED_TEST(CamFunctionsReferencePosition, distance_refpos_latlon)
|
||||
{
|
||||
using SomeReferencePosition = TypeParam;
|
||||
|
||||
SomeReferencePosition refpos;
|
||||
refpos.latitude = -latitude(6, 21.23);
|
||||
refpos.longitude = -longitude(33, 22.12);
|
||||
GeoAngle lat = -(6 + (22.48 / 60.0)) * degree;
|
||||
GeoAngle lon = -(33 + (22.55 / 60.0)) * degree;
|
||||
EXPECT_TRUE(NearDistance(distance(refpos, lat, lon), 2440.0 * si::meter , 10.0 * si::meter));
|
||||
}
|
||||
|
||||
TYPED_TEST(CamFunctionsReferencePosition, distance_unavailable)
|
||||
{
|
||||
using SomeReferencePosition = TypeParam;
|
||||
|
||||
SomeReferencePosition pos1 {};
|
||||
SomeReferencePosition pos2 {};
|
||||
EXPECT_TRUE(is_available(pos1));
|
||||
EXPECT_TRUE(is_available(pos2));
|
||||
EXPECT_FALSE(std::isnan(distance(pos1, pos2).value()));
|
||||
|
||||
pos1.latitude = Latitude_unavailable;
|
||||
EXPECT_FALSE(is_available(pos1));
|
||||
EXPECT_TRUE(std::isnan(distance(pos1, pos2).value()));
|
||||
EXPECT_TRUE(std::isnan(distance(pos2, pos1).value()));
|
||||
|
||||
pos1.latitude = 0;
|
||||
pos1.longitude = Longitude_unavailable;
|
||||
EXPECT_FALSE(is_available(pos1));
|
||||
EXPECT_TRUE(std::isnan(distance(pos1, pos2).value()));
|
||||
EXPECT_TRUE(std::isnan(distance(pos2, pos1).value()));
|
||||
}
|
||||
|
||||
TYPED_TEST(CamFunctionsReferencePosition, copy)
|
||||
{
|
||||
using SomeReferencePosition = TypeParam;
|
||||
|
||||
PositionFix src;
|
||||
src.latitude = 1.23 * vanetza::units::degree;
|
||||
src.longitude = 4.56 * vanetza::units::degree;
|
||||
src.confidence.orientation = vanetza::units::TrueNorth::from_value(10.0);
|
||||
src.confidence.semi_major = 20 * vanetza::units::si::meter;
|
||||
src.confidence.semi_minor = 15 * vanetza::units::si::meter;
|
||||
|
||||
SomeReferencePosition dest;
|
||||
copy(src, dest);
|
||||
|
||||
EXPECT_EQ(dest.latitude, 123 * 100000);
|
||||
EXPECT_EQ(dest.longitude, 456 * 100000);
|
||||
EXPECT_EQ(dest.positionConfidenceEllipse.semiMajorConfidence, 20 * 100);
|
||||
EXPECT_EQ(dest.positionConfidenceEllipse.semiMinorConfidence, 15 * 100);
|
||||
EXPECT_EQ(dest.positionConfidenceEllipse.semiMajorOrientation, 10 * 10);
|
||||
EXPECT_EQ(dest.altitude.altitudeValue, AltitudeValue_unavailable);
|
||||
EXPECT_EQ(dest.altitude.altitudeConfidence, AltitudeConfidence_unavailable);
|
||||
}
|
||||
|
||||
using PathHistoryTypes = ::testing::Types<::PathHistory, Vanetza_ITS2_PathHistory>;
|
||||
template<typename T>
|
||||
class CamFunctionsPathHistory : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
vanetza::facilities::PathHistory path_history;
|
||||
|
||||
void add_sample(double lat, double lon, const std::string& time)
|
||||
{
|
||||
vanetza::facilities::PathPoint path_point;
|
||||
path_point.latitude = lat * degree;
|
||||
path_point.longitude = lon * degree;
|
||||
path_point.time = boost::posix_time::from_iso_string(time);
|
||||
path_history.addSample(path_point);
|
||||
}
|
||||
};
|
||||
TYPED_TEST_SUITE(CamFunctionsPathHistory, PathHistoryTypes);
|
||||
|
||||
TYPED_TEST(CamFunctionsPathHistory, copy_path_history)
|
||||
{
|
||||
using SomePathHistory = TypeParam;
|
||||
|
||||
this->add_sample(40.906, 29.155, "20241027T031000");
|
||||
this->add_sample(40.907, 29.156, "20241027T031010");
|
||||
this->add_sample(40.908, 29.157, "20241027T031020");
|
||||
|
||||
SomePathHistory dest_path_history = {}; // zero-initialize struct
|
||||
copy(this->path_history, dest_path_history);
|
||||
|
||||
int size = dest_path_history.list.count;
|
||||
EXPECT_EQ(size, 2);
|
||||
// incremental encoding: each point relative to the previous
|
||||
EXPECT_EQ(dest_path_history.list.array[0]->pathPosition.deltaLatitude,
|
||||
dest_path_history.list.array[1]->pathPosition.deltaLatitude);
|
||||
EXPECT_EQ(*dest_path_history.list.array[0]->pathDeltaTime, 1000); // 10 s
|
||||
EXPECT_EQ(*dest_path_history.list.array[1]->pathDeltaTime, 1000); // 10 s
|
||||
for (int i = 0; i < size; i++) {
|
||||
auto current_path_point = dest_path_history.list.array[i];
|
||||
ASSERT_NE(current_path_point->pathDeltaTime, nullptr);
|
||||
|
||||
// check ASN.1 constraints
|
||||
EXPECT_GE(*current_path_point->pathDeltaTime, 1);
|
||||
EXPECT_LE(*current_path_point->pathDeltaTime, 65535);
|
||||
EXPECT_GE(current_path_point->pathPosition.deltaLatitude, -131071);
|
||||
EXPECT_LE(current_path_point->pathPosition.deltaLatitude, 131072);
|
||||
EXPECT_GE(current_path_point->pathPosition.deltaLongitude, -131071);
|
||||
EXPECT_LE(current_path_point->pathPosition.deltaLongitude, 131072);
|
||||
EXPECT_GE(current_path_point->pathPosition.deltaAltitude, -12700);
|
||||
EXPECT_LE(current_path_point->pathPosition.deltaAltitude, 12800);
|
||||
}
|
||||
|
||||
asn1::reset(dest_path_history);
|
||||
}
|
||||
|
||||
TYPED_TEST(CamFunctionsPathHistory, copy_clamps_stationary_delta_time)
|
||||
{
|
||||
using SomePathHistory = TypeParam;
|
||||
|
||||
this->add_sample(40.906, 29.155, "20241027T031000");
|
||||
this->add_sample(40.907, 29.156, "20241027T031010");
|
||||
this->add_sample(40.908, 29.157, "20241027T031020");
|
||||
// parked ~20 min at the last position: reference far past the max PathDeltaTime
|
||||
this->add_sample(40.908, 29.157, "20241027T033000");
|
||||
|
||||
SomePathHistory dest = {}; // zero-initialize struct
|
||||
copy(this->path_history, dest);
|
||||
|
||||
ASSERT_EQ(dest.list.count, 3); // whole trace kept alive
|
||||
ASSERT_NE(dest.list.array[0]->pathDeltaTime, nullptr);
|
||||
EXPECT_EQ(*dest.list.array[0]->pathDeltaTime, 65535); // only the first point is clamped
|
||||
for (int i = 1; i < dest.list.count; i++) {
|
||||
ASSERT_NE(dest.list.array[i]->pathDeltaTime, nullptr);
|
||||
EXPECT_LT(*dest.list.array[i]->pathDeltaTime, 65535); // later points keep their short gaps
|
||||
}
|
||||
|
||||
asn1::reset(dest);
|
||||
}
|
||||
|
||||
TYPED_TEST(CamFunctionsPathHistory, copy_truncates_after_long_stop)
|
||||
{
|
||||
using SomePathHistory = TypeParam;
|
||||
|
||||
// moved, parked ~21 min, then resumed: the stop is a >655 s gap in the middle
|
||||
this->add_sample(40.9000, 29.1500, "20241027T031000"); // pre-stop trail
|
||||
this->add_sample(40.9005, 29.1505, "20241027T031010");
|
||||
this->add_sample(40.9010, 29.1510, "20241027T031020");
|
||||
this->add_sample(40.9015, 29.1515, "20241027T033100"); // resumed ~21 min later
|
||||
this->add_sample(40.9020, 29.1520, "20241027T033110");
|
||||
this->add_sample(40.9025, 29.1525, "20241027T033120");
|
||||
|
||||
SomePathHistory dest = {}; // zero-initialize struct
|
||||
copy(this->path_history, dest);
|
||||
|
||||
// trail is cut at the discontinuity, not carried across it with a clamped mid gap
|
||||
ASSERT_GT(dest.list.count, 0);
|
||||
for (int i = 0; i < dest.list.count; i++) {
|
||||
ASSERT_NE(dest.list.array[i]->pathDeltaTime, nullptr);
|
||||
EXPECT_LT(*dest.list.array[i]->pathDeltaTime, 65535);
|
||||
}
|
||||
|
||||
asn1::reset(dest);
|
||||
}
|
||||
+223
@@ -0,0 +1,223 @@
|
||||
#include <vanetza/facilities/path_history.hpp>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using vanetza::facilities::PathHistory;
|
||||
using vanetza::facilities::PathPoint;
|
||||
namespace units = vanetza::units;
|
||||
|
||||
#define EXPECT_PATHPOINT_EQ(a, b) \
|
||||
EXPECT_DOUBLE_EQ(a.latitude.value(), b.latitude.value()); \
|
||||
EXPECT_DOUBLE_EQ(a.longitude.value(), b.longitude.value()); \
|
||||
EXPECT_DOUBLE_EQ(a.heading.value(), b.heading.value()); \
|
||||
EXPECT_EQ(a.time, b.time)
|
||||
|
||||
const units::GeoAngle cOneMeterLatitude = 1.0 / 111320.0 * units::degrees;
|
||||
|
||||
TEST(PathHistory, reference_point) {
|
||||
PathHistory ph;
|
||||
const PathPoint default_pp;
|
||||
EXPECT_PATHPOINT_EQ(default_pp, ph.getReferencePoint());
|
||||
|
||||
PathPoint first_pp;
|
||||
first_pp.longitude = 34.4 * units::degrees;
|
||||
first_pp.latitude = -10.3 * units::degrees;
|
||||
first_pp.heading = units::Angle { 48.3 * units::degrees };
|
||||
first_pp.time = boost::posix_time::time_from_string("2014-11-21 11:11:48");
|
||||
|
||||
ph.addSample(first_pp);
|
||||
EXPECT_PATHPOINT_EQ(first_pp, ph.getReferencePoint());
|
||||
|
||||
PathPoint second_pp;
|
||||
second_pp.longitude = 34.4 * units::degrees;
|
||||
second_pp.latitude = -10.3 * units::degrees;
|
||||
second_pp.heading = units::Angle { 48.3 * units::degrees };
|
||||
second_pp.time = boost::posix_time::time_from_string("2014-11-21 11:11:48.1");
|
||||
|
||||
ph.addSample(second_pp);
|
||||
EXPECT_PATHPOINT_EQ(second_pp, ph.getReferencePoint());
|
||||
}
|
||||
|
||||
TEST(PathHistory, concise_points_of_equal_samples) {
|
||||
PathHistory ph;
|
||||
EXPECT_EQ(0, ph.getConcisePoints().size());
|
||||
|
||||
PathPoint first_pp;
|
||||
first_pp.longitude = -3.45 * units::degrees;
|
||||
first_pp.latitude = 32.98 * units::degrees;
|
||||
ph.addSample(first_pp);
|
||||
ASSERT_EQ(1, ph.getConcisePoints().size());
|
||||
EXPECT_PATHPOINT_EQ(first_pp, ph.getConcisePoints().front());
|
||||
|
||||
for (unsigned i = 0; i < 10; ++i) {
|
||||
ph.addSample(first_pp);
|
||||
EXPECT_EQ(1, ph.getConcisePoints().size());
|
||||
}
|
||||
}
|
||||
|
||||
TEST(PathHistory, concise_points_chord_length_threshold) {
|
||||
PathHistory ph;
|
||||
|
||||
PathPoint pp;
|
||||
pp.longitude = 0.0 * units::degrees;
|
||||
pp.latitude = 0.0 * units::degrees;
|
||||
ph.addSample(pp);
|
||||
pp.latitude += cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
pp.latitude += cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(1, ph.getConcisePoints().size());
|
||||
|
||||
pp.latitude += cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(1, ph.getConcisePoints().size());
|
||||
|
||||
pp.latitude += 20.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
ASSERT_EQ(2, ph.getConcisePoints().size());
|
||||
EXPECT_DOUBLE_EQ(3.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().front().latitude.value());
|
||||
|
||||
pp.latitude += 10.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(3, ph.getConcisePoints().size());
|
||||
EXPECT_DOUBLE_EQ(23.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().front().latitude.value());
|
||||
|
||||
pp.latitude += 10.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(3, ph.getConcisePoints().size());
|
||||
|
||||
pp.latitude += 3.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(4, ph.getConcisePoints().size());
|
||||
EXPECT_DOUBLE_EQ(43.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().front().latitude.value());
|
||||
|
||||
pp.latitude += 23.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(5, ph.getConcisePoints().size());
|
||||
EXPECT_DOUBLE_EQ(46.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().front().latitude.value());
|
||||
}
|
||||
|
||||
TEST(PathHistory, concise_points_actual_error_threshold) {
|
||||
PathHistory ph;
|
||||
PathPoint pp;
|
||||
ph.addSample(pp);
|
||||
|
||||
pp.heading += units::Angle(5.0 * units::degrees);
|
||||
ph.addSample(pp);
|
||||
pp.latitude += 5.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(1, ph.getConcisePoints().size());
|
||||
|
||||
pp.latitude += 10.0 * units::degrees;
|
||||
pp.longitude += 10.0 * units::degrees;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(2, ph.getConcisePoints().size());
|
||||
}
|
||||
|
||||
TEST(PathHistory, concise_points_truncation) {
|
||||
PathHistory::Parameters params;
|
||||
params.retention_distance = 200.0 * units::si::meter;
|
||||
PathHistory ph(params);
|
||||
PathPoint pp;
|
||||
pp.latitude = 0.0 * units::degree;
|
||||
pp.longitude = 0.0 * units::degree;
|
||||
ph.addSample(pp);
|
||||
|
||||
pp.latitude += 25.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
pp.latitude += 25.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(2, ph.getConcisePoints().size());
|
||||
|
||||
pp.latitude += 25.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(3, ph.getConcisePoints().size());
|
||||
|
||||
pp.latitude += 205.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
ASSERT_EQ(4, ph.getConcisePoints().size());
|
||||
EXPECT_DOUBLE_EQ(0.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().back().latitude.value());
|
||||
EXPECT_DOUBLE_EQ(75.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().front().latitude.value());
|
||||
|
||||
ph.addSample(pp);
|
||||
ASSERT_EQ(2, ph.getConcisePoints().size());
|
||||
EXPECT_DOUBLE_EQ(75.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().back().latitude.value());
|
||||
EXPECT_DOUBLE_EQ(280.0 * cOneMeterLatitude.value(),
|
||||
ph.getConcisePoints().front().latitude.value());
|
||||
}
|
||||
|
||||
TEST(PathHistory, clear_resets) {
|
||||
PathHistory ph;
|
||||
PathPoint pp;
|
||||
pp.latitude = 0.0 * units::degrees;
|
||||
pp.longitude = 0.0 * units::degrees;
|
||||
for (unsigned i = 0; i < 5; ++i) {
|
||||
pp.latitude += 25.0 * cOneMeterLatitude;
|
||||
ph.addSample(pp);
|
||||
}
|
||||
ASSERT_GT(ph.getConcisePoints().size(), 1u);
|
||||
|
||||
ph.clear();
|
||||
EXPECT_EQ(0u, ph.getConcisePoints().size());
|
||||
EXPECT_PATHPOINT_EQ(PathPoint(), ph.getReferencePoint());
|
||||
|
||||
// usable again, no stale points carried across the reset
|
||||
ph.addSample(pp);
|
||||
EXPECT_EQ(1u, ph.getConcisePoints().size());
|
||||
}
|
||||
|
||||
TEST(PathHistory, custom_retention_keeps_more) {
|
||||
PathHistory::Parameters short_params;
|
||||
short_params.retention_distance = 200.0 * units::si::meter;
|
||||
PathHistory short_hist(short_params);
|
||||
|
||||
PathHistory::Parameters long_params;
|
||||
long_params.retention_distance = 500.0 * units::si::meter;
|
||||
PathHistory long_hist(long_params);
|
||||
|
||||
PathPoint pp;
|
||||
pp.latitude = 0.0 * units::degrees;
|
||||
pp.longitude = 0.0 * units::degrees;
|
||||
short_hist.addSample(pp);
|
||||
long_hist.addSample(pp);
|
||||
for (unsigned i = 0; i < 20; ++i) {
|
||||
pp.latitude += 25.0 * cOneMeterLatitude; // ~25 m steps, ~500 m total
|
||||
short_hist.addSample(pp);
|
||||
long_hist.addSample(pp);
|
||||
}
|
||||
EXPECT_GT(long_hist.getConcisePoints().size(), short_hist.getConcisePoints().size());
|
||||
}
|
||||
|
||||
TEST(PathHistory, concise_points_retrieval_limits) {
|
||||
PathHistory ph;
|
||||
PathPoint pp;
|
||||
pp.latitude = 0.0 * units::degrees;
|
||||
pp.longitude = 0.0 * units::degrees;
|
||||
ph.addSample(pp);
|
||||
for (unsigned i = 0; i < 6; ++i) {
|
||||
pp.latitude += 25.0 * cOneMeterLatitude; // beyond chord threshold: a concise point each
|
||||
ph.addSample(pp);
|
||||
}
|
||||
const std::list<PathPoint>& full = ph.getConcisePoints();
|
||||
ASSERT_GT(full.size(), 3u);
|
||||
|
||||
// concise points ~25 m apart, so at a 30 m threshold:
|
||||
// "covering" includes the point crossing 30 m, "within" excludes it
|
||||
const auto covering = ph.getConcisePointsMinLength(30.0 * units::si::meter);
|
||||
const auto within = ph.getConcisePointsMaxLength(30.0 * units::si::meter);
|
||||
EXPECT_EQ(3, std::distance(covering.begin(), covering.end()));
|
||||
EXPECT_EQ(2, std::distance(within.begin(), within.end()));
|
||||
EXPECT_DOUBLE_EQ(full.front().latitude.value(), covering.front().latitude.value());
|
||||
EXPECT_DOUBLE_EQ(full.front().latitude.value(), within.front().latitude.value());
|
||||
|
||||
// optional point limit keeps the newest points
|
||||
const auto capped = ph.getConcisePointsMinLength(10000.0 * units::si::meter, 2);
|
||||
EXPECT_EQ(2, std::distance(capped.begin(), capped.end()));
|
||||
EXPECT_DOUBLE_EQ(full.front().latitude.value(), capped.front().latitude.value());
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
#include <vanetza/facilities/path_point.hpp>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
namespace units = vanetza::units;
|
||||
using vanetza::facilities::PathPoint;
|
||||
|
||||
TEST(PathPoint, chord_length) {
|
||||
PathPoint a;
|
||||
a.longitude = 12.14094444 * units::degrees;
|
||||
a.latitude = 49.53858333 * units::degrees;
|
||||
|
||||
PathPoint b;
|
||||
b.longitude = 12.27191666 * units::degrees;
|
||||
b.latitude = 49.72394444 * units::degrees;
|
||||
|
||||
EXPECT_EQ(chord_length(a, b), chord_length(b, a));
|
||||
|
||||
units::Length length = chord_length(a, b);
|
||||
EXPECT_NEAR(22692.54, length / units::si::meters, 0.01);
|
||||
}
|
||||
Reference in New Issue
Block a user