test_cam_access.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <cmath>
3 
4 namespace cam_access = etsi_its_cam_msgs::access;
5 
6 TEST(etsi_its_cam_msgs, test_set_get_cam) {
7  cam_msgs::CAM cam;
8 
9  int station_id = randomInt(cam_msgs::StationID::MIN, cam_msgs::StationID::MAX);
10  int protocol_version =
11  randomInt(cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MIN, cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MAX);
12  cam_access::setItsPduHeader(cam, station_id, protocol_version);
13  EXPECT_EQ(cam_msgs::ItsPduHeader::MESSAGE_ID_CAM, cam.header.message_id);
14  EXPECT_EQ(protocol_version, cam.header.protocol_version);
15  EXPECT_EQ(station_id, cam_access::getStationID(cam));
16 
17  // https://www.etsi.org/deliver/etsi_ts/102800_102899/10289402/01.02.01_60/ts_10289402v010201p.pdf
18  // DE_TimestampITS
19  // The value for TimestampIts for 2007-01-01T00:00:00.000Z is
20  // 94694401000 milliseconds, which includes one leap second insertion
21  // since 2004-01-01T00:00:00.000Z.
22  uint64_t t_2007 = ((uint64_t)1167609600) * 1e9;
23  cam_msgs::TimestampIts t_its;
24  EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9));
25  cam_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9));
26  EXPECT_EQ(94694401000, t_its.value);
27  cam_access::setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9));
28  EXPECT_EQ(94694401000 % 65536, cam_access::getGenerationDeltaTimeValue(cam));
29  cam_msgs::TimestampIts t_its2;
30  uint64_t t_2007_off = t_2007 + 5 * 1e9;
31  cam_access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9));
32  EXPECT_EQ(94694401000,
33  cam_access::getTimestampITSFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_its2).value);
34  EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime(
35  cam_access::getGenerationDeltaTime(cam), t_its2,
36  etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)));
37  EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime(
38  cam_access::getGenerationDeltaTime(cam), t_2007_off,
39  etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)));
40 
41  int stationType_val = randomInt(cam_msgs::StationType::MIN, cam_msgs::StationType::MAX);
42  cam_access::setStationType(cam, stationType_val);
43  EXPECT_EQ(stationType_val, cam_access::getStationType(cam));
44 
45  double latitude = randomDouble(-90.0, 90.0);
46  double longitude = randomDouble(-180.0, 180.0);
47  cam_access::setReferencePosition(cam, latitude, longitude);
48  EXPECT_NEAR(latitude, cam_access::getLatitude(cam), 1e-7);
49  EXPECT_NEAR(longitude, cam_access::getLongitude(cam), 1e-7);
50  latitude = randomDouble(-90.0, 90.0);
51  longitude = randomDouble(-180.0, 180.0);
52  double altitude = randomDouble(-1000.0, 8000.0);
53  cam_access::setReferencePosition(cam, latitude, longitude, altitude);
54  EXPECT_NEAR(latitude, cam_access::getLatitude(cam), 1e-7);
55  EXPECT_NEAR(longitude, cam_access::getLongitude(cam), 1e-7);
56  EXPECT_NEAR(altitude, cam_access::getAltitude(cam), 1e-2);
57 
58  // Set specific position to test utm projection
59  latitude = 50.787467;
60  longitude = 6.046498;
61  altitude = 209.0;
62  cam_access::setReferencePosition(cam, latitude, longitude, altitude);
63  int zone;
64  bool northp;
65  gm::PointStamped utm = cam_access::getUTMPosition(cam, zone, northp);
66  EXPECT_NEAR(291827.02, utm.point.x, 1e-1);
67  EXPECT_NEAR(5630349.72, utm.point.y, 1e-1);
68  EXPECT_EQ(altitude, utm.point.z);
69  EXPECT_EQ(32, zone);
70  EXPECT_EQ(true, northp);
71  cam_access::setFromUTMPosition(cam, utm, zone, northp);
72  EXPECT_NEAR(latitude, cam_access::getLatitude(cam), 1e-7);
73  EXPECT_NEAR(longitude, cam_access::getLongitude(cam), 1e-7);
74  EXPECT_NEAR(altitude, cam_access::getAltitude(cam), 1e-2);
75 
76  double heading_val = randomDouble(0.0, 360.0);
77  double heading_conf = randomDouble(0.0, 6.25);
78  cam_access::setHeading(cam, heading_val, heading_conf);
79  EXPECT_NEAR(heading_val, cam_access::getHeading(cam), 1e-1);
80  EXPECT_NEAR(heading_conf, cam_access::getHeadingConfidence(cam), 1e-1);
81 
82  std::array<double, 4> covariance_matrix = {randomDouble(1.0, 100.0), 0.0,
83  0.0, randomDouble(1.0, 100.0)};
84  // Make y component larger than x component so the ellipse will have its major axis 90° rotated
85  covariance_matrix[3] += covariance_matrix[0];
86  cam_access::setRefPosConfidence(cam, covariance_matrix, heading_val * M_PI / 180.0);
87  EXPECT_NEAR(heading_val, cam_access::getHeading(cam), 1e-1);
88  EXPECT_NEAR(covariance_matrix[3], std::pow( cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse.semi_major_confidence.value*0.01 / etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR, 2), 1e-1);
89  EXPECT_NEAR(covariance_matrix[0], std::pow( cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse.semi_minor_confidence.value*0.01 / etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR, 2), 1e-1);
90  double expected_heading = heading_val + 90.0;
91  // Normalize to [0, 180)
92  expected_heading = std::fmod(expected_heading + 180, 180);
93  while (expected_heading < 0) {
94  expected_heading += 180;
95  }
96  while (expected_heading >= 180) {
97  expected_heading -= 180;
98  }
99  EXPECT_NEAR(expected_heading, cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse.semi_major_orientation.value/10.0, 1e-1);
100  auto cov_get = cam_access::getRefPosConfidence(cam);
101  EXPECT_NEAR(covariance_matrix[0], cov_get[0], 1e-1);
102  EXPECT_NEAR(covariance_matrix[1], cov_get[1], 1e-1);
103  EXPECT_NEAR(covariance_matrix[2], cov_get[2], 1e-1);
104  EXPECT_NEAR(covariance_matrix[3], cov_get[3], 1e-1);
105 
106  // Rotate the covariance matrix by a random angle
107  // and repeat the test
108  double phi = randomDouble(0.0, M_PI_2);
109  std::array<double, 4> covariance_matrix_rotated = {
110  covariance_matrix[0] * std::cos(phi) * std::cos(phi) +
111  covariance_matrix[3] * std::sin(phi) * std::sin(phi),
112  (covariance_matrix[0] - covariance_matrix[3]) * std::cos(phi) * std::sin(phi),
113  (covariance_matrix[0] - covariance_matrix[3]) * std::cos(phi) * std::sin(phi),
114  covariance_matrix[0] * std::sin(phi) * std::sin(phi) +
115  covariance_matrix[3] * std::cos(phi) * std::cos(phi)
116  };
117  cam_access::setRefPosConfidence(cam, covariance_matrix_rotated, heading_val * M_PI / 180.0);
118  EXPECT_NEAR(heading_val, cam_access::getHeading(cam), 1e-1);
119  EXPECT_NEAR(covariance_matrix[3], std::pow( cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse.semi_major_confidence.value*0.01 / etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR, 2), 1e-1);
120  EXPECT_NEAR(covariance_matrix[0], std::pow( cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse.semi_minor_confidence.value*0.01 / etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR, 2), 1e-1);
121  expected_heading = heading_val - phi * 180.0 / M_PI + 90.0;
122  // Normalize to [0, 180)
123  expected_heading = std::fmod(expected_heading + 180, 180);
124  while (expected_heading < 0) {
125  expected_heading += 180;
126  }
127  while (expected_heading >= 180) {
128  expected_heading -= 180;
129  }
130  EXPECT_NEAR(expected_heading, cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse.semi_major_orientation.value/10.0, 1e-1);
131  auto cov_get_rotated = cam_access::getRefPosConfidence(cam);
132  EXPECT_NEAR(covariance_matrix_rotated[0], cov_get_rotated[0], 1e-1);
133  EXPECT_NEAR(covariance_matrix_rotated[1], cov_get_rotated[1], 1e-1);
134  EXPECT_NEAR(covariance_matrix_rotated[2], cov_get_rotated[2], 1e-1);
135  EXPECT_NEAR(covariance_matrix_rotated[3], cov_get_rotated[3], 1e-1);
136 
137  // Set WGS84 confidence ellipse
138  cam_access::setWGSRefPosConfidence(cam, covariance_matrix_rotated);
139  cov_get_rotated = cam_access::getWGSRefPosConfidence(cam);
140  EXPECT_NEAR(covariance_matrix_rotated[0], cov_get_rotated[0], 1e-1);
141  EXPECT_NEAR(covariance_matrix_rotated[1], cov_get_rotated[1], 1e-1);
142  EXPECT_NEAR(covariance_matrix_rotated[2], cov_get_rotated[2], 1e-1);
143  EXPECT_NEAR(covariance_matrix_rotated[3], cov_get_rotated[3], 1e-1);
144 
145  double length = randomDouble(0.0, 102.2);
146  double width = randomDouble(0.0, 6.2);
147  cam_access::setVehicleDimensions(cam, length, width);
148  EXPECT_NEAR(length, cam_access::getVehicleLength(cam), 1e-1);
149  EXPECT_NEAR(width, cam_access::getVehicleWidth(cam), 1e-1);
150 
151  double speed_val = randomDouble(0.0, 163.82);
152  double speed_conf = randomDouble(0.0, 0.625);
153  cam_access::setSpeed(cam, speed_val, speed_conf);
154  EXPECT_NEAR(speed_val, cam_access::getSpeed(cam), 1e-2);
155  EXPECT_NEAR(speed_conf, cam_access::getSpeedConfidence(cam), 1e-2);
156 
157  double lon_accel = randomDouble(-16.0, 16.0);
158  double lat_accel = randomDouble(-16.0, 16.0);
159  double lon_accel_conf = randomDouble(0.0, 5.0);
160  double lat_accel_conf = randomDouble(0.0, 5.0);
161  cam_access::setLongitudinalAcceleration(cam, lon_accel, lon_accel_conf);
162  EXPECT_NEAR(lon_accel, cam_access::getLongitudinalAcceleration(cam), 1e-1);
163  EXPECT_NEAR(lon_accel_conf, cam_access::getLongitudinalAccelerationConfidence(cam), 1e-1);
164  cam_access::setLateralAcceleration(cam, lat_accel, lat_accel_conf);
165  EXPECT_NEAR(lat_accel, cam_access::getLateralAcceleration(cam), 1e-1);
166  EXPECT_NEAR(lat_accel_conf, cam_access::getLateralAccelerationConfidence(cam), 1e-1);
167 
168  std::vector<bool> exterior_lights(cam_msgs::ExteriorLights::SIZE_BITS);
169  for (int i = 0; i < cam_msgs::ExteriorLights::SIZE_BITS; i++) {
170  exterior_lights.at(i) = randomInt(0, 1);
171  }
172  cam.cam.cam_parameters.low_frequency_container_is_present = true;
173  cam.cam.cam_parameters.low_frequency_container.choice =
174  cam_msgs::LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY;
175  cam_access::setExteriorLights(cam, exterior_lights);
176  EXPECT_EQ(exterior_lights, cam_access::getExteriorLights(cam));
177 }
getGenerationDeltaTimeValue
uint16_t getGenerationDeltaTimeValue(const CAM &cam)
Get the GenerationDeltaTime-Value.
Definition: cam_getters_common.h:59
getLatitude
double getLatitude(const CAM &cam)
Get the Latitude value of CAM.
Definition: cam_getters_common.h:75
getVehicleLength
double getVehicleLength(const VehicleLength &vehicle_length)
Get the Vehicle Length.
Definition: cam_getters_common.h:128
setExteriorLights
void setExteriorLights(ExteriorLights &exterior_lights, const std::vector< bool > &bits)
Set the Exterior Lights by a vector of bools.
Definition: cam_setters_common.h:224
getStationType
uint8_t getStationType(const CAM &cam)
Get the stationType object.
Definition: cam_getters_common.h:67
getLongitude
double getLongitude(const CAM &cam)
Get the Longitude value of CAM.
Definition: cam_getters_common.h:85
getStationID
uint32_t getStationID(const CAM &cam)
Get the Station ID object.
Definition: cam_getters_common.h:43
etsi_its_cam_msgs::access::getLateralAccelerationConfidence
double getLateralAccelerationConfidence(const LateralAcceleration &lateral_acceleration)
Get the Lateral Acceleration Confidence.
Definition: cam_getters.h:73
setReferencePosition
void setReferencePosition(CAM &cam, const double latitude, const double longitude, const double altitude=AltitudeValue::UNAVAILABLE)
Set the ReferencePosition for a CAM.
Definition: cam_setters_common.h:197
getExteriorLights
std::vector< bool > getExteriorLights(const ExteriorLights &exterior_lights)
Get the Exterior Lights in form of bool vector.
Definition: cam_getters_common.h:273
randomInt
int randomInt(int min, int max)
Definition: test_access.cpp:28
etsi_its_cam_msgs::access::getLongitudinalAccelerationConfidence
double getLongitudinalAccelerationConfidence(const LongitudinalAcceleration &longitudinal_acceleration)
Get the Longitudinal Acceleration Confidence.
Definition: cam_getters.h:53
etsi_its_cam_msgs::access::setLongitudinalAcceleration
void setLongitudinalAcceleration(LongitudinalAcceleration &accel, const double value, const double confidence=std::numeric_limits< double >::infinity())
Set the LongitudinalAcceleration object.
Definition: cam_setters.h:76
etsi_its_cam_msgs
Definition: cam_access.h:38
etsi_its_cam_msgs::access::getRefPosConfidence
const std::array< double, 4 > getRefPosConfidence(const CAM &cam)
Get the confidence ellipse of the reference position as Covariance matrix.
Definition: cam_getters.h:88
etsi_its_cam_msgs::access::setLateralAcceleration
void setLateralAcceleration(LateralAcceleration &accel, const double value, const double confidence=std::numeric_limits< double >::infinity())
Set the LateralAcceleration object.
Definition: cam_setters.h:108
getAltitude
double getAltitude(const CAM &cam)
Get the Altitude value of CAM.
Definition: cam_getters_common.h:95
getUnixNanosecondsFromGenerationDeltaTime
uint64_t getUnixNanosecondsFromGenerationDeltaTime(const GenerationDeltaTime &generation_delta_time, const TimestampIts &timestamp_estimate, const uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin() ->second)
Get the Unix-Nanoseconds from a given GenerationDeltaTime object.
Definition: cam_utils.h:61
setVehicleDimensions
void setVehicleDimensions(CAM &cam, const double vehicle_length, const double vehicle_width)
Set the vehicle dimensions.
Definition: cam_setters_common.h:138
setHeading
void setHeading(CAM &cam, const double heading_val, const double confidence=std::numeric_limits< double >::infinity())
Set the Heading for a CAM.
Definition: cam_setters_common.h:89
getUTMPosition
gm::PointStamped getUTMPosition(const CAM &cam, int &zone, bool &northp)
Get the UTM Position defined within the BasicContainer of the CAM.
Definition: cam_getters_common.h:248
getHeading
double getHeading(const CAM &cam)
Get the Heading value of CAM.
Definition: cam_getters_common.h:107
setGenerationDeltaTime
void setGenerationDeltaTime(GenerationDeltaTime &generation_delta_time, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin() ->second)
Set the GenerationDeltaTime-Value.
Definition: cam_setters_common.h:46
getTimestampITSFromGenerationDeltaTime
TimestampIts getTimestampITSFromGenerationDeltaTime(const GenerationDeltaTime &generation_delta_time, const TimestampIts &timestamp_estimate)
Get the TimestampITS from a given GenerationDeltaTime object.
Definition: cam_utils.h:45
getVehicleWidth
double getVehicleWidth(const VehicleWidth &vehicle_width)
Get the Vehicle Width.
Definition: cam_getters_common.h:149
etsi_its_msgs::getLeapSecondInsertionsSince2004
uint16_t getLeapSecondInsertionsSince2004(const uint64_t unix_seconds)
Get the leap second insertions since 2004 for given unix seconds.
Definition: constants.h:60
TEST
TEST(etsi_its_cam_msgs, test_set_get_cam)
Definition: test_cam_access.cpp:6
etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR
constexpr const double TWO_D_GAUSSIAN_FACTOR
Definition: constants.h:75
etsi_its_cam_msgs::access::setWGSRefPosConfidence
void setWGSRefPosConfidence(CAM &cam, const std::array< double, 4 > &covariance_matrix)
Set the confidence of the reference position.
Definition: cam_setters.h:148
setFromUTMPosition
void setFromUTMPosition(CAM &cam, const gm::PointStamped &utm_position, const int &zone, const bool &northp)
Set the ReferencePosition of a CAM from a given UTM-Position.
Definition: cam_setters_common.h:214
randomDouble
double randomDouble(double min, double max)
Definition: test_access.cpp:24
etsi_its_cam_msgs::access::getLongitudinalAcceleration
double getLongitudinalAcceleration(const LongitudinalAcceleration &longitudinal_acceleration)
Get the longitudinal acceleration.
Definition: cam_getters.h:43
etsi_its_cam_msgs::access::getWGSRefPosConfidence
const std::array< double, 4 > getWGSRefPosConfidence(const CAM &cam)
Get the confidence ellipse of the reference position as Covariance matrix.
Definition: cam_getters.h:102
etsi_its_cam_msgs::access
Definition: impl/cam/cam_access.h:57
getHeadingConfidence
double getHeadingConfidence(const CAM &cam)
Get the Heading confidence of CAM.
Definition: cam_getters_common.h:118
setTimestampITS
void setTimestampITS(TimestampIts &timestamp_its, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin() ->second)
Set the TimestampITS object.
Definition: cdd_setters_common.h:52
length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
etsi_its_cam_msgs::access::getLateralAcceleration
double getLateralAcceleration(const LateralAcceleration &lateral_acceleration)
Get the lateral acceleration.
Definition: cam_getters.h:63
etsi_its_cam_msgs::access::setRefPosConfidence
void setRefPosConfidence(CAM &cam, const std::array< double, 4 > &covariance_matrix, const double object_heading)
Set the confidence of the reference position.
Definition: cam_setters.h:124
setSpeed
void setSpeed(CAM &cam, const double speed_val, const double confidence=SpeedConfidence::UNAVAILABLE)
Set the vehicle speed.
Definition: cam_setters_common.h:152
etsi_its_cam_msgs::access::setItsPduHeader
void setItsPduHeader(CAM &cam, const uint32_t station_id, const uint8_t protocol_version=0)
Set the ItsPduHeader-object for a CAM.
Definition: cam_setters.h:45
setStationType
void setStationType(CAM &cam, const uint8_t value)
Set the StationType for a CAM.
Definition: cam_setters_common.h:75
getSpeed
double getSpeed(const CAM &cam)
Get the vehicle speed.
Definition: cam_getters_common.h:168
getSpeedConfidence
double getSpeedConfidence(const CAM &cam)
Get the Speed Confidence.
Definition: cam_getters_common.h:178
getGenerationDeltaTime
GenerationDeltaTime getGenerationDeltaTime(const CAM &cam)
Get the GenerationDeltaTime.
Definition: cam_getters_common.h:51

etsi_its_msgs_utils
Author(s): Jean-Pierre Busch , Guido Küppers , Lennart Reiher
autogenerated on Sun May 18 2025 02:32:12