1 #include <gtest/gtest.h>
7 cpm_ts_msgs::CollectivePerceptionMessage cpm;
9 int station_id =
randomInt(cpm_ts_msgs::StationId::MIN, cpm_ts_msgs::StationId::MAX);
10 int protocol_version =
randomInt(cpm_ts_msgs::OrdinalNumber1B::MIN, cpm_ts_msgs::OrdinalNumber1B::MAX);
12 EXPECT_EQ(cpm_ts_msgs::MessageId::CPM, cpm.header.message_id.value);
13 EXPECT_EQ(protocol_version, cpm.header.protocol_version.value);
22 uint64_t t_2007 = ((uint64_t)1167609600) * 1e9;
23 cpm_ts_msgs::TimestampIts t_its;
26 EXPECT_EQ(94694401000, t_its.value);
45 std::array<double, 4> covariance_matrix = {
randomDouble(1.0, 100.0), 0.0,
48 std::array<double, 4> covariance_matrix_rotated = {
49 covariance_matrix[0] * std::cos(phi) * std::cos(phi) +
50 covariance_matrix[3] * std::sin(phi) * std::sin(phi),
51 (covariance_matrix[0] - covariance_matrix[3]) * std::cos(phi) * std::sin(phi),
52 (covariance_matrix[0] - covariance_matrix[3]) * std::cos(phi) * std::sin(phi),
53 covariance_matrix[0] * std::sin(phi) * std::sin(phi) +
54 covariance_matrix[3] * std::cos(phi) * std::cos(phi)
58 EXPECT_NEAR(covariance_matrix_rotated[0], cov_get_rotated[0], 1e-1);
59 EXPECT_NEAR(covariance_matrix_rotated[1], cov_get_rotated[1], 1e-1);
60 EXPECT_NEAR(covariance_matrix_rotated[2], cov_get_rotated[2], 1e-1);
61 EXPECT_NEAR(covariance_matrix_rotated[3], cov_get_rotated[3], 1e-1);
71 EXPECT_NEAR(291827.02, utm.point.x, 1e-1);
72 EXPECT_NEAR(5630349.72, utm.point.y, 1e-1);
73 EXPECT_EQ(altitude, utm.point.z);
75 EXPECT_EQ(
true, northp);
83 cpm_ts_msgs::PerceivedObject object;
95 EXPECT_NEAR(utm.point.x, point.point.x, 1e-1);
96 EXPECT_NEAR(utm.point.y, point.point.y, 1e-1);
97 EXPECT_NEAR(utm.point.z, point.point.z, 1e-1);
99 EXPECT_NEAR(dx, dpoint.x, 1e-1);
100 EXPECT_NEAR(dy, dpoint.y, 1e-1);
101 EXPECT_NEAR(dz, dpoint.z, 1e-1);
103 EXPECT_NEAR(var_x, var_x_get, 1e-2);
104 EXPECT_NEAR(var_y, var_y_get, 1e-2);
105 EXPECT_NEAR(var_z, var_z_get, 1e-2);
108 gm::Vector3 dimensions;
120 EXPECT_NEAR(var_x, var_x_get, 1e-1);
121 EXPECT_NEAR(var_y, var_y_get, 1e-1);
122 EXPECT_NEAR(var_z, var_z_get, 1e-1);
136 std::array<double, 6> yaw_std_possible_values{1.0, 2.0, 5.0, 10.0, 20.0, 50.0};
137 std::for_each(yaw_std_possible_values.begin(), yaw_std_possible_values.end(),
138 [](
double& val) { val *= 0.5 * M_PI / 180.0; });
139 double expected_yaw_rate = *std::lower_bound(yaw_std_possible_values.begin(), yaw_std_possible_values.end(), yaw_rate_std);
143 gm::Vector3 velocity;
155 EXPECT_NEAR(velocity_x_std, vel_x_std_get, 1e-2);
156 EXPECT_NEAR(velocity_y_std, vel_y_std_get, 1e-2);
157 EXPECT_NEAR(velocity_z_std, vel_z_std_get, 1e-2);
160 gm::Vector3 acceleration;
172 EXPECT_NEAR(acceleration_x_std, acc_x_std_get, 1e-1);
173 EXPECT_NEAR(acceleration_y_std, acc_y_std_get, 1e-1);
174 EXPECT_NEAR(acceleration_z_std, acc_z_std_get, 1e-1);
177 cpm_ts_msgs::WrappedCpmContainer sensor_information_container;
179 int sensor_id =
randomInt(cpm_ts_msgs::Identifier1B::MIN, cpm_ts_msgs::Identifier1B::MAX);
180 int sensor_type =
randomInt(cpm_ts_msgs::SensorType::MIN, cpm_ts_msgs::SensorType::MAX);
181 cpm_ts_msgs::SensorInformation sensor_information;