Get the acceleration component of the PerceivedObject.
795 {
796
798
808
815inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
816 return cpm.payload.management_container.reference_time;
817}
818
825inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
826
833inline double getLatitude(
const CollectivePerceptionMessage &cpm) {
834 return getLatitude(cpm.payload.management_container.reference_position.latitude);
835}
836
843inline double getLongitude(
const CollectivePerceptionMessage &cpm) {
844 return getLongitude(cpm.payload.management_container.reference_position.longitude);
845}
846
853inline double getAltitude(
const CollectivePerceptionMessage &cpm) {
854 return getAltitude(cpm.payload.management_container.reference_position.altitude);
855}
856
868inline gm::PointStamped
getUTMPosition(
const CollectivePerceptionMessage &cpm,
int &zone,
bool &northp) {
869 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
870}
871
881inline gm::PointStamped
getUTMPosition(
const CollectivePerceptionMessage &cpm) {
882 int zone;
883 bool northp;
885}
886
896inline std::vector<uint8_t>
getCpmContainerIds(
const CollectivePerceptionMessage &cpm) {
897 std::vector<uint8_t> container_ids;
898 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
899 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
900 }
901 return container_ids;
902}
903
912inline WrappedCpmContainer
getCpmContainer(
const CollectivePerceptionMessage &cpm,
const uint8_t container_id) {
913 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
914 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
915 return cpm.payload.cpm_containers.value.array[i];
916 }
917 }
918 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
919}
920
930 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
931}
932
941 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
942 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
943 }
944 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
945 return number;
946}
947
956}
957
961
970inline PerceivedObject
getPerceivedObject(
const WrappedCpmContainer &container,
const uint8_t i) {
972 throw std::invalid_argument("Index out of range");
973 }
974 return container.container_data_perceived_object_container.perceived_objects.array[i];
975}
976
986
994 return coordinate.value.value;
995}
996
1004 return coordinate.confidence.value;
1005}
1006
1014 gm::Point point;
1017 if (object.position.z_coordinate_is_present) {
1019 }
1020 return point;
1021}
1022
1033 double z_confidence = object.position.z_coordinate_is_present
1035 : std::numeric_limits<double>::infinity();
1036 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1037}
1038
1045inline uint16_t
getCartesianAngle(
const CartesianAngle &angle) {
return angle.value.value; }
1046
1054
1066 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1067 tf2::Quaternion q;
1068 double roll{0}, pitch{0}, yaw{0};
1069
1070 if (object.angles.x_angle_is_present) {
1072 M_PI;
1073 }
1074 if (object.angles.y_angle_is_present) {
1075 pitch = (((double(
getCartesianAngle(
object.angles.y_angle)) / 10.0) ) / 180.0) *
1076 M_PI;
1077 }
1079 M_PI;
1080
1081 q.setRPY(roll, pitch, yaw);
1082
1083 return tf2::toMsg(q);
1084}
1085
1094 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1095 double roll, pitch, yaw;
1096 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1097 return yaw;
1098}
1099
1108 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1109 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1110 yaw_confidence *= M_PI / 180.0;
1111 return yaw_confidence;
1112}
1113
1121 gm::Pose pose;
1124 return pose;
1125}
1126
1135 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1136 return object.z_angular_velocity.value.value * M_PI / 180.0;
1137}
1138
1149 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1150 auto val = object.z_angular_velocity.confidence.value;
1151 static const std::map<uint8_t, double> confidence_map = {
1152 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1153 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1154 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1155 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1156 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1157 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1158 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1159 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1160 };
1161 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1162}
1163
1170inline double getVelocityComponent(
const VelocityComponent &velocity) {
return double(velocity.value.value) / 100.0; }
1171
1179 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1180}
1181
1190 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1191 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1192 throw std::invalid_argument("Velocity is not Cartesian");
1193 }
1194 gm::Vector3 velocity;
1197 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1199 }
1200 return velocity;
1201}
1202
1212 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1213 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1214 throw std::invalid_argument("Velocity is not Cartesian");
1215 }
1218 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1220 : std::numeric_limits<double>::infinity();
1221 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1222}
1223
1231 return double(acceleration.value.value) / 10.0;
1232}
1233
1241 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1242}
1243
1252 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1253 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1254 throw std::invalid_argument("Acceleration is not Cartesian");
1255 }
1256 gm::Vector3 acceleration;
1259 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1261 }
1262 return acceleration;
1263}
1264
1274 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1275 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1276 throw std::invalid_argument("Acceleration is not Cartesian");
1277 }
1280 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1282 : std::numeric_limits<double>::infinity();
1283 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1284}
1285
1298 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1299 return object.object_dimension_x.value.value;
1300}
1301
1310 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1311 return object.object_dimension_x.confidence.value;
1312}
1313
1326 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1327 return object.object_dimension_y.value.value;
1328}
1329
1338 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1339 return object.object_dimension_y.confidence.value;
1340}
1341
1354 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1355 return object.object_dimension_z.value.value;
1356}
1357
1366 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1367 return object.object_dimension_z.confidence.value;
1368}
1369
1380 gm::Vector3 dimensions;
1384 return dimensions;
1385}
1386
1397 return {conf_x, conf_y, conf_z};
1398}
1399
1411 const PerceivedObject &object) {
1412 gm::PointStamped utm_position;
1415
1416 utm_position.header.frame_id = reference_position.header.frame_id;
1417 utm_position.point.x = reference_position.point.x + relative_position.x;
1418 utm_position.point.y = reference_position.point.y + relative_position.y;
1419 utm_position.point.z = reference_position.point.z + relative_position.z;
1420
1421 return utm_position;
1422}
1423
1435}
1436
1443inline uint8_t
getSensorID(
const SensorInformation &sensor_information) {
1444 return sensor_information.sensor_id.value;
1445}
1446
1453inline uint8_t
getSensorType(
const SensorInformation &sensor_information) {
1454 return sensor_information.sensor_type.value;
1455}
1456
1457}
Getter functions for the ETSI ITS Common Data Dictionary (CDD) v2.1.1.
double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Yaw Rate Confidence Of Perceived Object.
std::tuple< double, double, double > getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Dimensions Confidence Of Perceived Object.
std::tuple< double, double, double > getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian Acceleration Confidence Of Perceived Object.
double getYawConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Yaw Confidence Of Perceived Object object.
int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate)
Retrieves the Cartesian coordinate value from a CartesianCoordinateWithConfidence object.
double getAltitude(const Altitude &altitude)
Get the Altitude value.
uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object)
Retrieves the z-dimension of a perceived object.
uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container)
gm::Point getPositionOfPerceivedObject(const PerceivedObject &object)
uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate)
Retrieves the confidence value from a CartesianCoordinateWithConfidence object.
uint8_t getCartesianAngleConfidence(const CartesianAngle &angle)
Get the confidence of the Cartesian angle.
uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object)
Retrieves the y-dimension of a perceived object.
double getVelocityComponent(const VelocityComponent &velocity)
Get the velocity component of the PerceivedObject.
WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm)
Retrieves the perceived object container from a CPM.
uint8_t getSensorType(const SensorInformation &sensor_information)
Get the sensorType of a SensorInformation object.
double getYawRateOfPerceivedObject(const PerceivedObject &object)
Get the yaw rate of the PerceivedObject.
gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object)
Retrieves the dimensions of a perceived object.
gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object)
Get the pose of the PerceivedObject.
double getYawOfPerceivedObject(const PerceivedObject &object)
Get the yaw of the PerceivedObject.
double getLongitude(const Longitude &longitude)
Get the Longitude value.
uint32_t getStationID(const ItsPduHeader &header)
Get the StationID of ItsPduHeader.
uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object)
Gets the confidence of the y-dimension of a perceived object.
gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object)
Calculates the UTM position of a perceived object based on the CPMs referencep position.
double getLatitude(const Latitude &latitude)
Get the Latitude value.
std::tuple< double, double, double > getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian Velocity Confidence Of Perceived Object object.
uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object)
Gets the confidence of the z-dimension of a perceived object.
uint16_t getCartesianAngle(const CartesianAngle &angle)
Get the Cartesian angle of the PerceivedObject.
double getVelocityComponentConfidence(const VelocityComponent &velocity)
Get the confidence of the velocity component.
std::array< double, 4 > getWGSPosConfidenceEllipse(const PosConfidenceEllipse &position_confidence_ellipse)
Get the covariance matrix of the position confidence ellipse.
uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object)
Gets the confidence of the x-dimension of a perceived object.
uint16_t getIdOfPerceivedObject(const PerceivedObject &object)
Retrieves the ID of a perceived object.
gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian velocity of the PerceivedObject.
uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object)
Gets the x-dimension of a perceived object.
uint8_t getSensorID(const SensorInformation &sensor_information)
Get the sensorId of a SensorInformation object.
std::vector< uint8_t > getCpmContainerIds(const CollectivePerceptionMessage &cpm)
Retrieves the container IDs from a CPM.
double getAccelerationComponentConfidence(const AccelerationComponent &acceleration)
Get the confidence of the acceleration component.
const std::array< double, 4 > getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm)
Get the confidence ellipse of the reference position as Covariance matrix.
WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id)
PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i)
Retrieves the PerceivedObject at the specified index from the given WrappedCpmContainer.
gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian acceleration of the PerceivedObject.
double getAccelerationComponent(const AccelerationComponent &acceleration)
Get the acceleration component of the PerceivedObject.
std::tuple< double, double, double > getPositionConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Position Confidences Of Perceived Object.
gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object)
Calculates the orientation of a perceived object.
gm::PointStamped getUTMPosition(const T &reference_position, int &zone, bool &northp)
Get the UTM Position defined by the given ReferencePosition.
1.13.2