etsi_its_messages v3.3.0
 
Loading...
Searching...
No Matches
Macros | Functions
cpm_ts_getters.h File Reference

Getter functions for the ETSI ITS CPM (TS) More...

#include <tuple>
#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>

Go to the source code of this file.

Functions

uint32_t etsi_its_cpm_ts_msgs::access::getStationID (const ItsPduHeader &header)
 Get the StationID of ItsPduHeader.
 
double etsi_its_cpm_ts_msgs::access::getLatitude (const Latitude &latitude)
 Get the Latitude value.
 
double etsi_its_cpm_ts_msgs::access::getLongitude (const Longitude &longitude)
 Get the Longitude value.
 
double etsi_its_cpm_ts_msgs::access::getAltitude (const Altitude &altitude)
 Get the Altitude value.
 
double etsi_its_cpm_ts_msgs::access::getSpeed (const Speed &speed)
 Get the vehicle speed.
 
double etsi_its_cpm_ts_msgs::access::getSpeedConfidence (const Speed &speed)
 Get the Speed Confidence.
 
template<typename AccelerationMagnitude>
double etsi_its_cpm_ts_msgs::access::getAccelerationMagnitude (const AccelerationMagnitude &acceleration_magnitude)
 Get the AccelerationMagnitude value.
 
template<typename AccelerationMagnitude>
double etsi_its_cpm_ts_msgs::access::getAccelerationMagnitudeConfidence (const AccelerationMagnitude &acceleration_magnitude)
 Get the AccelerationMagnitude Confidence.
 
template<typename T>
gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPosition (const T &reference_position, int &zone, bool &northp)
 Get the UTM Position defined by the given ReferencePosition.
 
template<typename Heading>
double etsi_its_cpm_ts_msgs::access::getHeadingCDD (const Heading &heading)
 Get the Heading value.
 
template<typename Heading>
double etsi_its_cpm_ts_msgs::access::getHeadingConfidenceCDD (const Heading &heading)
 Get the Heading value.
 
template<typename YawRate>
double etsi_its_cpm_ts_msgs::access::getYawRateCDD (const YawRate &yaw_rate)
 Get the Yaw Rate value.
 
template<typename YawRate, typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
double etsi_its_cpm_ts_msgs::access::getYawRateConfidenceCDD (const YawRate &yaw_rate)
 Get the Yaw Rate Confidence.
 
template<typename SemiAxisLength>
double etsi_its_cpm_ts_msgs::access::getSemiAxis (const SemiAxisLength &semi_axis_length)
 Get the Semi Axis object.
 
template<typename PosConfidenceEllipse>
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getPosConfidenceEllipse (const PosConfidenceEllipse &position_confidence_ellipse)
 Extract major axis length, minor axis length and orientation from the given position confidence ellipse.
 
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::CovMatrixFromConfidenceEllipse (double semi_major, double semi_minor, double major_orientation, const double object_heading)
 Convert the confidence ellipse to a covariance matrix.
 
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::WGSCovMatrixFromConfidenceEllipse (double semi_major, double semi_minor, double major_orientation)
 Convert the confidence ellipse to a covariance matrix.
 
template<typename PosConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getPosConfidenceEllipse (const PosConfidenceEllipse &position_confidence_ellipse, const double object_heading)
 Get the covariance matrix of the position confidence ellipse.
 
template<typename PosConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getWGSPosConfidenceEllipse (const PosConfidenceEllipse &position_confidence_ellipse)
 Get the covariance matrix of the position confidence ellipse.
 
double etsi_its_cpm_ts_msgs::access::getLongitudinalAcceleration (const AccelerationComponent &longitudinal_acceleration)
 Get the longitudinal acceleration.
 
double etsi_its_cpm_ts_msgs::access::getLongitudinalAccelerationConfidence (const AccelerationComponent &longitudinal_acceleration)
 Get the Longitudinal Acceleration Confidence.
 
double etsi_its_cpm_ts_msgs::access::getLateralAcceleration (const AccelerationComponent &lateral_acceleration)
 Get the lateral acceleration.
 
double etsi_its_cpm_ts_msgs::access::getLateralAccelerationConfidence (const AccelerationComponent &lateral_acceleration)
 Get the Lateral Acceleration Confidence.
 
template<typename PositionConfidenceEllipse>
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getPositionConfidenceEllipse (PositionConfidenceEllipse &position_confidence_ellipse)
 Extract major axis length, minor axis length and orientation from the given position confidence ellipse.
 
template<typename PositionConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getPositionConfidenceEllipse (const PositionConfidenceEllipse &position_confidence_ellipse, const double object_heading)
 Get the covariance matrix of the position confidence ellipse.
 
template<typename PositionConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getWGSPositionConfidenceEllipse (const PositionConfidenceEllipse &position_confidence_ellipse)
 Get the covariance matrix of the position confidence ellipse.
 
uint32_t etsi_its_cpm_ts_msgs::access::getStationID (const CollectivePerceptionMessage &cpm)
 Retrieves the station ID from the given CPM.
 
TimestampIts etsi_its_cpm_ts_msgs::access::getReferenceTime (const CollectivePerceptionMessage &cpm)
 Get the Reference Time object.
 
uint64_t etsi_its_cpm_ts_msgs::access::getReferenceTimeValue (const CollectivePerceptionMessage &cpm)
 Get the ReferenceTime-Value.
 
double etsi_its_cpm_ts_msgs::access::getLatitude (const CollectivePerceptionMessage &cpm)
 Get the Latitude value of CPM.
 
double etsi_its_cpm_ts_msgs::access::getLongitude (const CollectivePerceptionMessage &cpm)
 Get the Longitude value of CPM.
 
double etsi_its_cpm_ts_msgs::access::getAltitude (const CollectivePerceptionMessage &cpm)
 Get the Altitude value of CPM.
 
gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPosition (const CollectivePerceptionMessage &cpm, int &zone, bool &northp)
 Get the UTM Position defined within the ManagementContainer of the CPM.
 
gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPosition (const CollectivePerceptionMessage &cpm)
 Get the UTM Position defined within the ManagementContainer of the CPM.
 
std::vector< uint8_t > etsi_its_cpm_ts_msgs::access::getCpmContainerIds (const CollectivePerceptionMessage &cpm)
 Retrieves the container IDs from a CPM.
 
WrappedCpmContainer etsi_its_cpm_ts_msgs::access::getCpmContainer (const CollectivePerceptionMessage &cpm, const uint8_t container_id)
 
WrappedCpmContainer etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer (const CollectivePerceptionMessage &cpm)
 Retrieves the perceived object container from a CPM.
 
uint8_t etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects (const WrappedCpmContainer &container)
 
uint8_t etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects (const CollectivePerceptionMessage &cpm)
 
PerceivedObject etsi_its_cpm_ts_msgs::access::getPerceivedObject (const WrappedCpmContainer &container, const uint8_t i)
 Retrieves the PerceivedObject at the specified index from the given WrappedCpmContainer.
 
uint16_t etsi_its_cpm_ts_msgs::access::getIdOfPerceivedObject (const PerceivedObject &object)
 Retrieves the ID of a perceived object.
 
int32_t etsi_its_cpm_ts_msgs::access::getCartesianCoordinate (const CartesianCoordinateWithConfidence &coordinate)
 Retrieves the Cartesian coordinate value from a CartesianCoordinateWithConfidence object.
 
uint16_t etsi_its_cpm_ts_msgs::access::getCartesianCoordinateConfidence (const CartesianCoordinateWithConfidence &coordinate)
 Retrieves the confidence value from a CartesianCoordinateWithConfidence object.
 
gm::Point etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject (const PerceivedObject &object)
 
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getPositionConfidenceOfPerceivedObject (const PerceivedObject &object)
 Get the Position Confidences Of Perceived Object.
 
uint16_t etsi_its_cpm_ts_msgs::access::getCartesianAngle (const CartesianAngle &angle)
 Get the Cartesian angle of the PerceivedObject.
 
uint8_t etsi_its_cpm_ts_msgs::access::getCartesianAngleConfidence (const CartesianAngle &angle)
 Get the confidence of the Cartesian angle.
 
gm::Quaternion etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject (const PerceivedObject &object)
 Calculates the orientation of a perceived object.
 
double etsi_its_cpm_ts_msgs::access::getYawOfPerceivedObject (const PerceivedObject &object)
 Get the yaw of the PerceivedObject.
 
double etsi_its_cpm_ts_msgs::access::getYawConfidenceOfPerceivedObject (const PerceivedObject &object)
 Get the Yaw Confidence Of Perceived Object object.
 
gm::Pose etsi_its_cpm_ts_msgs::access::getPoseOfPerceivedObject (const PerceivedObject &object)
 Get the pose of the PerceivedObject.
 
double etsi_its_cpm_ts_msgs::access::getYawRateOfPerceivedObject (const PerceivedObject &object)
 Get the yaw rate of the PerceivedObject.
 
double etsi_its_cpm_ts_msgs::access::getYawRateConfidenceOfPerceivedObject (const PerceivedObject &object)
 Get the Yaw Rate Confidence Of Perceived Object.
 
double etsi_its_cpm_ts_msgs::access::getVelocityComponent (const VelocityComponent &velocity)
 Get the velocity component of the PerceivedObject.
 
double etsi_its_cpm_ts_msgs::access::getVelocityComponentConfidence (const VelocityComponent &velocity)
 Get the confidence of the velocity component.
 
gm::Vector3 etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject (const PerceivedObject &object)
 Get the Cartesian velocity of the PerceivedObject.
 
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getCartesianVelocityConfidenceOfPerceivedObject (const PerceivedObject &object)
 Get the Cartesian Velocity Confidence Of Perceived Object object.
 
double etsi_its_cpm_ts_msgs::access::getAccelerationComponent (const AccelerationComponent &acceleration)
 Get the acceleration component of the PerceivedObject.
 
double etsi_its_cpm_ts_msgs::access::getAccelerationComponentConfidence (const AccelerationComponent &acceleration)
 Get the confidence of the acceleration component.
 
gm::Vector3 etsi_its_cpm_ts_msgs::access::getCartesianAccelerationOfPerceivedObject (const PerceivedObject &object)
 Get the Cartesian acceleration of the PerceivedObject.
 
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getCartesianAccelerationConfidenceOfPerceivedObject (const PerceivedObject &object)
 Get the Cartesian Acceleration Confidence Of Perceived Object.
 
uint16_t etsi_its_cpm_ts_msgs::access::getXDimensionOfPerceivedObject (const PerceivedObject &object)
 Gets the x-dimension of a perceived object.
 
uint8_t etsi_its_cpm_ts_msgs::access::getXDimensionConfidenceOfPerceivedObject (const PerceivedObject &object)
 Gets the confidence of the x-dimension of a perceived object.
 
uint16_t etsi_its_cpm_ts_msgs::access::getYDimensionOfPerceivedObject (const PerceivedObject &object)
 Retrieves the y-dimension of a perceived object.
 
uint8_t etsi_its_cpm_ts_msgs::access::getYDimensionConfidenceOfPerceivedObject (const PerceivedObject &object)
 Gets the confidence of the y-dimension of a perceived object.
 
uint16_t etsi_its_cpm_ts_msgs::access::getZDimensionOfPerceivedObject (const PerceivedObject &object)
 Retrieves the z-dimension of a perceived object.
 
uint8_t etsi_its_cpm_ts_msgs::access::getZDimensionConfidenceOfPerceivedObject (const PerceivedObject &object)
 Gets the confidence of the z-dimension of a perceived object.
 
gm::Vector3 etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject (const PerceivedObject &object)
 Retrieves the dimensions of a perceived object.
 
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getDimensionsConfidenceOfPerceivedObject (const PerceivedObject &object)
 Get the Dimensions Confidence Of Perceived Object.
 
gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject (const CollectivePerceptionMessage &cpm, const PerceivedObject &object)
 Calculates the UTM position of a perceived object based on the CPMs referencep position.
 
const std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getWGSRefPosConfidence (const CollectivePerceptionMessage &cpm)
 Get the confidence ellipse of the reference position as Covariance matrix.
 
uint8_t etsi_its_cpm_ts_msgs::access::getSensorID (const SensorInformation &sensor_information)
 Get the sensorId of a SensorInformation object.
 
uint8_t etsi_its_cpm_ts_msgs::access::getSensorType (const SensorInformation &sensor_information)
 Get the sensorType of a SensorInformation object.
 

Detailed Description

Getter functions for the ETSI ITS CPM (TS)

Definition in file cpm_ts_getters.h.

Function Documentation

◆ CovMatrixFromConfidenceEllipse()

std::array< double, 4 > etsi_its_cpm_ts_msgs::access::CovMatrixFromConfidenceEllipse ( double semi_major,
double semi_minor,
double major_orientation,
const double object_heading )
inline

Convert the confidence ellipse to a covariance matrix.

Note that the major_orientation is given in degrees, while the object_heading is given in radians!

Parameters
semi_majorSemi major axis length in meters
semi_minorSemi minor axis length in meters
major_orientationOrientation of the major axis in degrees, relative to WGS84
object_headingobject heading in radians, relative to WGS84
Returns
std::array<double, 4> The covariance matrix in vehicle coordinates (x = longitudinal, y = lateral)

Definition at line 244 of file cpm_ts_getters.h.

249 {
250 return coordinate.confidence.value;
251}
252
259inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
260 gm::Point point;
261 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;

◆ getAccelerationComponent()

double etsi_its_cpm_ts_msgs::access::getAccelerationComponent ( const AccelerationComponent & acceleration)
inline

Get the acceleration component of the PerceivedObject.

Parameters
accelerationAccelerationComponent to get the value from
Returns
double value of the acceleration component in m/s^2

Definition at line 857 of file cpm_ts_getters.h.

892 {
893
894#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
895
904inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
905
912inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
913 return cpm.payload.management_container.reference_time;
914}
915
922inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
923
930inline double getLatitude(const CollectivePerceptionMessage &cpm) {
931 return getLatitude(cpm.payload.management_container.reference_position.latitude);
932}
933
940inline double getLongitude(const CollectivePerceptionMessage &cpm) {
941 return getLongitude(cpm.payload.management_container.reference_position.longitude);
942}
943
950inline double getAltitude(const CollectivePerceptionMessage &cpm) {
951 return getAltitude(cpm.payload.management_container.reference_position.altitude);
952}
953
965inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
966 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
967}
968
978inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
979 int zone;
980 bool northp;
981 return getUTMPosition(cpm, zone, northp);
982}
983
993inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
994 std::vector<uint8_t> container_ids;
995 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
996 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
997 }
998 return container_ids;
999}
1000
1009inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1010 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1011 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1012 return cpm.payload.cpm_containers.value.array[i];
1013 }
1014 }
1015 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1016}
1017
1026inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1027 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1028}
1029
1037inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1038 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1039 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1040 }
1041 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1042 return number;
1043}
1044
1051inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1052 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1053}
1054
1058
1067inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1068 if (i >= getNumberOfPerceivedObjects(container)) {
1069 throw std::invalid_argument("Index out of range");
1070 }
1071 return container.container_data_perceived_object_container.perceived_objects.array[i];
1072}
1073
1082inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1083 if (!object.object_id_is_present) {
1084 throw std::invalid_argument("No object_id present in PerceivedObject");
1085 }
1086 return object.object_id.value;
1087}
1088
1095inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1096 return coordinate.value.value;
1097}
1098
1105inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1106 return coordinate.confidence.value;
1107}
1108
1115inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1116 gm::Point point;
1117 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1118 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1119 if (object.position.z_coordinate_is_present) {
1120 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1121 }
1122 return point;
1123}
1124
1132inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1133 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1134 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1135 double z_confidence = object.position.z_coordinate_is_present
1136 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1137 : std::numeric_limits<double>::infinity();
1138 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1139}
1140
1147inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1148
1155inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1156
1167inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1168 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1169 tf2::Quaternion q;
1170 double roll{0}, pitch{0}, yaw{0};
1171
1172 if (object.angles.x_angle_is_present) {
1173 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1174 M_PI;
1175 }
1176 if (object.angles.y_angle_is_present) {
1177 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1178 M_PI;
1179 }
1180 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1181 M_PI;
1182 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1183 q.setRPY(roll, pitch, yaw);
1184
1185 return tf2::toMsg(q);
1186}
1187
1194inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1195 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1196 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1197 double roll, pitch, yaw;
1198 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1199 return yaw;
1200}
1201
1209inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1210 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1211 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1212 yaw_confidence *= M_PI / 180.0; // convert to radians
1213 return yaw_confidence;
1214}
1215
1222inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1223 gm::Pose pose;
1224 pose.position = getPositionOfPerceivedObject(object);
1225 pose.orientation = getOrientationOfPerceivedObject(object);
1226 return pose;
1227}
1228
1236inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1237 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1238 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1239}
1240
1250inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1251 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1252 auto val = object.z_angular_velocity.confidence.value;
1253 static const std::map<uint8_t, double> confidence_map = {
1254 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1255 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1256 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1257 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1258 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1259 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1260 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1261 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1262 };
1263 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1264}
1265
1272inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1273
1280inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1281 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1282}
1283
1291inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1292 if (!object.velocity_is_present) {
1293 throw std::invalid_argument("No velocity present in PerceivedObject");
1294 }
1295 gm::Vector3 velocity;
1296 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1297 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1298 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1299 velocity.x = speed * cos(angle);
1300 velocity.y = speed * sin(angle);
1301 if (object.velocity.polar_velocity.z_velocity_is_present) {
1302 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1303 }
1304 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1305 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1306 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1307 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1308 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1309 }
1310 } else {
1311 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1312 }
1313 return velocity;
1314}
1315
1324inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1325 if (!object.velocity_is_present) {
1326 throw std::invalid_argument("No velocity present in PerceivedObject");
1327 }
1328 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1329 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1330 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1331 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1332 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1333 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1334 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1335 + lateral_confidence * sin(angle) * sin(angle);
1336 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1337 + lateral_confidence * cos(angle) * cos(angle);
1338 // neglect xy covariance, as it is not present in the output of this function
1339 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1340 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1341 : std::numeric_limits<double>::infinity();
1342 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1343 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1344 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1345 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1346 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1347 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1348 : std::numeric_limits<double>::infinity();
1349 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1350 } else {
1351 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1352 }
1353
1354}
1355
1362inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1363 return double(acceleration.value.value) / 10.0;
1364}
1365
1372inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1373 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1374}
1375
1383inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1384 if (!object.acceleration_is_present) {
1385 throw std::invalid_argument("No acceleration present in PerceivedObject");
1386 }
1387 gm::Vector3 acceleration;
1388
1389 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1390 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1391 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1392 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1393 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1394 }
1395 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1396 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1397 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1398 acceleration.x = magnitude * cos(angle);
1399 acceleration.y = magnitude * sin(angle);
1400 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1401 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1402 }
1403 } else {
1404 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1405 }
1406
1407 return acceleration;
1408}
1409
1418inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1419 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1420
1421 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1422 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1423 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1424 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1425 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1426 : std::numeric_limits<double>::infinity();
1427 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1428 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1429 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1430 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1431 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1432 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1433 double lateral_confidence = magnitude * angle_confidence; // best approximation
1434 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1435 + lateral_confidence * sin(angle) * sin(angle);
1436 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1437 + lateral_confidence * cos(angle) * cos(angle);
1438 // neglect xy covariance, as it is not present in the output of this function
1439 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1440 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1441 : std::numeric_limits<double>::infinity();
1442 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1443 } else {
1444 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1445 }
1446}
1447
1459inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1460 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1461 return object.object_dimension_x.value.value;
1462}
1463
1471inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1472 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1473 return object.object_dimension_x.confidence.value;
1474}
1475
1487inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1488 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1489 return object.object_dimension_y.value.value;
1490}
1491
1499inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1500 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1501 return object.object_dimension_y.confidence.value;
1502}
1503
1515inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1516 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1517 return object.object_dimension_z.value.value;
1518}
1519
1527inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1528 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1529 return object.object_dimension_z.confidence.value;
1530}
1531
1541inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1542 gm::Vector3 dimensions;
1543 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1544 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1545 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1546 return dimensions;
1547}
1548
1555inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1556 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1557 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1558 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1559 return {conf_x, conf_y, conf_z};
1560}
1561
1572inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1573 const PerceivedObject &object) {
1574 gm::PointStamped utm_position;
1575 gm::PointStamped reference_position = getUTMPosition(cpm);
1576 gm::Point relative_position = getPositionOfPerceivedObject(object);
1577
1578 utm_position.header.frame_id = reference_position.header.frame_id;
1579 utm_position.point.x = reference_position.point.x + relative_position.x;
1580 utm_position.point.y = reference_position.point.y + relative_position.y;
1581 utm_position.point.z = reference_position.point.z + relative_position.z;
1582
1583 return utm_position;
1584}
1585
1595inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1596 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1597}
1598
1605inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1606 return sensor_information.sensor_id.value;
1607}
1608
1615inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1616 return sensor_information.sensor_type.value;
1617}
1618
1619} // namespace etsi_its_cpm_ts_msgs::access
cdd_v2-1-1_getters.h
Getter functions for the ETSI ITS Common Data Dictionary (CDD) v2.1.1.
etsi_its_cpm_ts_msgs::access::getYawRateConfidenceOfPerceivedObject
double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Yaw Rate Confidence Of Perceived Object.
Definition cpm_ts_getters.h:745
etsi_its_cpm_ts_msgs::access::getSpeed
double getSpeed(const Speed &speed)
Get the vehicle speed.
Definition cpm_ts_getters.h:84
etsi_its_cpm_ts_msgs::access::getAccelerationMagnitude
double getAccelerationMagnitude(const AccelerationMagnitude &acceleration_magnitude)
Get the AccelerationMagnitude value.
Definition cpm_ts_getters.h:103
etsi_its_cpm_ts_msgs::access::getDimensionsConfidenceOfPerceivedObject
std::tuple< double, double, double > getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Dimensions Confidence Of Perceived Object.
Definition cpm_ts_getters.h:1050
etsi_its_cpm_ts_msgs::access::getCartesianAccelerationConfidenceOfPerceivedObject
std::tuple< double, double, double > getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian Acceleration Confidence Of Perceived Object.
Definition cpm_ts_getters.h:913
etsi_its_cpm_ts_msgs::access::getYawConfidenceOfPerceivedObject
double getYawConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Yaw Confidence Of Perceived Object object.
Definition cpm_ts_getters.h:704
etsi_its_cpm_ts_msgs::access::getCartesianCoordinate
int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate)
Retrieves the Cartesian coordinate value from a CartesianCoordinateWithConfidence object.
Definition cpm_ts_getters.h:590
etsi_its_cpm_ts_msgs::access::getAltitude
double getAltitude(const Altitude &altitude)
Get the Altitude value.
Definition cpm_ts_getters.h:71
etsi_its_cpm_ts_msgs::access::getZDimensionOfPerceivedObject
uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object)
Retrieves the z-dimension of a perceived object.
Definition cpm_ts_getters.h:1010
etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects
uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container)
Definition cpm_ts_getters.h:532
etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject
gm::Point getPositionOfPerceivedObject(const PerceivedObject &object)
Definition cpm_ts_getters.h:610
etsi_its_cpm_ts_msgs::access::getCartesianCoordinateConfidence
uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate)
Retrieves the confidence value from a CartesianCoordinateWithConfidence object.
Definition cpm_ts_getters.h:600
etsi_its_cpm_ts_msgs::access::getCartesianAngleConfidence
uint8_t getCartesianAngleConfidence(const CartesianAngle &angle)
Get the confidence of the Cartesian angle.
Definition cpm_ts_getters.h:650
etsi_its_cpm_ts_msgs::access::getYDimensionOfPerceivedObject
uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object)
Retrieves the y-dimension of a perceived object.
Definition cpm_ts_getters.h:982
etsi_its_cpm_ts_msgs::access::getVelocityComponent
double getVelocityComponent(const VelocityComponent &velocity)
Get the velocity component of the PerceivedObject.
Definition cpm_ts_getters.h:767
etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer
WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm)
Retrieves the perceived object container from a CPM.
Definition cpm_ts_getters.h:521
etsi_its_cpm_ts_msgs::access::getSensorType
uint8_t getSensorType(const SensorInformation &sensor_information)
Get the sensorType of a SensorInformation object.
Definition cpm_ts_getters.h:1110
etsi_its_cpm_ts_msgs::access::getYawRateOfPerceivedObject
double getYawRateOfPerceivedObject(const PerceivedObject &object)
Get the yaw rate of the PerceivedObject.
Definition cpm_ts_getters.h:731
etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject
gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object)
Retrieves the dimensions of a perceived object.
Definition cpm_ts_getters.h:1036
etsi_its_cpm_ts_msgs::access::getPoseOfPerceivedObject
gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object)
Get the pose of the PerceivedObject.
Definition cpm_ts_getters.h:717
etsi_its_cpm_ts_msgs::access::getSpeedConfidence
double getSpeedConfidence(const Speed &speed)
Get the Speed Confidence.
Definition cpm_ts_getters.h:92
etsi_its_cpm_ts_msgs::access::getYawOfPerceivedObject
double getYawOfPerceivedObject(const PerceivedObject &object)
Get the yaw of the PerceivedObject.
Definition cpm_ts_getters.h:689
etsi_its_cpm_ts_msgs::access::getLongitude
double getLongitude(const Longitude &longitude)
Get the Longitude value.
Definition cpm_ts_getters.h:63
etsi_its_cpm_ts_msgs::access::getStationID
uint32_t getStationID(const ItsPduHeader &header)
Get the StationID of ItsPduHeader.
Definition cpm_ts_getters.h:47
etsi_its_cpm_ts_msgs::access::getYDimensionConfidenceOfPerceivedObject
uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object)
Gets the confidence of the y-dimension of a perceived object.
Definition cpm_ts_getters.h:994
etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject
gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object)
Calculates the UTM position of a perceived object based on the CPMs referencep position.
Definition cpm_ts_getters.h:1067
etsi_its_cpm_ts_msgs::access::getLatitude
double getLatitude(const Latitude &latitude)
Get the Latitude value.
Definition cpm_ts_getters.h:55
etsi_its_cpm_ts_msgs::access::getCartesianVelocityConfidenceOfPerceivedObject
std::tuple< double, double, double > getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian Velocity Confidence Of Perceived Object object.
Definition cpm_ts_getters.h:819
etsi_its_cpm_ts_msgs::access::getZDimensionConfidenceOfPerceivedObject
uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object)
Gets the confidence of the z-dimension of a perceived object.
Definition cpm_ts_getters.h:1022
etsi_its_cpm_ts_msgs::access::getCartesianAngle
uint16_t getCartesianAngle(const CartesianAngle &angle)
Get the Cartesian angle of the PerceivedObject.
Definition cpm_ts_getters.h:642
etsi_its_cpm_ts_msgs::access::getVelocityComponentConfidence
double getVelocityComponentConfidence(const VelocityComponent &velocity)
Get the confidence of the velocity component.
Definition cpm_ts_getters.h:775
etsi_its_cpm_ts_msgs::access::getWGSPosConfidenceEllipse
std::array< double, 4 > getWGSPosConfidenceEllipse(const PosConfidenceEllipse &position_confidence_ellipse)
Get the covariance matrix of the position confidence ellipse.
Definition cpm_ts_getters.h:299
etsi_its_cpm_ts_msgs::access::getXDimensionConfidenceOfPerceivedObject
uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object)
Gets the confidence of the x-dimension of a perceived object.
Definition cpm_ts_getters.h:966
etsi_its_cpm_ts_msgs::access::getIdOfPerceivedObject
uint16_t getIdOfPerceivedObject(const PerceivedObject &object)
Retrieves the ID of a perceived object.
Definition cpm_ts_getters.h:577
etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject
gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian velocity of the PerceivedObject.
Definition cpm_ts_getters.h:786
etsi_its_cpm_ts_msgs::access::getXDimensionOfPerceivedObject
uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object)
Gets the x-dimension of a perceived object.
Definition cpm_ts_getters.h:954
etsi_its_cpm_ts_msgs::access::getAccelerationMagnitudeConfidence
double getAccelerationMagnitudeConfidence(const AccelerationMagnitude &acceleration_magnitude)
Get the AccelerationMagnitude Confidence.
Definition cpm_ts_getters.h:114
etsi_its_cpm_ts_msgs::access::getSensorID
uint8_t getSensorID(const SensorInformation &sensor_information)
Get the sensorId of a SensorInformation object.
Definition cpm_ts_getters.h:1100
etsi_its_cpm_ts_msgs::access::getCpmContainerIds
std::vector< uint8_t > getCpmContainerIds(const CollectivePerceptionMessage &cpm)
Retrieves the container IDs from a CPM.
Definition cpm_ts_getters.h:488
etsi_its_cpm_ts_msgs::access::getAccelerationComponentConfidence
double getAccelerationComponentConfidence(const AccelerationComponent &acceleration)
Get the confidence of the acceleration component.
Definition cpm_ts_getters.h:867
etsi_its_cpm_ts_msgs::access::getWGSRefPosConfidence
const std::array< double, 4 > getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm)
Get the confidence ellipse of the reference position as Covariance matrix.
Definition cpm_ts_getters.h:1090
etsi_its_cpm_ts_msgs::access::getCpmContainer
WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id)
Definition cpm_ts_getters.h:504
etsi_its_cpm_ts_msgs::access::getPerceivedObject
PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i)
Retrieves the PerceivedObject at the specified index from the given WrappedCpmContainer.
Definition cpm_ts_getters.h:562
etsi_its_cpm_ts_msgs::access::getCartesianAccelerationOfPerceivedObject
gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian acceleration of the PerceivedObject.
Definition cpm_ts_getters.h:878
etsi_its_cpm_ts_msgs::access::getAccelerationComponent
double getAccelerationComponent(const AccelerationComponent &acceleration)
Get the acceleration component of the PerceivedObject.
Definition cpm_ts_getters.h:857
etsi_its_cpm_ts_msgs::access::getPositionConfidenceOfPerceivedObject
std::tuple< double, double, double > getPositionConfidenceOfPerceivedObject(const PerceivedObject &object)
Get the Position Confidences Of Perceived Object.
Definition cpm_ts_getters.h:627
etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject
gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object)
Calculates the orientation of a perceived object.
Definition cpm_ts_getters.h:662
etsi_its_cpm_ts_msgs::access::getUTMPosition
gm::PointStamped getUTMPosition(const T &reference_position, int &zone, bool &northp)
Get the UTM Position defined by the given ReferencePosition.
Definition cpm_ts_getters.h:130

◆ getAccelerationComponentConfidence()

double etsi_its_cpm_ts_msgs::access::getAccelerationComponentConfidence ( const AccelerationComponent & acceleration)
inline

Get the confidence of the acceleration component.

Parameters
accelerationAccelerationComponent to get the confidence from
Returns
double value of the confidence of the acceleration component in m/s^2

Definition at line 867 of file cpm_ts_getters.h.

902 {
903
904#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
905
914inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
915
922inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
923 return cpm.payload.management_container.reference_time;
924}
925
932inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
933
940inline double getLatitude(const CollectivePerceptionMessage &cpm) {
941 return getLatitude(cpm.payload.management_container.reference_position.latitude);
942}
943
950inline double getLongitude(const CollectivePerceptionMessage &cpm) {
951 return getLongitude(cpm.payload.management_container.reference_position.longitude);
952}
953
960inline double getAltitude(const CollectivePerceptionMessage &cpm) {
961 return getAltitude(cpm.payload.management_container.reference_position.altitude);
962}
963
975inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
976 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
977}
978
988inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
989 int zone;
990 bool northp;
991 return getUTMPosition(cpm, zone, northp);
992}
993
1003inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1004 std::vector<uint8_t> container_ids;
1005 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1006 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1007 }
1008 return container_ids;
1009}
1010
1019inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1020 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1021 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1022 return cpm.payload.cpm_containers.value.array[i];
1023 }
1024 }
1025 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1026}
1027
1036inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1037 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1038}
1039
1047inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1048 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1049 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1050 }
1051 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1052 return number;
1053}
1054
1061inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1062 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1063}
1064
1068
1077inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1078 if (i >= getNumberOfPerceivedObjects(container)) {
1079 throw std::invalid_argument("Index out of range");
1080 }
1081 return container.container_data_perceived_object_container.perceived_objects.array[i];
1082}
1083
1092inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1093 if (!object.object_id_is_present) {
1094 throw std::invalid_argument("No object_id present in PerceivedObject");
1095 }
1096 return object.object_id.value;
1097}
1098
1105inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1106 return coordinate.value.value;
1107}
1108
1115inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1116 return coordinate.confidence.value;
1117}
1118
1125inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1126 gm::Point point;
1127 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1128 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1129 if (object.position.z_coordinate_is_present) {
1130 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1131 }
1132 return point;
1133}
1134
1142inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1143 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1144 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1145 double z_confidence = object.position.z_coordinate_is_present
1146 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1147 : std::numeric_limits<double>::infinity();
1148 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1149}
1150
1157inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1158
1165inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1166
1177inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1178 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1179 tf2::Quaternion q;
1180 double roll{0}, pitch{0}, yaw{0};
1181
1182 if (object.angles.x_angle_is_present) {
1183 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1184 M_PI;
1185 }
1186 if (object.angles.y_angle_is_present) {
1187 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1188 M_PI;
1189 }
1190 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1191 M_PI;
1192 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1193 q.setRPY(roll, pitch, yaw);
1194
1195 return tf2::toMsg(q);
1196}
1197
1204inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1205 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1206 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1207 double roll, pitch, yaw;
1208 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1209 return yaw;
1210}
1211
1219inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1220 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1221 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1222 yaw_confidence *= M_PI / 180.0; // convert to radians
1223 return yaw_confidence;
1224}
1225
1232inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1233 gm::Pose pose;
1234 pose.position = getPositionOfPerceivedObject(object);
1235 pose.orientation = getOrientationOfPerceivedObject(object);
1236 return pose;
1237}
1238
1246inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1247 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1248 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1249}
1250
1260inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1261 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1262 auto val = object.z_angular_velocity.confidence.value;
1263 static const std::map<uint8_t, double> confidence_map = {
1264 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1265 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1266 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1267 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1268 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1269 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1270 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1271 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1272 };
1273 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1274}
1275
1282inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1283
1290inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1291 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1292}
1293
1301inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1302 if (!object.velocity_is_present) {
1303 throw std::invalid_argument("No velocity present in PerceivedObject");
1304 }
1305 gm::Vector3 velocity;
1306 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1307 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1308 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1309 velocity.x = speed * cos(angle);
1310 velocity.y = speed * sin(angle);
1311 if (object.velocity.polar_velocity.z_velocity_is_present) {
1312 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1313 }
1314 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1315 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1316 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1317 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1318 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1319 }
1320 } else {
1321 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1322 }
1323 return velocity;
1324}
1325
1334inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1335 if (!object.velocity_is_present) {
1336 throw std::invalid_argument("No velocity present in PerceivedObject");
1337 }
1338 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1339 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1340 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1341 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1342 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1343 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1344 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1345 + lateral_confidence * sin(angle) * sin(angle);
1346 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1347 + lateral_confidence * cos(angle) * cos(angle);
1348 // neglect xy covariance, as it is not present in the output of this function
1349 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1350 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1351 : std::numeric_limits<double>::infinity();
1352 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1353 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1354 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1355 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1356 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1357 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1358 : std::numeric_limits<double>::infinity();
1359 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1360 } else {
1361 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1362 }
1363
1364}
1365
1372inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1373 return double(acceleration.value.value) / 10.0;
1374}
1375
1382inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1383 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1384}
1385
1393inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1394 if (!object.acceleration_is_present) {
1395 throw std::invalid_argument("No acceleration present in PerceivedObject");
1396 }
1397 gm::Vector3 acceleration;
1398
1399 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1400 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1401 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1402 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1403 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1404 }
1405 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1406 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1407 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1408 acceleration.x = magnitude * cos(angle);
1409 acceleration.y = magnitude * sin(angle);
1410 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1411 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1412 }
1413 } else {
1414 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1415 }
1416
1417 return acceleration;
1418}
1419
1428inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1429 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1430
1431 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1432 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1433 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1434 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1435 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1436 : std::numeric_limits<double>::infinity();
1437 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1438 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1439 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1440 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1441 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1442 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1443 double lateral_confidence = magnitude * angle_confidence; // best approximation
1444 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1445 + lateral_confidence * sin(angle) * sin(angle);
1446 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1447 + lateral_confidence * cos(angle) * cos(angle);
1448 // neglect xy covariance, as it is not present in the output of this function
1449 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1450 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1451 : std::numeric_limits<double>::infinity();
1452 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1453 } else {
1454 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1455 }
1456}
1457
1469inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1470 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1471 return object.object_dimension_x.value.value;
1472}
1473
1481inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1482 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1483 return object.object_dimension_x.confidence.value;
1484}
1485
1497inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1498 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1499 return object.object_dimension_y.value.value;
1500}
1501
1509inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1510 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1511 return object.object_dimension_y.confidence.value;
1512}
1513
1525inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1526 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1527 return object.object_dimension_z.value.value;
1528}
1529
1537inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1538 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1539 return object.object_dimension_z.confidence.value;
1540}
1541
1551inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1552 gm::Vector3 dimensions;
1553 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1554 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1555 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1556 return dimensions;
1557}
1558
1565inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1566 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1567 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1568 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1569 return {conf_x, conf_y, conf_z};
1570}
1571
1582inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1583 const PerceivedObject &object) {
1584 gm::PointStamped utm_position;
1585 gm::PointStamped reference_position = getUTMPosition(cpm);
1586 gm::Point relative_position = getPositionOfPerceivedObject(object);
1587
1588 utm_position.header.frame_id = reference_position.header.frame_id;
1589 utm_position.point.x = reference_position.point.x + relative_position.x;
1590 utm_position.point.y = reference_position.point.y + relative_position.y;
1591 utm_position.point.z = reference_position.point.z + relative_position.z;
1592
1593 return utm_position;
1594}
1595
1605inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1606 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1607}
1608
1615inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1616 return sensor_information.sensor_id.value;
1617}
1618
1625inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1626 return sensor_information.sensor_type.value;
1627}
1628
1629} // namespace etsi_its_cpm_ts_msgs::access

◆ getAccelerationMagnitude()

template<typename AccelerationMagnitude>
double etsi_its_cpm_ts_msgs::access::getAccelerationMagnitude ( const AccelerationMagnitude & acceleration_magnitude)
inline

Get the AccelerationMagnitude value.

Parameters
acceleration_magnitudeto get the AccelerationMagnitude from
Returns
double acceleration magnitude in m/s^2 as decimal number

Definition at line 103 of file cpm_ts_getters.h.

◆ getAccelerationMagnitudeConfidence()

template<typename AccelerationMagnitude>
double etsi_its_cpm_ts_msgs::access::getAccelerationMagnitudeConfidence ( const AccelerationMagnitude & acceleration_magnitude)
inline

Get the AccelerationMagnitude Confidence.

Parameters
acceleration_magnitudeto get the AccelerationMagnitudeConfidence from
Returns
double acceleration magnitude standard deviation in m/s^2 as decimal number

Definition at line 114 of file cpm_ts_getters.h.

◆ getAltitude() [1/2]

double etsi_its_cpm_ts_msgs::access::getAltitude ( const Altitude & altitude)
inline

Get the Altitude value.

Parameters
altitudeto get the Altitude value from
Returns
Altitude value (above the reference ellipsoid surface) in meter as decimal number (0 if unavailable)

Definition at line 71 of file cpm_ts_getters.h.

74 {
75 return getLatitude(cpm.payload.management_container.reference_position.latitude);
76}

◆ getAltitude() [2/2]

double etsi_its_cpm_ts_msgs::access::getAltitude ( const CollectivePerceptionMessage & cpm)
inline

Get the Altitude value of CPM.

Parameters
cpmCPM to get the Altitude value from
Returns
Altitude value in (above the reference ellipsoid surface) in meter as decimal number

Definition at line 445 of file cpm_ts_getters.h.

445 {
446 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
447 }

◆ getCartesianAccelerationConfidenceOfPerceivedObject()

std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getCartesianAccelerationConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Cartesian Acceleration Confidence Of Perceived Object.

Parameters
objectPerceivedObject to get the Cartesian acceleration from
Returns
std::tuple<double, double, double> the x,y and z standard deviations of the acceleration in m/s^2 The z standard deviation is infinity if the z acceleration is not present.
Exceptions
std::invalid_argumentIf the acceleration is no cartesian acceleration.

Definition at line 913 of file cpm_ts_getters.h.

948 {
949
950#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
951
960inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
961
968inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
969 return cpm.payload.management_container.reference_time;
970}
971
978inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
979
986inline double getLatitude(const CollectivePerceptionMessage &cpm) {
987 return getLatitude(cpm.payload.management_container.reference_position.latitude);
988}
989
996inline double getLongitude(const CollectivePerceptionMessage &cpm) {
997 return getLongitude(cpm.payload.management_container.reference_position.longitude);
998}
999
1006inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1007 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1008}
1009
1021inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1022 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1023}
1024
1034inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1035 int zone;
1036 bool northp;
1037 return getUTMPosition(cpm, zone, northp);
1038}
1039
1049inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1050 std::vector<uint8_t> container_ids;
1051 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1052 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1053 }
1054 return container_ids;
1055}
1056
1065inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1066 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1067 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1068 return cpm.payload.cpm_containers.value.array[i];
1069 }
1070 }
1071 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1072}
1073
1082inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1083 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1084}
1085
1093inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1094 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1095 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1096 }
1097 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1098 return number;
1099}
1100
1107inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1108 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1109}
1110
1114
1123inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1124 if (i >= getNumberOfPerceivedObjects(container)) {
1125 throw std::invalid_argument("Index out of range");
1126 }
1127 return container.container_data_perceived_object_container.perceived_objects.array[i];
1128}
1129
1138inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1139 if (!object.object_id_is_present) {
1140 throw std::invalid_argument("No object_id present in PerceivedObject");
1141 }
1142 return object.object_id.value;
1143}
1144
1151inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1152 return coordinate.value.value;
1153}
1154
1161inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1162 return coordinate.confidence.value;
1163}
1164
1171inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1172 gm::Point point;
1173 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1174 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1175 if (object.position.z_coordinate_is_present) {
1176 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1177 }
1178 return point;
1179}
1180
1188inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1189 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1190 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1191 double z_confidence = object.position.z_coordinate_is_present
1192 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1193 : std::numeric_limits<double>::infinity();
1194 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1195}
1196
1203inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1204
1211inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1212
1223inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1224 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1225 tf2::Quaternion q;
1226 double roll{0}, pitch{0}, yaw{0};
1227
1228 if (object.angles.x_angle_is_present) {
1229 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1230 M_PI;
1231 }
1232 if (object.angles.y_angle_is_present) {
1233 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1234 M_PI;
1235 }
1236 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1237 M_PI;
1238 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1239 q.setRPY(roll, pitch, yaw);
1240
1241 return tf2::toMsg(q);
1242}
1243
1250inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1251 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1252 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1253 double roll, pitch, yaw;
1254 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1255 return yaw;
1256}
1257
1265inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1266 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1267 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1268 yaw_confidence *= M_PI / 180.0; // convert to radians
1269 return yaw_confidence;
1270}
1271
1278inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1279 gm::Pose pose;
1280 pose.position = getPositionOfPerceivedObject(object);
1281 pose.orientation = getOrientationOfPerceivedObject(object);
1282 return pose;
1283}
1284
1292inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1293 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1294 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1295}
1296
1306inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1307 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1308 auto val = object.z_angular_velocity.confidence.value;
1309 static const std::map<uint8_t, double> confidence_map = {
1310 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1311 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1312 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1313 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1314 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1315 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1316 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1317 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1318 };
1319 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1320}
1321
1328inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1329
1336inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1337 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1338}
1339
1347inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1348 if (!object.velocity_is_present) {
1349 throw std::invalid_argument("No velocity present in PerceivedObject");
1350 }
1351 gm::Vector3 velocity;
1352 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1353 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1354 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1355 velocity.x = speed * cos(angle);
1356 velocity.y = speed * sin(angle);
1357 if (object.velocity.polar_velocity.z_velocity_is_present) {
1358 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1359 }
1360 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1361 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1362 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1363 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1364 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1365 }
1366 } else {
1367 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1368 }
1369 return velocity;
1370}
1371
1380inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1381 if (!object.velocity_is_present) {
1382 throw std::invalid_argument("No velocity present in PerceivedObject");
1383 }
1384 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1385 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1386 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1387 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1388 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1389 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1390 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1391 + lateral_confidence * sin(angle) * sin(angle);
1392 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1393 + lateral_confidence * cos(angle) * cos(angle);
1394 // neglect xy covariance, as it is not present in the output of this function
1395 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1396 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1397 : std::numeric_limits<double>::infinity();
1398 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1399 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1400 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1401 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1402 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1403 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1404 : std::numeric_limits<double>::infinity();
1405 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1406 } else {
1407 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1408 }
1409
1410}
1411
1418inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1419 return double(acceleration.value.value) / 10.0;
1420}
1421
1428inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1429 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1430}
1431
1439inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1440 if (!object.acceleration_is_present) {
1441 throw std::invalid_argument("No acceleration present in PerceivedObject");
1442 }
1443 gm::Vector3 acceleration;
1444
1445 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1446 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1447 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1448 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1449 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1450 }
1451 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1452 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1453 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1454 acceleration.x = magnitude * cos(angle);
1455 acceleration.y = magnitude * sin(angle);
1456 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1457 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1458 }
1459 } else {
1460 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1461 }
1462
1463 return acceleration;
1464}
1465
1474inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1475 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1476
1477 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1478 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1479 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1480 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1481 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1482 : std::numeric_limits<double>::infinity();
1483 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1484 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1485 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1486 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1487 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1488 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1489 double lateral_confidence = magnitude * angle_confidence; // best approximation
1490 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1491 + lateral_confidence * sin(angle) * sin(angle);
1492 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1493 + lateral_confidence * cos(angle) * cos(angle);
1494 // neglect xy covariance, as it is not present in the output of this function
1495 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1496 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1497 : std::numeric_limits<double>::infinity();
1498 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1499 } else {
1500 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1501 }
1502}
1503
1515inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1516 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1517 return object.object_dimension_x.value.value;
1518}
1519
1527inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1528 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1529 return object.object_dimension_x.confidence.value;
1530}
1531
1543inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1544 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1545 return object.object_dimension_y.value.value;
1546}
1547
1555inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1556 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1557 return object.object_dimension_y.confidence.value;
1558}
1559
1571inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1572 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1573 return object.object_dimension_z.value.value;
1574}
1575
1583inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1584 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1585 return object.object_dimension_z.confidence.value;
1586}
1587
1597inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1598 gm::Vector3 dimensions;
1599 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1600 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1601 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1602 return dimensions;
1603}
1604
1611inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1612 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1613 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1614 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1615 return {conf_x, conf_y, conf_z};
1616}
1617
1628inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1629 const PerceivedObject &object) {
1630 gm::PointStamped utm_position;
1631 gm::PointStamped reference_position = getUTMPosition(cpm);
1632 gm::Point relative_position = getPositionOfPerceivedObject(object);
1633
1634 utm_position.header.frame_id = reference_position.header.frame_id;
1635 utm_position.point.x = reference_position.point.x + relative_position.x;
1636 utm_position.point.y = reference_position.point.y + relative_position.y;
1637 utm_position.point.z = reference_position.point.z + relative_position.z;
1638
1639 return utm_position;
1640}
1641
1651inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1652 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1653}
1654
1661inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1662 return sensor_information.sensor_id.value;
1663}
1664
1671inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1672 return sensor_information.sensor_type.value;
1673}
1674
1675} // namespace etsi_its_cpm_ts_msgs::access

◆ getCartesianAccelerationOfPerceivedObject()

gm::Vector3 etsi_its_cpm_ts_msgs::access::getCartesianAccelerationOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Cartesian acceleration of the PerceivedObject.

Parameters
objectPerceivedObject to get the Cartesian acceleration from
Returns
gm::Vector3 Cartesian acceleration of the PerceivedObject in m/s^2 (local coordinate system)
Exceptions
std::invalid_argumentIf the acceleration is no cartesian acceleration.

Definition at line 878 of file cpm_ts_getters.h.

913 {
914
915#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
916
925inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
926
933inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
934 return cpm.payload.management_container.reference_time;
935}
936
943inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
944
951inline double getLatitude(const CollectivePerceptionMessage &cpm) {
952 return getLatitude(cpm.payload.management_container.reference_position.latitude);
953}
954
961inline double getLongitude(const CollectivePerceptionMessage &cpm) {
962 return getLongitude(cpm.payload.management_container.reference_position.longitude);
963}
964
971inline double getAltitude(const CollectivePerceptionMessage &cpm) {
972 return getAltitude(cpm.payload.management_container.reference_position.altitude);
973}
974
986inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
987 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
988}
989
999inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1000 int zone;
1001 bool northp;
1002 return getUTMPosition(cpm, zone, northp);
1003}
1004
1014inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1015 std::vector<uint8_t> container_ids;
1016 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1017 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1018 }
1019 return container_ids;
1020}
1021
1030inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1031 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1032 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1033 return cpm.payload.cpm_containers.value.array[i];
1034 }
1035 }
1036 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1037}
1038
1047inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1048 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1049}
1050
1058inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1059 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1060 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1061 }
1062 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1063 return number;
1064}
1065
1072inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1073 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1074}
1075
1079
1088inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1089 if (i >= getNumberOfPerceivedObjects(container)) {
1090 throw std::invalid_argument("Index out of range");
1091 }
1092 return container.container_data_perceived_object_container.perceived_objects.array[i];
1093}
1094
1103inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1104 if (!object.object_id_is_present) {
1105 throw std::invalid_argument("No object_id present in PerceivedObject");
1106 }
1107 return object.object_id.value;
1108}
1109
1116inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1117 return coordinate.value.value;
1118}
1119
1126inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1127 return coordinate.confidence.value;
1128}
1129
1136inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1137 gm::Point point;
1138 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1139 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1140 if (object.position.z_coordinate_is_present) {
1141 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1142 }
1143 return point;
1144}
1145
1153inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1154 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1155 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1156 double z_confidence = object.position.z_coordinate_is_present
1157 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1158 : std::numeric_limits<double>::infinity();
1159 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1160}
1161
1168inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1169
1176inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1177
1188inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1189 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1190 tf2::Quaternion q;
1191 double roll{0}, pitch{0}, yaw{0};
1192
1193 if (object.angles.x_angle_is_present) {
1194 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1195 M_PI;
1196 }
1197 if (object.angles.y_angle_is_present) {
1198 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1199 M_PI;
1200 }
1201 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1202 M_PI;
1203 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1204 q.setRPY(roll, pitch, yaw);
1205
1206 return tf2::toMsg(q);
1207}
1208
1215inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1216 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1217 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1218 double roll, pitch, yaw;
1219 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1220 return yaw;
1221}
1222
1230inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1231 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1232 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1233 yaw_confidence *= M_PI / 180.0; // convert to radians
1234 return yaw_confidence;
1235}
1236
1243inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1244 gm::Pose pose;
1245 pose.position = getPositionOfPerceivedObject(object);
1246 pose.orientation = getOrientationOfPerceivedObject(object);
1247 return pose;
1248}
1249
1257inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1258 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1259 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1260}
1261
1271inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1272 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1273 auto val = object.z_angular_velocity.confidence.value;
1274 static const std::map<uint8_t, double> confidence_map = {
1275 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1276 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1277 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1278 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1279 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1280 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1281 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1282 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1283 };
1284 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1285}
1286
1293inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1294
1301inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1302 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1303}
1304
1312inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1313 if (!object.velocity_is_present) {
1314 throw std::invalid_argument("No velocity present in PerceivedObject");
1315 }
1316 gm::Vector3 velocity;
1317 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1318 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1319 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1320 velocity.x = speed * cos(angle);
1321 velocity.y = speed * sin(angle);
1322 if (object.velocity.polar_velocity.z_velocity_is_present) {
1323 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1324 }
1325 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1326 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1327 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1328 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1329 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1330 }
1331 } else {
1332 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1333 }
1334 return velocity;
1335}
1336
1345inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1346 if (!object.velocity_is_present) {
1347 throw std::invalid_argument("No velocity present in PerceivedObject");
1348 }
1349 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1350 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1351 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1352 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1353 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1354 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1355 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1356 + lateral_confidence * sin(angle) * sin(angle);
1357 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1358 + lateral_confidence * cos(angle) * cos(angle);
1359 // neglect xy covariance, as it is not present in the output of this function
1360 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1361 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1362 : std::numeric_limits<double>::infinity();
1363 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1364 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1365 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1366 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1367 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1368 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1369 : std::numeric_limits<double>::infinity();
1370 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1371 } else {
1372 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1373 }
1374
1375}
1376
1383inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1384 return double(acceleration.value.value) / 10.0;
1385}
1386
1393inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1394 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1395}
1396
1404inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1405 if (!object.acceleration_is_present) {
1406 throw std::invalid_argument("No acceleration present in PerceivedObject");
1407 }
1408 gm::Vector3 acceleration;
1409
1410 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1411 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1412 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1413 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1414 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1415 }
1416 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1417 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1418 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1419 acceleration.x = magnitude * cos(angle);
1420 acceleration.y = magnitude * sin(angle);
1421 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1422 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1423 }
1424 } else {
1425 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1426 }
1427
1428 return acceleration;
1429}
1430
1439inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1440 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1441
1442 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1443 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1444 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1445 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1446 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1447 : std::numeric_limits<double>::infinity();
1448 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1449 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1450 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1451 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1452 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1453 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1454 double lateral_confidence = magnitude * angle_confidence; // best approximation
1455 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1456 + lateral_confidence * sin(angle) * sin(angle);
1457 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1458 + lateral_confidence * cos(angle) * cos(angle);
1459 // neglect xy covariance, as it is not present in the output of this function
1460 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1461 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1462 : std::numeric_limits<double>::infinity();
1463 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1464 } else {
1465 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1466 }
1467}
1468
1480inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1481 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1482 return object.object_dimension_x.value.value;
1483}
1484
1492inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1493 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1494 return object.object_dimension_x.confidence.value;
1495}
1496
1508inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1509 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1510 return object.object_dimension_y.value.value;
1511}
1512
1520inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1521 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1522 return object.object_dimension_y.confidence.value;
1523}
1524
1536inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1537 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1538 return object.object_dimension_z.value.value;
1539}
1540
1548inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1549 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1550 return object.object_dimension_z.confidence.value;
1551}
1552
1562inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1563 gm::Vector3 dimensions;
1564 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1565 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1566 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1567 return dimensions;
1568}
1569
1576inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1577 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1578 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1579 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1580 return {conf_x, conf_y, conf_z};
1581}
1582
1593inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1594 const PerceivedObject &object) {
1595 gm::PointStamped utm_position;
1596 gm::PointStamped reference_position = getUTMPosition(cpm);
1597 gm::Point relative_position = getPositionOfPerceivedObject(object);
1598
1599 utm_position.header.frame_id = reference_position.header.frame_id;
1600 utm_position.point.x = reference_position.point.x + relative_position.x;
1601 utm_position.point.y = reference_position.point.y + relative_position.y;
1602 utm_position.point.z = reference_position.point.z + relative_position.z;
1603
1604 return utm_position;
1605}
1606
1616inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1617 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1618}
1619
1626inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1627 return sensor_information.sensor_id.value;
1628}
1629
1636inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1637 return sensor_information.sensor_type.value;
1638}
1639
1640} // namespace etsi_its_cpm_ts_msgs::access

◆ getCartesianAngle()

uint16_t etsi_its_cpm_ts_msgs::access::getCartesianAngle ( const CartesianAngle & angle)
inline

Get the Cartesian angle of the PerceivedObject.

Parameters
objectPerceivedObject to get the Cartesian angle from
Returns
unit16_t Cartesian angle of the PerceivedObject in 0,1 degrees

Definition at line 642 of file cpm_ts_getters.h.

◆ getCartesianAngleConfidence()

uint8_t etsi_its_cpm_ts_msgs::access::getCartesianAngleConfidence ( const CartesianAngle & angle)
inline

Get the confidence of the Cartesian angle.

Parameters
angleCartesianAngle to get the confidence from
Returns
uint8_t confidence of the Cartesian angle in 0,1 degrees

Definition at line 650 of file cpm_ts_getters.h.

◆ getCartesianCoordinate()

int32_t etsi_its_cpm_ts_msgs::access::getCartesianCoordinate ( const CartesianCoordinateWithConfidence & coordinate)
inline

Retrieves the Cartesian coordinate value from a CartesianCoordinateWithConfidence object.

Parameters
coordinateThe CartesianCoordinateWithConfidence object from which to retrieve the value.
Returns
The Cartesian coordinate value in centimeters.

Definition at line 590 of file cpm_ts_getters.h.

◆ getCartesianCoordinateConfidence()

uint16_t etsi_its_cpm_ts_msgs::access::getCartesianCoordinateConfidence ( const CartesianCoordinateWithConfidence & coordinate)
inline

Retrieves the confidence value from a CartesianCoordinateWithConfidence object.

Parameters
coordinateThe CartesianCoordinateWithConfidence object from which to retrieve the confidence value.
Returns
The confidence value of the CartesianCoordinateWithConfidence object in centimeters.

Definition at line 600 of file cpm_ts_getters.h.

◆ getCartesianVelocityConfidenceOfPerceivedObject()

std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getCartesianVelocityConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Cartesian Velocity Confidence Of Perceived Object object.

Parameters
objectPerceivedObject to get the Cartesian velocity from
Returns
std::tuple<double, double, double> the x,y and z standard deviations of the velocity in m/s The z standard deviation is infinity if the z velocity is not present.
Exceptions
std::invalid_argumentIf the velocity is no cartesian velocity.

Definition at line 819 of file cpm_ts_getters.h.

854 {
855
856#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
857
866inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
867
874inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
875 return cpm.payload.management_container.reference_time;
876}
877
884inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
885
892inline double getLatitude(const CollectivePerceptionMessage &cpm) {
893 return getLatitude(cpm.payload.management_container.reference_position.latitude);
894}
895
902inline double getLongitude(const CollectivePerceptionMessage &cpm) {
903 return getLongitude(cpm.payload.management_container.reference_position.longitude);
904}
905
912inline double getAltitude(const CollectivePerceptionMessage &cpm) {
913 return getAltitude(cpm.payload.management_container.reference_position.altitude);
914}
915
927inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
928 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
929}
930
940inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
941 int zone;
942 bool northp;
943 return getUTMPosition(cpm, zone, northp);
944}
945
955inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
956 std::vector<uint8_t> container_ids;
957 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
958 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
959 }
960 return container_ids;
961}
962
971inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
972 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
973 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
974 return cpm.payload.cpm_containers.value.array[i];
975 }
976 }
977 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
978}
979
988inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
989 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
990}
991
999inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1000 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1001 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1002 }
1003 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1004 return number;
1005}
1006
1013inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1014 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1015}
1016
1020
1029inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1030 if (i >= getNumberOfPerceivedObjects(container)) {
1031 throw std::invalid_argument("Index out of range");
1032 }
1033 return container.container_data_perceived_object_container.perceived_objects.array[i];
1034}
1035
1044inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1045 if (!object.object_id_is_present) {
1046 throw std::invalid_argument("No object_id present in PerceivedObject");
1047 }
1048 return object.object_id.value;
1049}
1050
1057inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1058 return coordinate.value.value;
1059}
1060
1067inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1068 return coordinate.confidence.value;
1069}
1070
1077inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1078 gm::Point point;
1079 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1080 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1081 if (object.position.z_coordinate_is_present) {
1082 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1083 }
1084 return point;
1085}
1086
1094inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1095 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1096 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1097 double z_confidence = object.position.z_coordinate_is_present
1098 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1099 : std::numeric_limits<double>::infinity();
1100 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1101}
1102
1109inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1110
1117inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1118
1129inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1130 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1131 tf2::Quaternion q;
1132 double roll{0}, pitch{0}, yaw{0};
1133
1134 if (object.angles.x_angle_is_present) {
1135 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1136 M_PI;
1137 }
1138 if (object.angles.y_angle_is_present) {
1139 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1140 M_PI;
1141 }
1142 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1143 M_PI;
1144 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1145 q.setRPY(roll, pitch, yaw);
1146
1147 return tf2::toMsg(q);
1148}
1149
1156inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1157 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1158 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1159 double roll, pitch, yaw;
1160 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1161 return yaw;
1162}
1163
1171inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1172 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1173 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1174 yaw_confidence *= M_PI / 180.0; // convert to radians
1175 return yaw_confidence;
1176}
1177
1184inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1185 gm::Pose pose;
1186 pose.position = getPositionOfPerceivedObject(object);
1187 pose.orientation = getOrientationOfPerceivedObject(object);
1188 return pose;
1189}
1190
1198inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1199 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1200 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1201}
1202
1212inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1213 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1214 auto val = object.z_angular_velocity.confidence.value;
1215 static const std::map<uint8_t, double> confidence_map = {
1216 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1217 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1218 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1219 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1220 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1221 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1222 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1223 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1224 };
1225 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1226}
1227
1234inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1235
1242inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1243 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1244}
1245
1253inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1254 if (!object.velocity_is_present) {
1255 throw std::invalid_argument("No velocity present in PerceivedObject");
1256 }
1257 gm::Vector3 velocity;
1258 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1259 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1260 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1261 velocity.x = speed * cos(angle);
1262 velocity.y = speed * sin(angle);
1263 if (object.velocity.polar_velocity.z_velocity_is_present) {
1264 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1265 }
1266 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1267 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1268 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1269 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1270 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1271 }
1272 } else {
1273 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1274 }
1275 return velocity;
1276}
1277
1286inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1287 if (!object.velocity_is_present) {
1288 throw std::invalid_argument("No velocity present in PerceivedObject");
1289 }
1290 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1291 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1292 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1293 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1294 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1295 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1296 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1297 + lateral_confidence * sin(angle) * sin(angle);
1298 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1299 + lateral_confidence * cos(angle) * cos(angle);
1300 // neglect xy covariance, as it is not present in the output of this function
1301 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1302 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1303 : std::numeric_limits<double>::infinity();
1304 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1305 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1306 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1307 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1308 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1309 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1310 : std::numeric_limits<double>::infinity();
1311 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1312 } else {
1313 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1314 }
1315
1316}
1317
1324inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1325 return double(acceleration.value.value) / 10.0;
1326}
1327
1334inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1335 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1336}
1337
1345inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1346 if (!object.acceleration_is_present) {
1347 throw std::invalid_argument("No acceleration present in PerceivedObject");
1348 }
1349 gm::Vector3 acceleration;
1350
1351 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1352 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1353 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1354 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1355 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1356 }
1357 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1358 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1359 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1360 acceleration.x = magnitude * cos(angle);
1361 acceleration.y = magnitude * sin(angle);
1362 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1363 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1364 }
1365 } else {
1366 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1367 }
1368
1369 return acceleration;
1370}
1371
1380inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1381 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1382
1383 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1384 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1385 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1386 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1387 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1388 : std::numeric_limits<double>::infinity();
1389 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1390 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1391 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1392 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1393 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1394 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1395 double lateral_confidence = magnitude * angle_confidence; // best approximation
1396 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1397 + lateral_confidence * sin(angle) * sin(angle);
1398 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1399 + lateral_confidence * cos(angle) * cos(angle);
1400 // neglect xy covariance, as it is not present in the output of this function
1401 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1402 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1403 : std::numeric_limits<double>::infinity();
1404 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1405 } else {
1406 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1407 }
1408}
1409
1421inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1422 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1423 return object.object_dimension_x.value.value;
1424}
1425
1433inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1434 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1435 return object.object_dimension_x.confidence.value;
1436}
1437
1449inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1450 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1451 return object.object_dimension_y.value.value;
1452}
1453
1461inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1462 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1463 return object.object_dimension_y.confidence.value;
1464}
1465
1477inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1478 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1479 return object.object_dimension_z.value.value;
1480}
1481
1489inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1490 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1491 return object.object_dimension_z.confidence.value;
1492}
1493
1503inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1504 gm::Vector3 dimensions;
1505 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1506 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1507 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1508 return dimensions;
1509}
1510
1517inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1518 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1519 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1520 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1521 return {conf_x, conf_y, conf_z};
1522}
1523
1534inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1535 const PerceivedObject &object) {
1536 gm::PointStamped utm_position;
1537 gm::PointStamped reference_position = getUTMPosition(cpm);
1538 gm::Point relative_position = getPositionOfPerceivedObject(object);
1539
1540 utm_position.header.frame_id = reference_position.header.frame_id;
1541 utm_position.point.x = reference_position.point.x + relative_position.x;
1542 utm_position.point.y = reference_position.point.y + relative_position.y;
1543 utm_position.point.z = reference_position.point.z + relative_position.z;
1544
1545 return utm_position;
1546}
1547
1557inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1558 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1559}
1560
1567inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1568 return sensor_information.sensor_id.value;
1569}
1570
1577inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1578 return sensor_information.sensor_type.value;
1579}
1580
1581} // namespace etsi_its_cpm_ts_msgs::access

◆ getCartesianVelocityOfPerceivedObject()

gm::Vector3 etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Cartesian velocity of the PerceivedObject.

Parameters
objectPerceivedObject to get the Cartesian velocity from
Returns
gm::Vector3 Cartesian velocity of the PerceivedObject in m/s (local coordinate system)
Exceptions
std::invalid_argumentIf the velocity is no cartesian velocity.

Definition at line 786 of file cpm_ts_getters.h.

821 {
822
823#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
824
833inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
834
841inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
842 return cpm.payload.management_container.reference_time;
843}
844
851inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
852
859inline double getLatitude(const CollectivePerceptionMessage &cpm) {
860 return getLatitude(cpm.payload.management_container.reference_position.latitude);
861}
862
869inline double getLongitude(const CollectivePerceptionMessage &cpm) {
870 return getLongitude(cpm.payload.management_container.reference_position.longitude);
871}
872
879inline double getAltitude(const CollectivePerceptionMessage &cpm) {
880 return getAltitude(cpm.payload.management_container.reference_position.altitude);
881}
882
894inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
895 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
896}
897
907inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
908 int zone;
909 bool northp;
910 return getUTMPosition(cpm, zone, northp);
911}
912
922inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
923 std::vector<uint8_t> container_ids;
924 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
925 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
926 }
927 return container_ids;
928}
929
938inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
939 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
940 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
941 return cpm.payload.cpm_containers.value.array[i];
942 }
943 }
944 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
945}
946
955inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
956 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
957}
958
966inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
967 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
968 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
969 }
970 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
971 return number;
972}
973
980inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
981 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
982}
983
987
996inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
997 if (i >= getNumberOfPerceivedObjects(container)) {
998 throw std::invalid_argument("Index out of range");
999 }
1000 return container.container_data_perceived_object_container.perceived_objects.array[i];
1001}
1002
1011inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1012 if (!object.object_id_is_present) {
1013 throw std::invalid_argument("No object_id present in PerceivedObject");
1014 }
1015 return object.object_id.value;
1016}
1017
1024inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1025 return coordinate.value.value;
1026}
1027
1034inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1035 return coordinate.confidence.value;
1036}
1037
1044inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1045 gm::Point point;
1046 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1047 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1048 if (object.position.z_coordinate_is_present) {
1049 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1050 }
1051 return point;
1052}
1053
1061inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1062 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1063 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1064 double z_confidence = object.position.z_coordinate_is_present
1065 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1066 : std::numeric_limits<double>::infinity();
1067 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1068}
1069
1076inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1077
1084inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1085
1096inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1097 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1098 tf2::Quaternion q;
1099 double roll{0}, pitch{0}, yaw{0};
1100
1101 if (object.angles.x_angle_is_present) {
1102 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1103 M_PI;
1104 }
1105 if (object.angles.y_angle_is_present) {
1106 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1107 M_PI;
1108 }
1109 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1110 M_PI;
1111 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1112 q.setRPY(roll, pitch, yaw);
1113
1114 return tf2::toMsg(q);
1115}
1116
1123inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1124 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1125 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1126 double roll, pitch, yaw;
1127 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1128 return yaw;
1129}
1130
1138inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1139 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1140 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1141 yaw_confidence *= M_PI / 180.0; // convert to radians
1142 return yaw_confidence;
1143}
1144
1151inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1152 gm::Pose pose;
1153 pose.position = getPositionOfPerceivedObject(object);
1154 pose.orientation = getOrientationOfPerceivedObject(object);
1155 return pose;
1156}
1157
1165inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1166 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1167 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1168}
1169
1179inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1180 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1181 auto val = object.z_angular_velocity.confidence.value;
1182 static const std::map<uint8_t, double> confidence_map = {
1183 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1184 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1185 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1186 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1187 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1188 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1189 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1190 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1191 };
1192 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1193}
1194
1201inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1202
1209inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1210 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1211}
1212
1220inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1221 if (!object.velocity_is_present) {
1222 throw std::invalid_argument("No velocity present in PerceivedObject");
1223 }
1224 gm::Vector3 velocity;
1225 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1226 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1227 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1228 velocity.x = speed * cos(angle);
1229 velocity.y = speed * sin(angle);
1230 if (object.velocity.polar_velocity.z_velocity_is_present) {
1231 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1232 }
1233 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1234 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1235 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1236 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1237 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1238 }
1239 } else {
1240 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1241 }
1242 return velocity;
1243}
1244
1253inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1254 if (!object.velocity_is_present) {
1255 throw std::invalid_argument("No velocity present in PerceivedObject");
1256 }
1257 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1258 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1259 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1260 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1261 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1262 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1263 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1264 + lateral_confidence * sin(angle) * sin(angle);
1265 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1266 + lateral_confidence * cos(angle) * cos(angle);
1267 // neglect xy covariance, as it is not present in the output of this function
1268 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1269 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1270 : std::numeric_limits<double>::infinity();
1271 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1272 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1273 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1274 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1275 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1276 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1277 : std::numeric_limits<double>::infinity();
1278 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1279 } else {
1280 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1281 }
1282
1283}
1284
1291inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1292 return double(acceleration.value.value) / 10.0;
1293}
1294
1301inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1302 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1303}
1304
1312inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1313 if (!object.acceleration_is_present) {
1314 throw std::invalid_argument("No acceleration present in PerceivedObject");
1315 }
1316 gm::Vector3 acceleration;
1317
1318 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1319 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1320 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1321 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1322 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1323 }
1324 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1325 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1326 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1327 acceleration.x = magnitude * cos(angle);
1328 acceleration.y = magnitude * sin(angle);
1329 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1330 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1331 }
1332 } else {
1333 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1334 }
1335
1336 return acceleration;
1337}
1338
1347inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1348 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1349
1350 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1351 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1352 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1353 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1354 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1355 : std::numeric_limits<double>::infinity();
1356 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1357 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1358 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1359 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1360 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1361 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1362 double lateral_confidence = magnitude * angle_confidence; // best approximation
1363 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1364 + lateral_confidence * sin(angle) * sin(angle);
1365 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1366 + lateral_confidence * cos(angle) * cos(angle);
1367 // neglect xy covariance, as it is not present in the output of this function
1368 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1369 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1370 : std::numeric_limits<double>::infinity();
1371 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1372 } else {
1373 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1374 }
1375}
1376
1388inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1389 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1390 return object.object_dimension_x.value.value;
1391}
1392
1400inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1401 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1402 return object.object_dimension_x.confidence.value;
1403}
1404
1416inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1417 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1418 return object.object_dimension_y.value.value;
1419}
1420
1428inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1429 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1430 return object.object_dimension_y.confidence.value;
1431}
1432
1444inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1445 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1446 return object.object_dimension_z.value.value;
1447}
1448
1456inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1457 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1458 return object.object_dimension_z.confidence.value;
1459}
1460
1470inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1471 gm::Vector3 dimensions;
1472 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1473 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1474 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1475 return dimensions;
1476}
1477
1484inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1485 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1486 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1487 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1488 return {conf_x, conf_y, conf_z};
1489}
1490
1501inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1502 const PerceivedObject &object) {
1503 gm::PointStamped utm_position;
1504 gm::PointStamped reference_position = getUTMPosition(cpm);
1505 gm::Point relative_position = getPositionOfPerceivedObject(object);
1506
1507 utm_position.header.frame_id = reference_position.header.frame_id;
1508 utm_position.point.x = reference_position.point.x + relative_position.x;
1509 utm_position.point.y = reference_position.point.y + relative_position.y;
1510 utm_position.point.z = reference_position.point.z + relative_position.z;
1511
1512 return utm_position;
1513}
1514
1524inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1525 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1526}
1527
1534inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1535 return sensor_information.sensor_id.value;
1536}
1537
1544inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1545 return sensor_information.sensor_type.value;
1546}
1547
1548} // namespace etsi_its_cpm_ts_msgs::access

◆ getCpmContainer()

WrappedCpmContainer etsi_its_cpm_ts_msgs::access::getCpmContainer ( const CollectivePerceptionMessage & cpm,
const uint8_t container_id )
inline

Retrieves the CpmContainer with the specified container ID from the CPM.

Parameters
cpmThe CPM from which to retrieve the CpmContainer.
container_idThe ID of the CpmContainer to retrieve.
Returns
The CpmContainer with the specified container ID.
Exceptions
std::invalid_argumentif no CpmContainer with the specified ID is found in the CPM.

Definition at line 504 of file cpm_ts_getters.h.

506 {
507 return double(acceleration.value.value) / 10.0;
508}
509

◆ getCpmContainerIds()

std::vector< uint8_t > etsi_its_cpm_ts_msgs::access::getCpmContainerIds ( const CollectivePerceptionMessage & cpm)
inline

Retrieves the container IDs from a CPM.

This function iterates over the cpm_containers array in the given CPM and extracts the container IDs into a vector of uint8_t.

Parameters
cpmThe CPM from which to retrieve the container IDs.
Returns
A vector containing the container IDs.

Definition at line 488 of file cpm_ts_getters.h.

◆ getDimensionsConfidenceOfPerceivedObject()

std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getDimensionsConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Dimensions Confidence Of Perceived Object.

Parameters
objectThe PerceivedObject to get the dimensions confidence from
Returns
std::tuple<double, double, double> the x,y and z standard deviations of the dimensions in meters

Definition at line 1050 of file cpm_ts_getters.h.

1097inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1098
1105inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1106 return cpm.payload.management_container.reference_time;
1107}
1108
1115inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1116
1123inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1124 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1125}
1126
1133inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1134 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1135}
1136
1143inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1144 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1145}
1146
1158inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1159 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1160}
1161
1171inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1172 int zone;
1173 bool northp;
1174 return getUTMPosition(cpm, zone, northp);
1175}
1176
1186inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1187 std::vector<uint8_t> container_ids;
1188 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1189 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1190 }
1191 return container_ids;
1192}
1193
1202inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1203 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1204 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1205 return cpm.payload.cpm_containers.value.array[i];
1206 }
1207 }
1208 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1209}
1210
1219inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1220 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1221}
1222
1230inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1231 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1232 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1233 }
1234 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1235 return number;
1236}
1237
1244inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1245 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1246}
1247
1251
1260inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1261 if (i >= getNumberOfPerceivedObjects(container)) {
1262 throw std::invalid_argument("Index out of range");
1263 }
1264 return container.container_data_perceived_object_container.perceived_objects.array[i];
1265}
1266
1275inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1276 if (!object.object_id_is_present) {
1277 throw std::invalid_argument("No object_id present in PerceivedObject");
1278 }
1279 return object.object_id.value;
1280}
1281
1288inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1289 return coordinate.value.value;
1290}
1291
1298inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1299 return coordinate.confidence.value;
1300}
1301
1308inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1309 gm::Point point;
1310 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1311 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1312 if (object.position.z_coordinate_is_present) {
1313 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1314 }
1315 return point;
1316}
1317
1325inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1326 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1327 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1328 double z_confidence = object.position.z_coordinate_is_present
1329 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1330 : std::numeric_limits<double>::infinity();
1331 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1332}
1333
1340inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1341
1348inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1349
1360inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1361 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1362 tf2::Quaternion q;
1363 double roll{0}, pitch{0}, yaw{0};
1364
1365 if (object.angles.x_angle_is_present) {
1366 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1367 M_PI;
1368 }
1369 if (object.angles.y_angle_is_present) {
1370 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1371 M_PI;
1372 }
1373 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1374 M_PI;
1375 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1376 q.setRPY(roll, pitch, yaw);
1377
1378 return tf2::toMsg(q);
1379}
1380
1387inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1388 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1389 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1390 double roll, pitch, yaw;
1391 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1392 return yaw;
1393}
1394
1402inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1403 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1404 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1405 yaw_confidence *= M_PI / 180.0; // convert to radians
1406 return yaw_confidence;
1407}
1408
1415inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1416 gm::Pose pose;
1417 pose.position = getPositionOfPerceivedObject(object);
1418 pose.orientation = getOrientationOfPerceivedObject(object);
1419 return pose;
1420}
1421
1429inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1430 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1431 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1432}
1433
1443inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1444 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1445 auto val = object.z_angular_velocity.confidence.value;
1446 static const std::map<uint8_t, double> confidence_map = {
1447 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1448 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1449 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1450 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1451 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1452 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1453 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1454 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1455 };
1456 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1457}
1458
1465inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1466
1473inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1474 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1475}
1476
1484inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1485 if (!object.velocity_is_present) {
1486 throw std::invalid_argument("No velocity present in PerceivedObject");
1487 }
1488 gm::Vector3 velocity;
1489 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1490 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1491 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1492 velocity.x = speed * cos(angle);
1493 velocity.y = speed * sin(angle);
1494 if (object.velocity.polar_velocity.z_velocity_is_present) {
1495 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1496 }
1497 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1498 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1499 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1500 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1501 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1502 }
1503 } else {
1504 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1505 }
1506 return velocity;
1507}
1508
1517inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1518 if (!object.velocity_is_present) {
1519 throw std::invalid_argument("No velocity present in PerceivedObject");
1520 }
1521 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1522 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1523 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1524 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1525 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1526 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1527 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1528 + lateral_confidence * sin(angle) * sin(angle);
1529 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1530 + lateral_confidence * cos(angle) * cos(angle);
1531 // neglect xy covariance, as it is not present in the output of this function
1532 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1533 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1534 : std::numeric_limits<double>::infinity();
1535 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1536 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1537 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1538 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1539 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1540 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1541 : std::numeric_limits<double>::infinity();
1542 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1543 } else {
1544 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1545 }
1546
1547}
1548
1555inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1556 return double(acceleration.value.value) / 10.0;
1557}
1558
1565inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1566 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1567}
1568
1576inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1577 if (!object.acceleration_is_present) {
1578 throw std::invalid_argument("No acceleration present in PerceivedObject");
1579 }
1580 gm::Vector3 acceleration;
1581
1582 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1583 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1584 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1585 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1586 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1587 }
1588 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1589 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1590 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1591 acceleration.x = magnitude * cos(angle);
1592 acceleration.y = magnitude * sin(angle);
1593 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1594 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1595 }
1596 } else {
1597 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1598 }
1599
1600 return acceleration;
1601}
1602
1611inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1612 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1613
1614 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1615 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1616 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1617 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1618 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1619 : std::numeric_limits<double>::infinity();
1620 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1621 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1622 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1623 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1624 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1625 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1626 double lateral_confidence = magnitude * angle_confidence; // best approximation
1627 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1628 + lateral_confidence * sin(angle) * sin(angle);
1629 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1630 + lateral_confidence * cos(angle) * cos(angle);
1631 // neglect xy covariance, as it is not present in the output of this function
1632 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1633 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1634 : std::numeric_limits<double>::infinity();
1635 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1636 } else {
1637 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1638 }
1639}
1640
1652inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1653 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1654 return object.object_dimension_x.value.value;
1655}
1656
1664inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1665 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1666 return object.object_dimension_x.confidence.value;
1667}
1668
1680inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1681 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1682 return object.object_dimension_y.value.value;
1683}
1684
1692inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1693 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1694 return object.object_dimension_y.confidence.value;
1695}
1696
1708inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1709 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1710 return object.object_dimension_z.value.value;
1711}
1712
1720inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1721 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1722 return object.object_dimension_z.confidence.value;
1723}
1724
1734inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1735 gm::Vector3 dimensions;
1736 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1737 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1738 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1739 return dimensions;
1740}
1741
1748inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1749 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1750 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1751 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1752 return {conf_x, conf_y, conf_z};
1753}
1754
1765inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1766 const PerceivedObject &object) {
1767 gm::PointStamped utm_position;
1768 gm::PointStamped reference_position = getUTMPosition(cpm);
1769 gm::Point relative_position = getPositionOfPerceivedObject(object);
1770
1771 utm_position.header.frame_id = reference_position.header.frame_id;
1772 utm_position.point.x = reference_position.point.x + relative_position.x;
1773 utm_position.point.y = reference_position.point.y + relative_position.y;
1774 utm_position.point.z = reference_position.point.z + relative_position.z;
1775
1776 return utm_position;
1777}
1778
1788inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1789 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1790}
1791
1798inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1799 return sensor_information.sensor_id.value;
1800}
1801
1808inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1809 return sensor_information.sensor_type.value;
1810}
1811
1812} // namespace etsi_its_cpm_ts_msgs::access

◆ getDimensionsOfPerceivedObject()

gm::Vector3 etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject ( const PerceivedObject & object)
inline

Retrieves the dimensions of a perceived object.

This function extracts the dimensions of a perceived object from the given PerceivedObject. The dimensions are returned as a gm::Vector3 object with the x, y, and z dimensions in meters.

Parameters
objectThe PerceivedObject for which to calculate the dimensions.
Returns
The dimensions of the perceived object as a gm::Vector3 object in meters.

Definition at line 1036 of file cpm_ts_getters.h.

1071 {
1072
1073#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1074
1083inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1084
1091inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1092 return cpm.payload.management_container.reference_time;
1093}
1094
1101inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1102
1109inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1110 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1111}
1112
1119inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1120 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1121}
1122
1129inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1130 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1131}
1132
1144inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1145 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1146}
1147
1157inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1158 int zone;
1159 bool northp;
1160 return getUTMPosition(cpm, zone, northp);
1161}
1162
1172inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1173 std::vector<uint8_t> container_ids;
1174 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1175 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1176 }
1177 return container_ids;
1178}
1179
1188inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1189 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1190 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1191 return cpm.payload.cpm_containers.value.array[i];
1192 }
1193 }
1194 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1195}
1196
1205inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1206 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1207}
1208
1216inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1217 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1218 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1219 }
1220 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1221 return number;
1222}
1223
1230inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1231 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1232}
1233
1237
1246inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1247 if (i >= getNumberOfPerceivedObjects(container)) {
1248 throw std::invalid_argument("Index out of range");
1249 }
1250 return container.container_data_perceived_object_container.perceived_objects.array[i];
1251}
1252
1261inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1262 if (!object.object_id_is_present) {
1263 throw std::invalid_argument("No object_id present in PerceivedObject");
1264 }
1265 return object.object_id.value;
1266}
1267
1274inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1275 return coordinate.value.value;
1276}
1277
1284inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1285 return coordinate.confidence.value;
1286}
1287
1294inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1295 gm::Point point;
1296 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1297 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1298 if (object.position.z_coordinate_is_present) {
1299 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1300 }
1301 return point;
1302}
1303
1311inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1312 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1313 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1314 double z_confidence = object.position.z_coordinate_is_present
1315 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1316 : std::numeric_limits<double>::infinity();
1317 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1318}
1319
1326inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1327
1334inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1335
1346inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1347 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1348 tf2::Quaternion q;
1349 double roll{0}, pitch{0}, yaw{0};
1350
1351 if (object.angles.x_angle_is_present) {
1352 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1353 M_PI;
1354 }
1355 if (object.angles.y_angle_is_present) {
1356 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1357 M_PI;
1358 }
1359 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1360 M_PI;
1361 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1362 q.setRPY(roll, pitch, yaw);
1363
1364 return tf2::toMsg(q);
1365}
1366
1373inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1374 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1375 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1376 double roll, pitch, yaw;
1377 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1378 return yaw;
1379}
1380
1388inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1389 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1390 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1391 yaw_confidence *= M_PI / 180.0; // convert to radians
1392 return yaw_confidence;
1393}
1394
1401inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1402 gm::Pose pose;
1403 pose.position = getPositionOfPerceivedObject(object);
1404 pose.orientation = getOrientationOfPerceivedObject(object);
1405 return pose;
1406}
1407
1415inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1416 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1417 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1418}
1419
1429inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1430 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1431 auto val = object.z_angular_velocity.confidence.value;
1432 static const std::map<uint8_t, double> confidence_map = {
1433 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1434 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1435 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1436 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1437 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1438 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1439 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1440 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1441 };
1442 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1443}
1444
1451inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1452
1459inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1460 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1461}
1462
1470inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1471 if (!object.velocity_is_present) {
1472 throw std::invalid_argument("No velocity present in PerceivedObject");
1473 }
1474 gm::Vector3 velocity;
1475 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1476 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1477 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1478 velocity.x = speed * cos(angle);
1479 velocity.y = speed * sin(angle);
1480 if (object.velocity.polar_velocity.z_velocity_is_present) {
1481 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1482 }
1483 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1484 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1485 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1486 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1487 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1488 }
1489 } else {
1490 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1491 }
1492 return velocity;
1493}
1494
1503inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1504 if (!object.velocity_is_present) {
1505 throw std::invalid_argument("No velocity present in PerceivedObject");
1506 }
1507 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1508 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1509 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1510 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1511 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1512 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1513 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1514 + lateral_confidence * sin(angle) * sin(angle);
1515 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1516 + lateral_confidence * cos(angle) * cos(angle);
1517 // neglect xy covariance, as it is not present in the output of this function
1518 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1519 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1520 : std::numeric_limits<double>::infinity();
1521 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1522 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1523 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1524 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1525 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1526 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1527 : std::numeric_limits<double>::infinity();
1528 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1529 } else {
1530 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1531 }
1532
1533}
1534
1541inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1542 return double(acceleration.value.value) / 10.0;
1543}
1544
1551inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1552 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1553}
1554
1562inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1563 if (!object.acceleration_is_present) {
1564 throw std::invalid_argument("No acceleration present in PerceivedObject");
1565 }
1566 gm::Vector3 acceleration;
1567
1568 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1569 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1570 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1571 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1572 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1573 }
1574 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1575 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1576 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1577 acceleration.x = magnitude * cos(angle);
1578 acceleration.y = magnitude * sin(angle);
1579 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1580 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1581 }
1582 } else {
1583 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1584 }
1585
1586 return acceleration;
1587}
1588
1597inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1598 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1599
1600 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1601 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1602 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1603 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1604 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1605 : std::numeric_limits<double>::infinity();
1606 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1607 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1608 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1609 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1610 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1611 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1612 double lateral_confidence = magnitude * angle_confidence; // best approximation
1613 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1614 + lateral_confidence * sin(angle) * sin(angle);
1615 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1616 + lateral_confidence * cos(angle) * cos(angle);
1617 // neglect xy covariance, as it is not present in the output of this function
1618 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1619 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1620 : std::numeric_limits<double>::infinity();
1621 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1622 } else {
1623 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1624 }
1625}
1626
1638inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1639 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1640 return object.object_dimension_x.value.value;
1641}
1642
1650inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1651 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1652 return object.object_dimension_x.confidence.value;
1653}
1654
1666inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1667 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1668 return object.object_dimension_y.value.value;
1669}
1670
1678inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1679 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1680 return object.object_dimension_y.confidence.value;
1681}
1682
1694inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1695 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1696 return object.object_dimension_z.value.value;
1697}
1698
1706inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1707 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1708 return object.object_dimension_z.confidence.value;
1709}
1710
1720inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1721 gm::Vector3 dimensions;
1722 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1723 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1724 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1725 return dimensions;
1726}
1727
1734inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1735 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1736 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1737 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1738 return {conf_x, conf_y, conf_z};
1739}
1740
1751inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1752 const PerceivedObject &object) {
1753 gm::PointStamped utm_position;
1754 gm::PointStamped reference_position = getUTMPosition(cpm);
1755 gm::Point relative_position = getPositionOfPerceivedObject(object);
1756
1757 utm_position.header.frame_id = reference_position.header.frame_id;
1758 utm_position.point.x = reference_position.point.x + relative_position.x;
1759 utm_position.point.y = reference_position.point.y + relative_position.y;
1760 utm_position.point.z = reference_position.point.z + relative_position.z;
1761
1762 return utm_position;
1763}
1764
1774inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1775 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1776}
1777
1784inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1785 return sensor_information.sensor_id.value;
1786}
1787
1794inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1795 return sensor_information.sensor_type.value;
1796}
1797
1798} // namespace etsi_its_cpm_ts_msgs::access

◆ getHeadingCDD()

template<typename Heading>
double etsi_its_cpm_ts_msgs::access::getHeadingCDD ( const Heading & heading)
inline

Get the Heading value.

0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West

Parameters
headingto get the Heading value from
Returns
Heading value in degree as decimal number

Definition at line 159 of file cpm_ts_getters.h.

◆ getHeadingConfidenceCDD()

template<typename Heading>
double etsi_its_cpm_ts_msgs::access::getHeadingConfidenceCDD ( const Heading & heading)
inline

Get the Heading value.

0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West

Parameters
headingto get the Heading standard deviation from
Returns
Heading standard deviation in degree as decimal number

Definition at line 170 of file cpm_ts_getters.h.

170{

◆ getIdOfPerceivedObject()

uint16_t etsi_its_cpm_ts_msgs::access::getIdOfPerceivedObject ( const PerceivedObject & object)
inline

Retrieves the ID of a perceived object.

This function takes a PerceivedObject as input and returns the ID of the object.

Parameters
objectThe PerceivedObject for which to retrieve the ID.
Returns
The ID of the perceived object.

Definition at line 577 of file cpm_ts_getters.h.

◆ getLateralAcceleration()

double etsi_its_cpm_ts_msgs::access::getLateralAcceleration ( const AccelerationComponent & lateral_acceleration)
inline

Get the lateral acceleration.

Parameters
lateralAccelerationto get the lateral acceleration from
Returns
lateral acceleration in m/s^2 as decimal number (left is positive)

Definition at line 332 of file cpm_ts_getters.h.

◆ getLateralAccelerationConfidence()

double etsi_its_cpm_ts_msgs::access::getLateralAccelerationConfidence ( const AccelerationComponent & lateral_acceleration)
inline

Get the Lateral Acceleration Confidence.

Parameters
lateral_accelerationto get the LateralAccelerationConfidence from
Returns
double standard deviation of the lateral acceleration in m/s^2 as decimal number

Definition at line 342 of file cpm_ts_getters.h.

◆ getLatitude() [1/2]

double etsi_its_cpm_ts_msgs::access::getLatitude ( const CollectivePerceptionMessage & cpm)
inline

Get the Latitude value of CPM.

Parameters
cpmCPM to get the Latitude value from
Returns
Latitude value in degree as decimal number

Definition at line 425 of file cpm_ts_getters.h.

◆ getLatitude() [2/2]

double etsi_its_cpm_ts_msgs::access::getLatitude ( const Latitude & latitude)
inline

Get the Latitude value.

Parameters
latitudeto get the Latitude value from
Returns
Latitude value in degree as decimal number

Definition at line 55 of file cpm_ts_getters.h.

◆ getLongitude() [1/2]

double etsi_its_cpm_ts_msgs::access::getLongitude ( const CollectivePerceptionMessage & cpm)
inline

Get the Longitude value of CPM.

Parameters
cpmCPM to get the Longitude value from
Returns
Longitude value in degree as decimal number

Definition at line 435 of file cpm_ts_getters.h.

435 {
436 if (!object.velocity_is_present) {
437 throw std::invalid_argument("No velocity present in PerceivedObject");

◆ getLongitude() [2/2]

double etsi_its_cpm_ts_msgs::access::getLongitude ( const Longitude & longitude)
inline

Get the Longitude value.

Parameters
longitudeto get the Longitude value from
Returns
Longitude value in degree as decimal number

Definition at line 63 of file cpm_ts_getters.h.

◆ getLongitudinalAcceleration()

double etsi_its_cpm_ts_msgs::access::getLongitudinalAcceleration ( const AccelerationComponent & longitudinal_acceleration)
inline

Get the longitudinal acceleration.

Parameters
longitudinalAccelerationto get the longitudinal acceleration from
Returns
longitudinal acceleration in m/s^2 as decimal number (left is positive)

Definition at line 312 of file cpm_ts_getters.h.

314 {0}, pitch{0}, yaw{0};

◆ getLongitudinalAccelerationConfidence()

double etsi_its_cpm_ts_msgs::access::getLongitudinalAccelerationConfidence ( const AccelerationComponent & longitudinal_acceleration)
inline

Get the Longitudinal Acceleration Confidence.

Parameters
longitudinal_accelerationto get the LongitudinalAccelerationConfidence from
Returns
double standard deviation of the longitudinal acceleration in m/s^2 as decimal number

Definition at line 322 of file cpm_ts_getters.h.

◆ getNumberOfPerceivedObjects() [1/2]

uint8_t etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects ( const CollectivePerceptionMessage & cpm)
inline

Retrieves the number of perceived objects from the given CPM.

Parameters
cpmThe CPM from which to retrieve the number of perceived objects.
Returns
The number of perceived objects.

Definition at line 546 of file cpm_ts_getters.h.

547 {
548 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");

◆ getNumberOfPerceivedObjects() [2/2]

uint8_t etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects ( const WrappedCpmContainer & container)
inline

Retrieves the number of perceived objects from the given perceived object container.

Parameters
containerThe perceived object container to retrieve the number of perceived objects from.
Returns
The number of perceived objects.
Exceptions
std::invalid_argumentif the container is not a PerceivedObjectContainer.

Definition at line 532 of file cpm_ts_getters.h.

533 {
534 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
535 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
536 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
537 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
538 }

◆ getOrientationOfPerceivedObject()

gm::Quaternion etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject ( const PerceivedObject & object)
inline

Calculates the orientation of a perceived object.

This function calculates the orientation of a perceived object based on the angles provided in the PerceivedObject structure. The angles are converted to radians and used to set the roll, pitch, and yaw values of a tf2::Quaternion object. The resulting quaternion is then converted to a gm::Quaternion message and returned.

Parameters
objectThe PerceivedObject structure containing the angles of the perceived object.
Returns
gm::Quaternion The orientation of the perceived object in radians from -pi to pi.

Definition at line 662 of file cpm_ts_getters.h.

671 {
672 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
673 return object.object_dimension_z.confidence.value;
674}
675

◆ getPerceivedObject()

PerceivedObject etsi_its_cpm_ts_msgs::access::getPerceivedObject ( const WrappedCpmContainer & container,
const uint8_t i )
inline

Retrieves the PerceivedObject at the specified index from the given WrappedCpmContainer.

Parameters
containerThe WrappedCpmContainer from which to retrieve the PerceivedObject (should be a PerceivedObjectContainer).
iThe index of the PerceivedObject to retrieve.
Returns
The PerceivedObject at the specified index.
Exceptions
std::invalid_argumentif the index is out of range.

Definition at line 562 of file cpm_ts_getters.h.

◆ getPerceivedObjectContainer()

WrappedCpmContainer etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer ( const CollectivePerceptionMessage & cpm)
inline

Retrieves the perceived object container from a CPM.

This function returns the perceived object container from the given CPM.

Parameters
cpmThe CPM from which to retrieve the perceived object container.
Returns
The perceived object container.

Definition at line 521 of file cpm_ts_getters.h.

◆ getPosConfidenceEllipse() [1/2]

template<typename PosConfidenceEllipse>
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getPosConfidenceEllipse ( const PosConfidenceEllipse & position_confidence_ellipse)
inline

Extract major axis length, minor axis length and orientation from the given position confidence ellipse.

Parameters
position_confidence_ellipseThe position confidence ellipse to extract the values from
Returns
std::tuple<double, double, double> major axis length in meters, minor axis length in meters, and orientation in degrees

Definition at line 225 of file cpm_ts_getters.h.

◆ getPosConfidenceEllipse() [2/2]

template<typename PosConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getPosConfidenceEllipse ( const PosConfidenceEllipse & position_confidence_ellipse,
const double object_heading )
inline

Get the covariance matrix of the position confidence ellipse.

Parameters
position_confidence_ellipseThe position confidence ellipse to get the covariance matrix from
object_headingThe object heading in radians
Returns
std::array<double, 4> The covariance matrix of the position confidence ellipse in vehicle coordinates (x = longitudinal, y = lateral)

Definition at line 286 of file cpm_ts_getters.h.

◆ getPoseOfPerceivedObject()

gm::Pose etsi_its_cpm_ts_msgs::access::getPoseOfPerceivedObject ( const PerceivedObject & object)
inline

Get the pose of the PerceivedObject.

Parameters
objectPerceivedObject to get the pose from
Returns
gm::Pose pose of the PerceivedObject (position in m, orientation in rad)

Definition at line 717 of file cpm_ts_getters.h.

◆ getPositionConfidenceEllipse() [1/2]

template<typename PositionConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getPositionConfidenceEllipse ( const PositionConfidenceEllipse & position_confidence_ellipse,
const double object_heading )
inline

Get the covariance matrix of the position confidence ellipse.

Parameters
position_confidence_ellipseThe position confidence ellipse to get the covariance matrix from
object_headingThe object heading in radians
Returns
std::array<double, 4> The covariance matrix of the position confidence ellipse in vehicle coordinates (x = longitudinal, y = lateral)

Definition at line 370 of file cpm_ts_getters.h.

◆ getPositionConfidenceEllipse() [2/2]

template<typename PositionConfidenceEllipse>
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getPositionConfidenceEllipse ( PositionConfidenceEllipse & position_confidence_ellipse)
inline

Extract major axis length, minor axis length and orientation from the given position confidence ellipse.

Parameters
position_confidence_ellipseThe position confidence ellipse to extract the values from
Returns
std::tuple<double, double, double> major axis length in meters, minor axis length in meters, and orientation in degrees

Definition at line 354 of file cpm_ts_getters.h.

◆ getPositionConfidenceOfPerceivedObject()

std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::getPositionConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Position Confidences Of Perceived Object.

Parameters
objectThe PerceivedObject to get the position confidences from
Returns
std::tuple<double, double, double> x,y and z standard deviations of the positions in meters. The z standard deviation is infinity if the z coordinate is not present.

Definition at line 627 of file cpm_ts_getters.h.

◆ getPositionOfPerceivedObject()

gm::Point etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject ( const PerceivedObject & object)
inline

Returns the position of a perceived object.

Parameters
objectThe perceived object.
Returns
The position of the perceived object as a gm::Point (all values in meters).

Definition at line 610 of file cpm_ts_getters.h.

◆ getReferenceTime()

TimestampIts etsi_its_cpm_ts_msgs::access::getReferenceTime ( const CollectivePerceptionMessage & cpm)
inline

Get the Reference Time object.

Parameters
cpmCPM to get the ReferenceTime-Value from
Returns
TimestampIts

Definition at line 407 of file cpm_ts_getters.h.

◆ getReferenceTimeValue()

uint64_t etsi_its_cpm_ts_msgs::access::getReferenceTimeValue ( const CollectivePerceptionMessage & cpm)
inline

Get the ReferenceTime-Value.

Parameters
cpmCPM to get the ReferenceTime-Value from
Returns
uint64_t the ReferenceTime-Value in milliseconds

Definition at line 417 of file cpm_ts_getters.h.

◆ getSemiAxis()

template<typename SemiAxisLength>
double etsi_its_cpm_ts_msgs::access::getSemiAxis ( const SemiAxisLength & semi_axis_length)
inline

Get the Semi Axis object.

Parameters
semi_axis_lengthThe SemiAxisLength object to get the semi axis from
Returns
double the semi axis length in meters

Definition at line 214 of file cpm_ts_getters.h.

◆ getSensorID()

uint8_t etsi_its_cpm_ts_msgs::access::getSensorID ( const SensorInformation & sensor_information)
inline

Get the sensorId of a SensorInformation object.

Parameters
sensor_informationThe SensorInformation to get the sensorId from
Returns
uint8_t The sensorId of a SensorInformation object

Definition at line 1100 of file cpm_ts_getters.h.

1147inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1148
1155inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1156 return cpm.payload.management_container.reference_time;
1157}
1158
1165inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1166
1173inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1174 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1175}
1176
1183inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1184 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1185}
1186
1193inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1194 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1195}
1196
1208inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1209 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1210}
1211
1221inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1222 int zone;
1223 bool northp;
1224 return getUTMPosition(cpm, zone, northp);
1225}
1226
1236inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1237 std::vector<uint8_t> container_ids;
1238 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1239 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1240 }
1241 return container_ids;
1242}
1243
1252inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1253 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1254 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1255 return cpm.payload.cpm_containers.value.array[i];
1256 }
1257 }
1258 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1259}
1260
1269inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1270 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1271}
1272
1280inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1281 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1282 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1283 }
1284 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1285 return number;
1286}
1287
1294inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1295 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1296}
1297
1301
1310inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1311 if (i >= getNumberOfPerceivedObjects(container)) {
1312 throw std::invalid_argument("Index out of range");
1313 }
1314 return container.container_data_perceived_object_container.perceived_objects.array[i];
1315}
1316
1325inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1326 if (!object.object_id_is_present) {
1327 throw std::invalid_argument("No object_id present in PerceivedObject");
1328 }
1329 return object.object_id.value;
1330}
1331
1338inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1339 return coordinate.value.value;
1340}
1341
1348inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1349 return coordinate.confidence.value;
1350}
1351
1358inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1359 gm::Point point;
1360 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1361 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1362 if (object.position.z_coordinate_is_present) {
1363 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1364 }
1365 return point;
1366}
1367
1375inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1376 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1377 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1378 double z_confidence = object.position.z_coordinate_is_present
1379 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1380 : std::numeric_limits<double>::infinity();
1381 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1382}
1383
1390inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1391
1398inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1399
1410inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1411 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1412 tf2::Quaternion q;
1413 double roll{0}, pitch{0}, yaw{0};
1414
1415 if (object.angles.x_angle_is_present) {
1416 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1417 M_PI;
1418 }
1419 if (object.angles.y_angle_is_present) {
1420 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1421 M_PI;
1422 }
1423 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1424 M_PI;
1425 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1426 q.setRPY(roll, pitch, yaw);
1427
1428 return tf2::toMsg(q);
1429}
1430
1437inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1438 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1439 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1440 double roll, pitch, yaw;
1441 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1442 return yaw;
1443}
1444
1452inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1453 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1454 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1455 yaw_confidence *= M_PI / 180.0; // convert to radians
1456 return yaw_confidence;
1457}
1458
1465inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1466 gm::Pose pose;
1467 pose.position = getPositionOfPerceivedObject(object);
1468 pose.orientation = getOrientationOfPerceivedObject(object);
1469 return pose;
1470}
1471
1479inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1480 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1481 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1482}
1483
1493inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1494 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1495 auto val = object.z_angular_velocity.confidence.value;
1496 static const std::map<uint8_t, double> confidence_map = {
1497 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1498 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1499 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1500 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1501 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1502 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1503 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1504 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1505 };
1506 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1507}
1508
1515inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1516
1523inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1524 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1525}
1526
1534inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1535 if (!object.velocity_is_present) {
1536 throw std::invalid_argument("No velocity present in PerceivedObject");
1537 }
1538 gm::Vector3 velocity;
1539 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1540 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1541 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1542 velocity.x = speed * cos(angle);
1543 velocity.y = speed * sin(angle);
1544 if (object.velocity.polar_velocity.z_velocity_is_present) {
1545 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1546 }
1547 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1548 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1549 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1550 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1551 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1552 }
1553 } else {
1554 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1555 }
1556 return velocity;
1557}
1558
1567inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1568 if (!object.velocity_is_present) {
1569 throw std::invalid_argument("No velocity present in PerceivedObject");
1570 }
1571 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1572 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1573 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1574 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1575 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1576 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1577 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1578 + lateral_confidence * sin(angle) * sin(angle);
1579 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1580 + lateral_confidence * cos(angle) * cos(angle);
1581 // neglect xy covariance, as it is not present in the output of this function
1582 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1583 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1584 : std::numeric_limits<double>::infinity();
1585 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1586 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1587 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1588 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1589 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1590 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1591 : std::numeric_limits<double>::infinity();
1592 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1593 } else {
1594 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1595 }
1596
1597}
1598
1605inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1606 return double(acceleration.value.value) / 10.0;
1607}
1608
1615inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1616 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1617}
1618
1626inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1627 if (!object.acceleration_is_present) {
1628 throw std::invalid_argument("No acceleration present in PerceivedObject");
1629 }
1630 gm::Vector3 acceleration;
1631
1632 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1633 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1634 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1635 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1636 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1637 }
1638 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1639 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1640 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1641 acceleration.x = magnitude * cos(angle);
1642 acceleration.y = magnitude * sin(angle);
1643 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1644 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1645 }
1646 } else {
1647 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1648 }
1649
1650 return acceleration;
1651}
1652
1661inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1662 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1663
1664 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1665 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1666 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1667 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1668 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1669 : std::numeric_limits<double>::infinity();
1670 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1671 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1672 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1673 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1674 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1675 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1676 double lateral_confidence = magnitude * angle_confidence; // best approximation
1677 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1678 + lateral_confidence * sin(angle) * sin(angle);
1679 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1680 + lateral_confidence * cos(angle) * cos(angle);
1681 // neglect xy covariance, as it is not present in the output of this function
1682 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1683 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1684 : std::numeric_limits<double>::infinity();
1685 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1686 } else {
1687 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1688 }
1689}
1690
1702inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1703 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1704 return object.object_dimension_x.value.value;
1705}
1706
1714inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1715 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1716 return object.object_dimension_x.confidence.value;
1717}
1718
1730inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1731 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1732 return object.object_dimension_y.value.value;
1733}
1734
1742inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1743 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1744 return object.object_dimension_y.confidence.value;
1745}
1746
1758inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1759 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1760 return object.object_dimension_z.value.value;
1761}
1762
1770inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1771 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1772 return object.object_dimension_z.confidence.value;
1773}
1774
1784inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1785 gm::Vector3 dimensions;
1786 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1787 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1788 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1789 return dimensions;
1790}
1791
1798inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1799 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1800 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1801 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1802 return {conf_x, conf_y, conf_z};
1803}
1804
1815inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1816 const PerceivedObject &object) {
1817 gm::PointStamped utm_position;
1818 gm::PointStamped reference_position = getUTMPosition(cpm);
1819 gm::Point relative_position = getPositionOfPerceivedObject(object);
1820
1821 utm_position.header.frame_id = reference_position.header.frame_id;
1822 utm_position.point.x = reference_position.point.x + relative_position.x;
1823 utm_position.point.y = reference_position.point.y + relative_position.y;
1824 utm_position.point.z = reference_position.point.z + relative_position.z;
1825
1826 return utm_position;
1827}
1828
1838inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1839 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1840}
1841
1848inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1849 return sensor_information.sensor_id.value;
1850}
1851
1858inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1859 return sensor_information.sensor_type.value;
1860}
1861
1862} // namespace etsi_its_cpm_ts_msgs::access

◆ getSensorType()

uint8_t etsi_its_cpm_ts_msgs::access::getSensorType ( const SensorInformation & sensor_information)
inline

Get the sensorType of a SensorInformation object.

Parameters
sensor_informationThe SensorInformation to get the sensorType from
Returns
uint8_t The sensorType of a SensorInformation object

Definition at line 1110 of file cpm_ts_getters.h.

1145 {
1146
1147#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1148
1157inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1158
1165inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1166 return cpm.payload.management_container.reference_time;
1167}
1168
1175inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1176
1183inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1184 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1185}
1186
1193inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1194 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1195}
1196
1203inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1204 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1205}
1206
1218inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1219 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1220}
1221
1231inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1232 int zone;
1233 bool northp;
1234 return getUTMPosition(cpm, zone, northp);
1235}
1236
1246inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1247 std::vector<uint8_t> container_ids;
1248 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1249 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1250 }
1251 return container_ids;
1252}
1253
1262inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1263 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1264 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1265 return cpm.payload.cpm_containers.value.array[i];
1266 }
1267 }
1268 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1269}
1270
1279inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1280 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1281}
1282
1290inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1291 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1292 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1293 }
1294 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1295 return number;
1296}
1297
1304inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1305 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1306}
1307
1311
1320inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1321 if (i >= getNumberOfPerceivedObjects(container)) {
1322 throw std::invalid_argument("Index out of range");
1323 }
1324 return container.container_data_perceived_object_container.perceived_objects.array[i];
1325}
1326
1335inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1336 if (!object.object_id_is_present) {
1337 throw std::invalid_argument("No object_id present in PerceivedObject");
1338 }
1339 return object.object_id.value;
1340}
1341
1348inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1349 return coordinate.value.value;
1350}
1351
1358inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1359 return coordinate.confidence.value;
1360}
1361
1368inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1369 gm::Point point;
1370 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1371 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1372 if (object.position.z_coordinate_is_present) {
1373 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1374 }
1375 return point;
1376}
1377
1385inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1386 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1387 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1388 double z_confidence = object.position.z_coordinate_is_present
1389 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1390 : std::numeric_limits<double>::infinity();
1391 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1392}
1393
1400inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1401
1408inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1409
1420inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1421 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1422 tf2::Quaternion q;
1423 double roll{0}, pitch{0}, yaw{0};
1424
1425 if (object.angles.x_angle_is_present) {
1426 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1427 M_PI;
1428 }
1429 if (object.angles.y_angle_is_present) {
1430 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1431 M_PI;
1432 }
1433 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1434 M_PI;
1435 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1436 q.setRPY(roll, pitch, yaw);
1437
1438 return tf2::toMsg(q);
1439}
1440
1447inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1448 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1449 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1450 double roll, pitch, yaw;
1451 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1452 return yaw;
1453}
1454
1462inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1463 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1464 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1465 yaw_confidence *= M_PI / 180.0; // convert to radians
1466 return yaw_confidence;
1467}
1468
1475inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1476 gm::Pose pose;
1477 pose.position = getPositionOfPerceivedObject(object);
1478 pose.orientation = getOrientationOfPerceivedObject(object);
1479 return pose;
1480}
1481
1489inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1490 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1491 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1492}
1493
1503inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1504 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1505 auto val = object.z_angular_velocity.confidence.value;
1506 static const std::map<uint8_t, double> confidence_map = {
1507 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1508 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1509 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1510 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1511 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1512 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1513 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1514 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1515 };
1516 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1517}
1518
1525inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1526
1533inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1534 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1535}
1536
1544inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1545 if (!object.velocity_is_present) {
1546 throw std::invalid_argument("No velocity present in PerceivedObject");
1547 }
1548 gm::Vector3 velocity;
1549 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1550 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1551 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1552 velocity.x = speed * cos(angle);
1553 velocity.y = speed * sin(angle);
1554 if (object.velocity.polar_velocity.z_velocity_is_present) {
1555 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1556 }
1557 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1558 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1559 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1560 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1561 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1562 }
1563 } else {
1564 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1565 }
1566 return velocity;
1567}
1568
1577inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1578 if (!object.velocity_is_present) {
1579 throw std::invalid_argument("No velocity present in PerceivedObject");
1580 }
1581 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1582 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1583 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1584 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1585 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1586 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1587 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1588 + lateral_confidence * sin(angle) * sin(angle);
1589 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1590 + lateral_confidence * cos(angle) * cos(angle);
1591 // neglect xy covariance, as it is not present in the output of this function
1592 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1593 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1594 : std::numeric_limits<double>::infinity();
1595 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1596 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1597 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1598 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1599 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1600 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1601 : std::numeric_limits<double>::infinity();
1602 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1603 } else {
1604 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1605 }
1606
1607}
1608
1615inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1616 return double(acceleration.value.value) / 10.0;
1617}
1618
1625inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1626 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1627}
1628
1636inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1637 if (!object.acceleration_is_present) {
1638 throw std::invalid_argument("No acceleration present in PerceivedObject");
1639 }
1640 gm::Vector3 acceleration;
1641
1642 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1643 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1644 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1645 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1646 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1647 }
1648 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1649 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1650 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1651 acceleration.x = magnitude * cos(angle);
1652 acceleration.y = magnitude * sin(angle);
1653 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1654 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1655 }
1656 } else {
1657 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1658 }
1659
1660 return acceleration;
1661}
1662
1671inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1672 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1673
1674 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1675 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1676 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1677 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1678 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1679 : std::numeric_limits<double>::infinity();
1680 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1681 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1682 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1683 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1684 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1685 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1686 double lateral_confidence = magnitude * angle_confidence; // best approximation
1687 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1688 + lateral_confidence * sin(angle) * sin(angle);
1689 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1690 + lateral_confidence * cos(angle) * cos(angle);
1691 // neglect xy covariance, as it is not present in the output of this function
1692 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1693 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1694 : std::numeric_limits<double>::infinity();
1695 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1696 } else {
1697 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1698 }
1699}
1700
1712inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1713 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1714 return object.object_dimension_x.value.value;
1715}
1716
1724inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1725 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1726 return object.object_dimension_x.confidence.value;
1727}
1728
1740inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1741 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1742 return object.object_dimension_y.value.value;
1743}
1744
1752inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1753 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1754 return object.object_dimension_y.confidence.value;
1755}
1756
1768inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1769 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1770 return object.object_dimension_z.value.value;
1771}
1772
1780inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1781 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1782 return object.object_dimension_z.confidence.value;
1783}
1784
1794inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1795 gm::Vector3 dimensions;
1796 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1797 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1798 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1799 return dimensions;
1800}
1801
1808inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1809 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1810 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1811 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1812 return {conf_x, conf_y, conf_z};
1813}
1814
1825inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1826 const PerceivedObject &object) {
1827 gm::PointStamped utm_position;
1828 gm::PointStamped reference_position = getUTMPosition(cpm);
1829 gm::Point relative_position = getPositionOfPerceivedObject(object);
1830
1831 utm_position.header.frame_id = reference_position.header.frame_id;
1832 utm_position.point.x = reference_position.point.x + relative_position.x;
1833 utm_position.point.y = reference_position.point.y + relative_position.y;
1834 utm_position.point.z = reference_position.point.z + relative_position.z;
1835
1836 return utm_position;
1837}
1838
1848inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1849 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1850}
1851
1858inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1859 return sensor_information.sensor_id.value;
1860}
1861
1868inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1869 return sensor_information.sensor_type.value;
1870}
1871
1872} // namespace etsi_its_cpm_ts_msgs::access

◆ getSpeed()

double etsi_its_cpm_ts_msgs::access::getSpeed ( const Speed & speed)
inline

Get the vehicle speed.

Parameters
speedto get the speed value from
Returns
speed value in m/s as decimal number

Definition at line 84 of file cpm_ts_getters.h.

84{

◆ getSpeedConfidence()

double etsi_its_cpm_ts_msgs::access::getSpeedConfidence ( const Speed & speed)
inline

Get the Speed Confidence.

Parameters
speedto get the SpeedConfidence from
Returns
double speed standard deviation in m/s as decimal number

Definition at line 92 of file cpm_ts_getters.h.

94 {

◆ getStationID() [1/2]

uint32_t etsi_its_cpm_ts_msgs::access::getStationID ( const CollectivePerceptionMessage & cpm)
inline

Retrieves the station ID from the given CPM.

This function extracts the station ID from the header of the provided CPM.

Parameters
cpmThe CPM from which to retrieve the station ID.
Returns
The station ID extracted from the header of the CPM.

Definition at line 399 of file cpm_ts_getters.h.

399{AngularSpeedConfidence::DEG_SEC_01, 1.0},

◆ getStationID() [2/2]

uint32_t etsi_its_cpm_ts_msgs::access::getStationID ( const ItsPduHeader & header)
inline

Get the StationID of ItsPduHeader.

Parameters
headerItsPduHeader to get the StationID value from
Returns
stationID value

Definition at line 47 of file cpm_ts_getters.h.

◆ getUTMPosition() [1/3]

gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPosition ( const CollectivePerceptionMessage & cpm)
inline

Get the UTM Position defined within the ManagementContainer of the CPM.

The position is transformed into UTM by using GeographicLib::UTMUPS The altitude value is directly used as z-Coordinate

Parameters
cpmCPM from which to extract the UTM position.
Returns
geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id

Definition at line 473 of file cpm_ts_getters.h.

◆ getUTMPosition() [2/3]

gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPosition ( const CollectivePerceptionMessage & cpm,
int & zone,
bool & northp )
inline

Get the UTM Position defined within the ManagementContainer of the CPM.

The position is transformed into UTM by using GeographicLib::UTMUPS The altitude value is directly used as z-Coordinate

Parameters
[in]cpmCPM to get the UTM Position from
[out]zonethe UTM zone (zero means UPS)
[out]northphemisphere (true means north, false means south)
Returns
geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id

Definition at line 460 of file cpm_ts_getters.h.

◆ getUTMPosition() [3/3]

template<typename T>
gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPosition ( const T & reference_position,
int & zone,
bool & northp )
inline

Get the UTM Position defined by the given ReferencePosition.

The position is transformed into UTM by using GeographicLib::UTMUPS The altitude value is directly used as z-Coordinate

Parameters
[in]reference_positionReferencePosition or ReferencePositionWithConfidence to get the UTM Position from
[out]zonethe UTM zone (zero means UPS)
[out]northphemisphere (true means north, false means south)
Returns
gm::PointStamped geometry_msgs::PointStamped of the given position

Definition at line 130 of file cpm_ts_getters.h.

137 {
138 std::vector<uint8_t> container_ids;
139 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
140 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
141 }
142 return container_ids;
143}
144

◆ getUTMPositionOfPerceivedObject()

gm::PointStamped etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject ( const CollectivePerceptionMessage & cpm,
const PerceivedObject & object )
inline

Calculates the UTM position of a perceived object based on the CPMs referencep position.

This function takes a CPM and a PerceivedObject as input parameters and calculates the UTM position of the object. The UTM position is calculated by adding the relative position of the object to the UTM position of the reference point in the CPM.

Parameters
cpmThe Collective Perception Message (CPM) containing the reference position.
objectThe PerceivedObject for which the UTM position needs to be calculated.
Returns
The UTM position of the perceived object.

Definition at line 1067 of file cpm_ts_getters.h.

1114inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1115
1122inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1123 return cpm.payload.management_container.reference_time;
1124}
1125
1132inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1133
1140inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1141 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1142}
1143
1150inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1151 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1152}
1153
1160inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1161 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1162}
1163
1175inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1176 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1177}
1178
1188inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1189 int zone;
1190 bool northp;
1191 return getUTMPosition(cpm, zone, northp);
1192}
1193
1203inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1204 std::vector<uint8_t> container_ids;
1205 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1206 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1207 }
1208 return container_ids;
1209}
1210
1219inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1220 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1221 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1222 return cpm.payload.cpm_containers.value.array[i];
1223 }
1224 }
1225 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1226}
1227
1236inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1237 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1238}
1239
1247inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1248 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1249 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1250 }
1251 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1252 return number;
1253}
1254
1261inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1262 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1263}
1264
1268
1277inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1278 if (i >= getNumberOfPerceivedObjects(container)) {
1279 throw std::invalid_argument("Index out of range");
1280 }
1281 return container.container_data_perceived_object_container.perceived_objects.array[i];
1282}
1283
1292inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1293 if (!object.object_id_is_present) {
1294 throw std::invalid_argument("No object_id present in PerceivedObject");
1295 }
1296 return object.object_id.value;
1297}
1298
1305inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1306 return coordinate.value.value;
1307}
1308
1315inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1316 return coordinate.confidence.value;
1317}
1318
1325inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1326 gm::Point point;
1327 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1328 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1329 if (object.position.z_coordinate_is_present) {
1330 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1331 }
1332 return point;
1333}
1334
1342inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1343 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1344 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1345 double z_confidence = object.position.z_coordinate_is_present
1346 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1347 : std::numeric_limits<double>::infinity();
1348 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1349}
1350
1357inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1358
1365inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1366
1377inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1378 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1379 tf2::Quaternion q;
1380 double roll{0}, pitch{0}, yaw{0};
1381
1382 if (object.angles.x_angle_is_present) {
1383 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1384 M_PI;
1385 }
1386 if (object.angles.y_angle_is_present) {
1387 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1388 M_PI;
1389 }
1390 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1391 M_PI;
1392 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1393 q.setRPY(roll, pitch, yaw);
1394
1395 return tf2::toMsg(q);
1396}
1397
1404inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1405 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1406 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1407 double roll, pitch, yaw;
1408 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1409 return yaw;
1410}
1411
1419inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1420 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1421 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1422 yaw_confidence *= M_PI / 180.0; // convert to radians
1423 return yaw_confidence;
1424}
1425
1432inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1433 gm::Pose pose;
1434 pose.position = getPositionOfPerceivedObject(object);
1435 pose.orientation = getOrientationOfPerceivedObject(object);
1436 return pose;
1437}
1438
1446inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1447 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1448 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1449}
1450
1460inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1461 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1462 auto val = object.z_angular_velocity.confidence.value;
1463 static const std::map<uint8_t, double> confidence_map = {
1464 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1465 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1466 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1467 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1468 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1469 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1470 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1471 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1472 };
1473 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1474}
1475
1482inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1483
1490inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1491 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1492}
1493
1501inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1502 if (!object.velocity_is_present) {
1503 throw std::invalid_argument("No velocity present in PerceivedObject");
1504 }
1505 gm::Vector3 velocity;
1506 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1507 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1508 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1509 velocity.x = speed * cos(angle);
1510 velocity.y = speed * sin(angle);
1511 if (object.velocity.polar_velocity.z_velocity_is_present) {
1512 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1513 }
1514 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1515 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1516 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1517 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1518 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1519 }
1520 } else {
1521 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1522 }
1523 return velocity;
1524}
1525
1534inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1535 if (!object.velocity_is_present) {
1536 throw std::invalid_argument("No velocity present in PerceivedObject");
1537 }
1538 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1539 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1540 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1541 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1542 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1543 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1544 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1545 + lateral_confidence * sin(angle) * sin(angle);
1546 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1547 + lateral_confidence * cos(angle) * cos(angle);
1548 // neglect xy covariance, as it is not present in the output of this function
1549 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1550 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1551 : std::numeric_limits<double>::infinity();
1552 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1553 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1554 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1555 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1556 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1557 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1558 : std::numeric_limits<double>::infinity();
1559 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1560 } else {
1561 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1562 }
1563
1564}
1565
1572inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1573 return double(acceleration.value.value) / 10.0;
1574}
1575
1582inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1583 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1584}
1585
1593inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1594 if (!object.acceleration_is_present) {
1595 throw std::invalid_argument("No acceleration present in PerceivedObject");
1596 }
1597 gm::Vector3 acceleration;
1598
1599 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1600 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1601 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1602 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1603 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1604 }
1605 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1606 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1607 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1608 acceleration.x = magnitude * cos(angle);
1609 acceleration.y = magnitude * sin(angle);
1610 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1611 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1612 }
1613 } else {
1614 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1615 }
1616
1617 return acceleration;
1618}
1619
1628inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1629 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1630
1631 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1632 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1633 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1634 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1635 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1636 : std::numeric_limits<double>::infinity();
1637 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1638 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1639 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1640 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1641 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1642 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1643 double lateral_confidence = magnitude * angle_confidence; // best approximation
1644 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1645 + lateral_confidence * sin(angle) * sin(angle);
1646 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1647 + lateral_confidence * cos(angle) * cos(angle);
1648 // neglect xy covariance, as it is not present in the output of this function
1649 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1650 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1651 : std::numeric_limits<double>::infinity();
1652 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1653 } else {
1654 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1655 }
1656}
1657
1669inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1670 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1671 return object.object_dimension_x.value.value;
1672}
1673
1681inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1682 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1683 return object.object_dimension_x.confidence.value;
1684}
1685
1697inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1698 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1699 return object.object_dimension_y.value.value;
1700}
1701
1709inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1710 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1711 return object.object_dimension_y.confidence.value;
1712}
1713
1725inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1726 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1727 return object.object_dimension_z.value.value;
1728}
1729
1737inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1738 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1739 return object.object_dimension_z.confidence.value;
1740}
1741
1751inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1752 gm::Vector3 dimensions;
1753 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1754 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1755 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1756 return dimensions;
1757}
1758
1765inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1766 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1767 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1768 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1769 return {conf_x, conf_y, conf_z};
1770}
1771
1782inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1783 const PerceivedObject &object) {
1784 gm::PointStamped utm_position;
1785 gm::PointStamped reference_position = getUTMPosition(cpm);
1786 gm::Point relative_position = getPositionOfPerceivedObject(object);
1787
1788 utm_position.header.frame_id = reference_position.header.frame_id;
1789 utm_position.point.x = reference_position.point.x + relative_position.x;
1790 utm_position.point.y = reference_position.point.y + relative_position.y;
1791 utm_position.point.z = reference_position.point.z + relative_position.z;
1792
1793 return utm_position;
1794}
1795
1805inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1806 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1807}
1808
1815inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1816 return sensor_information.sensor_id.value;
1817}
1818
1825inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1826 return sensor_information.sensor_type.value;
1827}
1828
1829} // namespace etsi_its_cpm_ts_msgs::access

◆ getVelocityComponent()

double etsi_its_cpm_ts_msgs::access::getVelocityComponent ( const VelocityComponent & velocity)
inline

Get the velocity component of the PerceivedObject.

Parameters
velocityVelocityComponent to get the value from
Returns
double value of the velocity component in m/s

Definition at line 767 of file cpm_ts_getters.h.

802{
803
804#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
805
814inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
815
822inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
823 return cpm.payload.management_container.reference_time;
824}
825
832inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
833
840inline double getLatitude(const CollectivePerceptionMessage &cpm) {
841 return getLatitude(cpm.payload.management_container.reference_position.latitude);
842}
843
850inline double getLongitude(const CollectivePerceptionMessage &cpm) {
851 return getLongitude(cpm.payload.management_container.reference_position.longitude);
852}
853
860inline double getAltitude(const CollectivePerceptionMessage &cpm) {
861 return getAltitude(cpm.payload.management_container.reference_position.altitude);
862}
863
875inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
876 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
877}
878
888inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
889 int zone;
890 bool northp;
891 return getUTMPosition(cpm, zone, northp);
892}
893
903inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
904 std::vector<uint8_t> container_ids;
905 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
906 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
907 }
908 return container_ids;
909}
910
919inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
920 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
921 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
922 return cpm.payload.cpm_containers.value.array[i];
923 }
924 }
925 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
926}
927
936inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
937 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
938}
939
947inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
948 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
949 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
950 }
951 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
952 return number;
953}
954
961inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
962 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
963}
964
968
977inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
978 if (i >= getNumberOfPerceivedObjects(container)) {
979 throw std::invalid_argument("Index out of range");
980 }
981 return container.container_data_perceived_object_container.perceived_objects.array[i];
982}
983
992inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
993 if (!object.object_id_is_present) {
994 throw std::invalid_argument("No object_id present in PerceivedObject");
995 }
996 return object.object_id.value;
997}
998
1005inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1006 return coordinate.value.value;
1007}
1008
1015inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1016 return coordinate.confidence.value;
1017}
1018
1025inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1026 gm::Point point;
1027 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1028 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1029 if (object.position.z_coordinate_is_present) {
1030 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1031 }
1032 return point;
1033}
1034
1042inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1043 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1044 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1045 double z_confidence = object.position.z_coordinate_is_present
1046 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1047 : std::numeric_limits<double>::infinity();
1048 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1049}
1050
1057inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1058
1065inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1066
1077inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1078 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1079 tf2::Quaternion q;
1080 double roll{0}, pitch{0}, yaw{0};
1081
1082 if (object.angles.x_angle_is_present) {
1083 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1084 M_PI;
1085 }
1086 if (object.angles.y_angle_is_present) {
1087 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1088 M_PI;
1089 }
1090 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1091 M_PI;
1092 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1093 q.setRPY(roll, pitch, yaw);
1094
1095 return tf2::toMsg(q);
1096}
1097
1104inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1105 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1106 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1107 double roll, pitch, yaw;
1108 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1109 return yaw;
1110}
1111
1119inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1120 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1121 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1122 yaw_confidence *= M_PI / 180.0; // convert to radians
1123 return yaw_confidence;
1124}
1125
1132inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1133 gm::Pose pose;
1134 pose.position = getPositionOfPerceivedObject(object);
1135 pose.orientation = getOrientationOfPerceivedObject(object);
1136 return pose;
1137}
1138
1146inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1147 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1148 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1149}
1150
1160inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1161 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1162 auto val = object.z_angular_velocity.confidence.value;
1163 static const std::map<uint8_t, double> confidence_map = {
1164 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1165 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1166 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1167 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1168 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1169 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1170 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1171 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1172 };
1173 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1174}
1175
1182inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1183
1190inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1191 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1192}
1193
1201inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1202 if (!object.velocity_is_present) {
1203 throw std::invalid_argument("No velocity present in PerceivedObject");
1204 }
1205 gm::Vector3 velocity;
1206 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1207 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1208 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1209 velocity.x = speed * cos(angle);
1210 velocity.y = speed * sin(angle);
1211 if (object.velocity.polar_velocity.z_velocity_is_present) {
1212 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1213 }
1214 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1215 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1216 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1217 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1218 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1219 }
1220 } else {
1221 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1222 }
1223 return velocity;
1224}
1225
1234inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1235 if (!object.velocity_is_present) {
1236 throw std::invalid_argument("No velocity present in PerceivedObject");
1237 }
1238 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1239 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1240 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1241 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1242 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1243 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1244 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1245 + lateral_confidence * sin(angle) * sin(angle);
1246 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1247 + lateral_confidence * cos(angle) * cos(angle);
1248 // neglect xy covariance, as it is not present in the output of this function
1249 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1250 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1251 : std::numeric_limits<double>::infinity();
1252 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1253 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1254 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1255 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1256 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1257 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1258 : std::numeric_limits<double>::infinity();
1259 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1260 } else {
1261 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1262 }
1263
1264}
1265
1272inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1273 return double(acceleration.value.value) / 10.0;
1274}
1275
1282inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1283 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1284}
1285
1293inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1294 if (!object.acceleration_is_present) {
1295 throw std::invalid_argument("No acceleration present in PerceivedObject");
1296 }
1297 gm::Vector3 acceleration;
1298
1299 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1300 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1301 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1302 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1303 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1304 }
1305 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1306 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1307 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1308 acceleration.x = magnitude * cos(angle);
1309 acceleration.y = magnitude * sin(angle);
1310 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1311 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1312 }
1313 } else {
1314 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1315 }
1316
1317 return acceleration;
1318}
1319
1328inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1329 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1330
1331 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1332 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1333 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1334 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1335 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1336 : std::numeric_limits<double>::infinity();
1337 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1338 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1339 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1340 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1341 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1342 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1343 double lateral_confidence = magnitude * angle_confidence; // best approximation
1344 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1345 + lateral_confidence * sin(angle) * sin(angle);
1346 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1347 + lateral_confidence * cos(angle) * cos(angle);
1348 // neglect xy covariance, as it is not present in the output of this function
1349 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1350 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1351 : std::numeric_limits<double>::infinity();
1352 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1353 } else {
1354 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1355 }
1356}
1357
1369inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1370 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1371 return object.object_dimension_x.value.value;
1372}
1373
1381inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1382 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1383 return object.object_dimension_x.confidence.value;
1384}
1385
1397inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1398 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1399 return object.object_dimension_y.value.value;
1400}
1401
1409inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1410 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1411 return object.object_dimension_y.confidence.value;
1412}
1413
1425inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1426 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1427 return object.object_dimension_z.value.value;
1428}
1429
1437inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1438 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1439 return object.object_dimension_z.confidence.value;
1440}
1441
1451inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1452 gm::Vector3 dimensions;
1453 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1454 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1455 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1456 return dimensions;
1457}
1458
1465inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1466 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1467 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1468 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1469 return {conf_x, conf_y, conf_z};
1470}
1471
1482inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1483 const PerceivedObject &object) {
1484 gm::PointStamped utm_position;
1485 gm::PointStamped reference_position = getUTMPosition(cpm);
1486 gm::Point relative_position = getPositionOfPerceivedObject(object);
1487
1488 utm_position.header.frame_id = reference_position.header.frame_id;
1489 utm_position.point.x = reference_position.point.x + relative_position.x;
1490 utm_position.point.y = reference_position.point.y + relative_position.y;
1491 utm_position.point.z = reference_position.point.z + relative_position.z;
1492
1493 return utm_position;
1494}
1495
1505inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1506 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1507}
1508
1515inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1516 return sensor_information.sensor_id.value;
1517}
1518
1525inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1526 return sensor_information.sensor_type.value;
1527}
1528
1529} // namespace etsi_its_cpm_ts_msgs::access

◆ getVelocityComponentConfidence()

double etsi_its_cpm_ts_msgs::access::getVelocityComponentConfidence ( const VelocityComponent & velocity)
inline

Get the confidence of the velocity component.

Parameters
velocityVelocityComponent to get the confidence from
Returns
double value of the confidence of the velocity component in m/s

Definition at line 775 of file cpm_ts_getters.h.

810 {
811
812#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
813
822inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
823
830inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
831 return cpm.payload.management_container.reference_time;
832}
833
840inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
841
848inline double getLatitude(const CollectivePerceptionMessage &cpm) {
849 return getLatitude(cpm.payload.management_container.reference_position.latitude);
850}
851
858inline double getLongitude(const CollectivePerceptionMessage &cpm) {
859 return getLongitude(cpm.payload.management_container.reference_position.longitude);
860}
861
868inline double getAltitude(const CollectivePerceptionMessage &cpm) {
869 return getAltitude(cpm.payload.management_container.reference_position.altitude);
870}
871
883inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
884 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
885}
886
896inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
897 int zone;
898 bool northp;
899 return getUTMPosition(cpm, zone, northp);
900}
901
911inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
912 std::vector<uint8_t> container_ids;
913 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
914 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
915 }
916 return container_ids;
917}
918
927inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
928 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
929 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
930 return cpm.payload.cpm_containers.value.array[i];
931 }
932 }
933 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
934}
935
944inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
945 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
946}
947
955inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
956 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
957 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
958 }
959 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
960 return number;
961}
962
969inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
970 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
971}
972
976
985inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
986 if (i >= getNumberOfPerceivedObjects(container)) {
987 throw std::invalid_argument("Index out of range");
988 }
989 return container.container_data_perceived_object_container.perceived_objects.array[i];
990}
991
1000inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1001 if (!object.object_id_is_present) {
1002 throw std::invalid_argument("No object_id present in PerceivedObject");
1003 }
1004 return object.object_id.value;
1005}
1006
1013inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1014 return coordinate.value.value;
1015}
1016
1023inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1024 return coordinate.confidence.value;
1025}
1026
1033inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1034 gm::Point point;
1035 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1036 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1037 if (object.position.z_coordinate_is_present) {
1038 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1039 }
1040 return point;
1041}
1042
1050inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1051 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1052 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1053 double z_confidence = object.position.z_coordinate_is_present
1054 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1055 : std::numeric_limits<double>::infinity();
1056 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1057}
1058
1065inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1066
1073inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1074
1085inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1086 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1087 tf2::Quaternion q;
1088 double roll{0}, pitch{0}, yaw{0};
1089
1090 if (object.angles.x_angle_is_present) {
1091 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1092 M_PI;
1093 }
1094 if (object.angles.y_angle_is_present) {
1095 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1096 M_PI;
1097 }
1098 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1099 M_PI;
1100 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1101 q.setRPY(roll, pitch, yaw);
1102
1103 return tf2::toMsg(q);
1104}
1105
1112inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1113 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1114 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1115 double roll, pitch, yaw;
1116 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1117 return yaw;
1118}
1119
1127inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1128 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1129 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1130 yaw_confidence *= M_PI / 180.0; // convert to radians
1131 return yaw_confidence;
1132}
1133
1140inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1141 gm::Pose pose;
1142 pose.position = getPositionOfPerceivedObject(object);
1143 pose.orientation = getOrientationOfPerceivedObject(object);
1144 return pose;
1145}
1146
1154inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1155 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1156 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1157}
1158
1168inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1169 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1170 auto val = object.z_angular_velocity.confidence.value;
1171 static const std::map<uint8_t, double> confidence_map = {
1172 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1173 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1174 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1175 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1176 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1177 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1178 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1179 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1180 };
1181 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1182}
1183
1190inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1191
1198inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1199 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1200}
1201
1209inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1210 if (!object.velocity_is_present) {
1211 throw std::invalid_argument("No velocity present in PerceivedObject");
1212 }
1213 gm::Vector3 velocity;
1214 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1215 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1216 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1217 velocity.x = speed * cos(angle);
1218 velocity.y = speed * sin(angle);
1219 if (object.velocity.polar_velocity.z_velocity_is_present) {
1220 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1221 }
1222 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1223 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1224 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1225 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1226 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1227 }
1228 } else {
1229 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1230 }
1231 return velocity;
1232}
1233
1242inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1243 if (!object.velocity_is_present) {
1244 throw std::invalid_argument("No velocity present in PerceivedObject");
1245 }
1246 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1247 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1248 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1249 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1250 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1251 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1252 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1253 + lateral_confidence * sin(angle) * sin(angle);
1254 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1255 + lateral_confidence * cos(angle) * cos(angle);
1256 // neglect xy covariance, as it is not present in the output of this function
1257 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1258 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1259 : std::numeric_limits<double>::infinity();
1260 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1261 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1262 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1263 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1264 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1265 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1266 : std::numeric_limits<double>::infinity();
1267 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1268 } else {
1269 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1270 }
1271
1272}
1273
1280inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1281 return double(acceleration.value.value) / 10.0;
1282}
1283
1290inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1291 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1292}
1293
1301inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1302 if (!object.acceleration_is_present) {
1303 throw std::invalid_argument("No acceleration present in PerceivedObject");
1304 }
1305 gm::Vector3 acceleration;
1306
1307 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1308 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1309 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1310 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1311 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1312 }
1313 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1314 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1315 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1316 acceleration.x = magnitude * cos(angle);
1317 acceleration.y = magnitude * sin(angle);
1318 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1319 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1320 }
1321 } else {
1322 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1323 }
1324
1325 return acceleration;
1326}
1327
1336inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1337 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1338
1339 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1340 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1341 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1342 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1343 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1344 : std::numeric_limits<double>::infinity();
1345 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1346 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1347 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1348 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1349 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1350 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1351 double lateral_confidence = magnitude * angle_confidence; // best approximation
1352 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1353 + lateral_confidence * sin(angle) * sin(angle);
1354 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1355 + lateral_confidence * cos(angle) * cos(angle);
1356 // neglect xy covariance, as it is not present in the output of this function
1357 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1358 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1359 : std::numeric_limits<double>::infinity();
1360 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1361 } else {
1362 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1363 }
1364}
1365
1377inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1378 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1379 return object.object_dimension_x.value.value;
1380}
1381
1389inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1390 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1391 return object.object_dimension_x.confidence.value;
1392}
1393
1405inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1406 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1407 return object.object_dimension_y.value.value;
1408}
1409
1417inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1418 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1419 return object.object_dimension_y.confidence.value;
1420}
1421
1433inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1434 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1435 return object.object_dimension_z.value.value;
1436}
1437
1445inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1446 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1447 return object.object_dimension_z.confidence.value;
1448}
1449
1459inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1460 gm::Vector3 dimensions;
1461 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1462 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1463 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1464 return dimensions;
1465}
1466
1473inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1474 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1475 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1476 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1477 return {conf_x, conf_y, conf_z};
1478}
1479
1490inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1491 const PerceivedObject &object) {
1492 gm::PointStamped utm_position;
1493 gm::PointStamped reference_position = getUTMPosition(cpm);
1494 gm::Point relative_position = getPositionOfPerceivedObject(object);
1495
1496 utm_position.header.frame_id = reference_position.header.frame_id;
1497 utm_position.point.x = reference_position.point.x + relative_position.x;
1498 utm_position.point.y = reference_position.point.y + relative_position.y;
1499 utm_position.point.z = reference_position.point.z + relative_position.z;
1500
1501 return utm_position;
1502}
1503
1513inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1514 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1515}
1516
1523inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1524 return sensor_information.sensor_id.value;
1525}
1526
1533inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1534 return sensor_information.sensor_type.value;
1535}
1536
1537} // namespace etsi_its_cpm_ts_msgs::access

◆ getWGSPosConfidenceEllipse()

template<typename PosConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getWGSPosConfidenceEllipse ( const PosConfidenceEllipse & position_confidence_ellipse)
inline

Get the covariance matrix of the position confidence ellipse.

Parameters
position_confidence_ellipseThe position confidence ellipse to get the covariance matrix from
object_headingThe object heading in radians
Returns
std::array<double, 4> The covariance matrix of the position confidence ellipse in WGS coordinates (x = North, y = East)

Definition at line 299 of file cpm_ts_getters.h.

299 { return angle.confidence.value; }
300

◆ getWGSPositionConfidenceEllipse()

template<typename PositionConfidenceEllipse>
std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getWGSPositionConfidenceEllipse ( const PositionConfidenceEllipse & position_confidence_ellipse)
inline

Get the covariance matrix of the position confidence ellipse.

Parameters
position_confidence_ellipseThe position confidence ellipse to get the covariance matrix from
object_headingThe object heading in radians
Returns
std::array<double, 4> The covariance matrix of the position confidence ellipse in WGS coordinates (x = North, y = East)

Definition at line 383 of file cpm_ts_getters.h.

◆ getWGSRefPosConfidence()

const std::array< double, 4 > etsi_its_cpm_ts_msgs::access::getWGSRefPosConfidence ( const CollectivePerceptionMessage & cpm)
inline

Get the confidence ellipse of the reference position as Covariance matrix.

The covariance matrix will have the entries cov_xx, cov_xy, cov_yx, cov_yy where x is WGS84 North and y is East

Parameters
cpmThe CPM message to get the reference position from
Returns
const std::array<double, 4> the covariance matrix, as specified above

Definition at line 1090 of file cpm_ts_getters.h.

1137inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1138
1145inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1146 return cpm.payload.management_container.reference_time;
1147}
1148
1155inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1156
1163inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1164 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1165}
1166
1173inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1174 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1175}
1176
1183inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1184 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1185}
1186
1198inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1199 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1200}
1201
1211inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1212 int zone;
1213 bool northp;
1214 return getUTMPosition(cpm, zone, northp);
1215}
1216
1226inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1227 std::vector<uint8_t> container_ids;
1228 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1229 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1230 }
1231 return container_ids;
1232}
1233
1242inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1243 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1244 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1245 return cpm.payload.cpm_containers.value.array[i];
1246 }
1247 }
1248 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1249}
1250
1259inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1260 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1261}
1262
1270inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1271 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1272 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1273 }
1274 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1275 return number;
1276}
1277
1284inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1285 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1286}
1287
1291
1300inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1301 if (i >= getNumberOfPerceivedObjects(container)) {
1302 throw std::invalid_argument("Index out of range");
1303 }
1304 return container.container_data_perceived_object_container.perceived_objects.array[i];
1305}
1306
1315inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1316 if (!object.object_id_is_present) {
1317 throw std::invalid_argument("No object_id present in PerceivedObject");
1318 }
1319 return object.object_id.value;
1320}
1321
1328inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1329 return coordinate.value.value;
1330}
1331
1338inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1339 return coordinate.confidence.value;
1340}
1341
1348inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1349 gm::Point point;
1350 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1351 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1352 if (object.position.z_coordinate_is_present) {
1353 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1354 }
1355 return point;
1356}
1357
1365inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1366 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1367 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1368 double z_confidence = object.position.z_coordinate_is_present
1369 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1370 : std::numeric_limits<double>::infinity();
1371 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1372}
1373
1380inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1381
1388inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1389
1400inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1401 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1402 tf2::Quaternion q;
1403 double roll{0}, pitch{0}, yaw{0};
1404
1405 if (object.angles.x_angle_is_present) {
1406 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1407 M_PI;
1408 }
1409 if (object.angles.y_angle_is_present) {
1410 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1411 M_PI;
1412 }
1413 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1414 M_PI;
1415 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1416 q.setRPY(roll, pitch, yaw);
1417
1418 return tf2::toMsg(q);
1419}
1420
1427inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1428 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1429 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1430 double roll, pitch, yaw;
1431 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1432 return yaw;
1433}
1434
1442inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1443 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1444 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1445 yaw_confidence *= M_PI / 180.0; // convert to radians
1446 return yaw_confidence;
1447}
1448
1455inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1456 gm::Pose pose;
1457 pose.position = getPositionOfPerceivedObject(object);
1458 pose.orientation = getOrientationOfPerceivedObject(object);
1459 return pose;
1460}
1461
1469inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1470 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1471 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1472}
1473
1483inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1484 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1485 auto val = object.z_angular_velocity.confidence.value;
1486 static const std::map<uint8_t, double> confidence_map = {
1487 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1488 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1489 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1490 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1491 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1492 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1493 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1494 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1495 };
1496 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1497}
1498
1505inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1506
1513inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1514 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1515}
1516
1524inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1525 if (!object.velocity_is_present) {
1526 throw std::invalid_argument("No velocity present in PerceivedObject");
1527 }
1528 gm::Vector3 velocity;
1529 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1530 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1531 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1532 velocity.x = speed * cos(angle);
1533 velocity.y = speed * sin(angle);
1534 if (object.velocity.polar_velocity.z_velocity_is_present) {
1535 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1536 }
1537 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1538 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1539 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1540 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1541 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1542 }
1543 } else {
1544 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1545 }
1546 return velocity;
1547}
1548
1557inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1558 if (!object.velocity_is_present) {
1559 throw std::invalid_argument("No velocity present in PerceivedObject");
1560 }
1561 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1562 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1563 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1564 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1565 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1566 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1567 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1568 + lateral_confidence * sin(angle) * sin(angle);
1569 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1570 + lateral_confidence * cos(angle) * cos(angle);
1571 // neglect xy covariance, as it is not present in the output of this function
1572 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1573 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1574 : std::numeric_limits<double>::infinity();
1575 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1576 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1577 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1578 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1579 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1580 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1581 : std::numeric_limits<double>::infinity();
1582 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1583 } else {
1584 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1585 }
1586
1587}
1588
1595inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1596 return double(acceleration.value.value) / 10.0;
1597}
1598
1605inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1606 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1607}
1608
1616inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1617 if (!object.acceleration_is_present) {
1618 throw std::invalid_argument("No acceleration present in PerceivedObject");
1619 }
1620 gm::Vector3 acceleration;
1621
1622 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1623 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1624 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1625 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1626 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1627 }
1628 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1629 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1630 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1631 acceleration.x = magnitude * cos(angle);
1632 acceleration.y = magnitude * sin(angle);
1633 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1634 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1635 }
1636 } else {
1637 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1638 }
1639
1640 return acceleration;
1641}
1642
1651inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1652 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1653
1654 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1655 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1656 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1657 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1658 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1659 : std::numeric_limits<double>::infinity();
1660 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1661 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1662 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1663 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1664 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1665 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1666 double lateral_confidence = magnitude * angle_confidence; // best approximation
1667 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1668 + lateral_confidence * sin(angle) * sin(angle);
1669 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1670 + lateral_confidence * cos(angle) * cos(angle);
1671 // neglect xy covariance, as it is not present in the output of this function
1672 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1673 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1674 : std::numeric_limits<double>::infinity();
1675 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1676 } else {
1677 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1678 }
1679}
1680
1692inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1693 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1694 return object.object_dimension_x.value.value;
1695}
1696
1704inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1705 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1706 return object.object_dimension_x.confidence.value;
1707}
1708
1720inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1721 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1722 return object.object_dimension_y.value.value;
1723}
1724
1732inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1733 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1734 return object.object_dimension_y.confidence.value;
1735}
1736
1748inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1749 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1750 return object.object_dimension_z.value.value;
1751}
1752
1760inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1761 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1762 return object.object_dimension_z.confidence.value;
1763}
1764
1774inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1775 gm::Vector3 dimensions;
1776 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1777 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1778 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1779 return dimensions;
1780}
1781
1788inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1789 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1790 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1791 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1792 return {conf_x, conf_y, conf_z};
1793}
1794
1805inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1806 const PerceivedObject &object) {
1807 gm::PointStamped utm_position;
1808 gm::PointStamped reference_position = getUTMPosition(cpm);
1809 gm::Point relative_position = getPositionOfPerceivedObject(object);
1810
1811 utm_position.header.frame_id = reference_position.header.frame_id;
1812 utm_position.point.x = reference_position.point.x + relative_position.x;
1813 utm_position.point.y = reference_position.point.y + relative_position.y;
1814 utm_position.point.z = reference_position.point.z + relative_position.z;
1815
1816 return utm_position;
1817}
1818
1828inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1829 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1830}
1831
1838inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1839 return sensor_information.sensor_id.value;
1840}
1841
1848inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1849 return sensor_information.sensor_type.value;
1850}
1851
1852} // namespace etsi_its_cpm_ts_msgs::access

◆ getXDimensionConfidenceOfPerceivedObject()

uint8_t etsi_its_cpm_ts_msgs::access::getXDimensionConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Gets the confidence of the x-dimension of a perceived object.

Parameters
objectThe PerceivedObject from which to retrieve the x-dimension confidence.
Returns
uint8_t The confidence of the x-dimension of the perceived object in decimeters.
Exceptions
std::invalid_argumentif the x-dimension is not present in the PerceivedObject.

Definition at line 966 of file cpm_ts_getters.h.

1001 {
1002
1003#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1004
1013inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1014
1021inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1022 return cpm.payload.management_container.reference_time;
1023}
1024
1031inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1032
1039inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1040 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1041}
1042
1049inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1050 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1051}
1052
1059inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1060 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1061}
1062
1074inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1075 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1076}
1077
1087inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1088 int zone;
1089 bool northp;
1090 return getUTMPosition(cpm, zone, northp);
1091}
1092
1102inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1103 std::vector<uint8_t> container_ids;
1104 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1105 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1106 }
1107 return container_ids;
1108}
1109
1118inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1119 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1120 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1121 return cpm.payload.cpm_containers.value.array[i];
1122 }
1123 }
1124 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1125}
1126
1135inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1136 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1137}
1138
1146inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1147 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1148 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1149 }
1150 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1151 return number;
1152}
1153
1160inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1161 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1162}
1163
1167
1176inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1177 if (i >= getNumberOfPerceivedObjects(container)) {
1178 throw std::invalid_argument("Index out of range");
1179 }
1180 return container.container_data_perceived_object_container.perceived_objects.array[i];
1181}
1182
1191inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1192 if (!object.object_id_is_present) {
1193 throw std::invalid_argument("No object_id present in PerceivedObject");
1194 }
1195 return object.object_id.value;
1196}
1197
1204inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1205 return coordinate.value.value;
1206}
1207
1214inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1215 return coordinate.confidence.value;
1216}
1217
1224inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1225 gm::Point point;
1226 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1227 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1228 if (object.position.z_coordinate_is_present) {
1229 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1230 }
1231 return point;
1232}
1233
1241inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1242 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1243 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1244 double z_confidence = object.position.z_coordinate_is_present
1245 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1246 : std::numeric_limits<double>::infinity();
1247 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1248}
1249
1256inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1257
1264inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1265
1276inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1277 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1278 tf2::Quaternion q;
1279 double roll{0}, pitch{0}, yaw{0};
1280
1281 if (object.angles.x_angle_is_present) {
1282 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1283 M_PI;
1284 }
1285 if (object.angles.y_angle_is_present) {
1286 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1287 M_PI;
1288 }
1289 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1290 M_PI;
1291 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1292 q.setRPY(roll, pitch, yaw);
1293
1294 return tf2::toMsg(q);
1295}
1296
1303inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1304 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1305 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1306 double roll, pitch, yaw;
1307 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1308 return yaw;
1309}
1310
1318inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1319 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1320 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1321 yaw_confidence *= M_PI / 180.0; // convert to radians
1322 return yaw_confidence;
1323}
1324
1331inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1332 gm::Pose pose;
1333 pose.position = getPositionOfPerceivedObject(object);
1334 pose.orientation = getOrientationOfPerceivedObject(object);
1335 return pose;
1336}
1337
1345inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1346 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1347 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1348}
1349
1359inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1360 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1361 auto val = object.z_angular_velocity.confidence.value;
1362 static const std::map<uint8_t, double> confidence_map = {
1363 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1364 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1365 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1366 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1367 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1368 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1369 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1370 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1371 };
1372 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1373}
1374
1381inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1382
1389inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1390 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1391}
1392
1400inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1401 if (!object.velocity_is_present) {
1402 throw std::invalid_argument("No velocity present in PerceivedObject");
1403 }
1404 gm::Vector3 velocity;
1405 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1406 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1407 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1408 velocity.x = speed * cos(angle);
1409 velocity.y = speed * sin(angle);
1410 if (object.velocity.polar_velocity.z_velocity_is_present) {
1411 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1412 }
1413 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1414 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1415 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1416 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1417 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1418 }
1419 } else {
1420 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1421 }
1422 return velocity;
1423}
1424
1433inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1434 if (!object.velocity_is_present) {
1435 throw std::invalid_argument("No velocity present in PerceivedObject");
1436 }
1437 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1438 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1439 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1440 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1441 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1442 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1443 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1444 + lateral_confidence * sin(angle) * sin(angle);
1445 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1446 + lateral_confidence * cos(angle) * cos(angle);
1447 // neglect xy covariance, as it is not present in the output of this function
1448 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1449 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1450 : std::numeric_limits<double>::infinity();
1451 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1452 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1453 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1454 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1455 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1456 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1457 : std::numeric_limits<double>::infinity();
1458 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1459 } else {
1460 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1461 }
1462
1463}
1464
1471inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1472 return double(acceleration.value.value) / 10.0;
1473}
1474
1481inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1482 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1483}
1484
1492inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1493 if (!object.acceleration_is_present) {
1494 throw std::invalid_argument("No acceleration present in PerceivedObject");
1495 }
1496 gm::Vector3 acceleration;
1497
1498 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1499 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1500 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1501 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1502 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1503 }
1504 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1505 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1506 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1507 acceleration.x = magnitude * cos(angle);
1508 acceleration.y = magnitude * sin(angle);
1509 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1510 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1511 }
1512 } else {
1513 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1514 }
1515
1516 return acceleration;
1517}
1518
1527inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1528 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1529
1530 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1531 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1532 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1533 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1534 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1535 : std::numeric_limits<double>::infinity();
1536 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1537 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1538 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1539 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1540 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1541 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1542 double lateral_confidence = magnitude * angle_confidence; // best approximation
1543 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1544 + lateral_confidence * sin(angle) * sin(angle);
1545 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1546 + lateral_confidence * cos(angle) * cos(angle);
1547 // neglect xy covariance, as it is not present in the output of this function
1548 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1549 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1550 : std::numeric_limits<double>::infinity();
1551 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1552 } else {
1553 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1554 }
1555}
1556
1568inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1569 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1570 return object.object_dimension_x.value.value;
1571}
1572
1580inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1581 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1582 return object.object_dimension_x.confidence.value;
1583}
1584
1596inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1597 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1598 return object.object_dimension_y.value.value;
1599}
1600
1608inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1609 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1610 return object.object_dimension_y.confidence.value;
1611}
1612
1624inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1625 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1626 return object.object_dimension_z.value.value;
1627}
1628
1636inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1637 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1638 return object.object_dimension_z.confidence.value;
1639}
1640
1650inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1651 gm::Vector3 dimensions;
1652 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1653 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1654 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1655 return dimensions;
1656}
1657
1664inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1665 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1666 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1667 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1668 return {conf_x, conf_y, conf_z};
1669}
1670
1681inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1682 const PerceivedObject &object) {
1683 gm::PointStamped utm_position;
1684 gm::PointStamped reference_position = getUTMPosition(cpm);
1685 gm::Point relative_position = getPositionOfPerceivedObject(object);
1686
1687 utm_position.header.frame_id = reference_position.header.frame_id;
1688 utm_position.point.x = reference_position.point.x + relative_position.x;
1689 utm_position.point.y = reference_position.point.y + relative_position.y;
1690 utm_position.point.z = reference_position.point.z + relative_position.z;
1691
1692 return utm_position;
1693}
1694
1704inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1705 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1706}
1707
1714inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1715 return sensor_information.sensor_id.value;
1716}
1717
1724inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1725 return sensor_information.sensor_type.value;
1726}
1727
1728} // namespace etsi_its_cpm_ts_msgs::access

◆ getXDimensionOfPerceivedObject()

uint16_t etsi_its_cpm_ts_msgs::access::getXDimensionOfPerceivedObject ( const PerceivedObject & object)
inline

Gets the x-dimension of a perceived object.

This function extracts the x-dimension from a given PerceivedObject. If the x-dimension is not present in the object, it throws an std::invalid_argument exception.

Parameters
objectThe PerceivedObject from which to retrieve the x-dimension.
Returns
The x-dimension of the perceived object in decimeters.
Exceptions
std::invalid_argumentif the x-dimension is not present in the PerceivedObject.

Definition at line 954 of file cpm_ts_getters.h.

989 {
990
991#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
992
1001inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1002
1009inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1010 return cpm.payload.management_container.reference_time;
1011}
1012
1019inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1020
1027inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1028 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1029}
1030
1037inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1038 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1039}
1040
1047inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1048 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1049}
1050
1062inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1063 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1064}
1065
1075inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1076 int zone;
1077 bool northp;
1078 return getUTMPosition(cpm, zone, northp);
1079}
1080
1090inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1091 std::vector<uint8_t> container_ids;
1092 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1093 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1094 }
1095 return container_ids;
1096}
1097
1106inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1107 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1108 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1109 return cpm.payload.cpm_containers.value.array[i];
1110 }
1111 }
1112 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1113}
1114
1123inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1124 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1125}
1126
1134inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1135 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1136 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1137 }
1138 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1139 return number;
1140}
1141
1148inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1149 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1150}
1151
1155
1164inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1165 if (i >= getNumberOfPerceivedObjects(container)) {
1166 throw std::invalid_argument("Index out of range");
1167 }
1168 return container.container_data_perceived_object_container.perceived_objects.array[i];
1169}
1170
1179inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1180 if (!object.object_id_is_present) {
1181 throw std::invalid_argument("No object_id present in PerceivedObject");
1182 }
1183 return object.object_id.value;
1184}
1185
1192inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1193 return coordinate.value.value;
1194}
1195
1202inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1203 return coordinate.confidence.value;
1204}
1205
1212inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1213 gm::Point point;
1214 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1215 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1216 if (object.position.z_coordinate_is_present) {
1217 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1218 }
1219 return point;
1220}
1221
1229inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1230 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1231 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1232 double z_confidence = object.position.z_coordinate_is_present
1233 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1234 : std::numeric_limits<double>::infinity();
1235 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1236}
1237
1244inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1245
1252inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1253
1264inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1265 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1266 tf2::Quaternion q;
1267 double roll{0}, pitch{0}, yaw{0};
1268
1269 if (object.angles.x_angle_is_present) {
1270 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1271 M_PI;
1272 }
1273 if (object.angles.y_angle_is_present) {
1274 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1275 M_PI;
1276 }
1277 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1278 M_PI;
1279 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1280 q.setRPY(roll, pitch, yaw);
1281
1282 return tf2::toMsg(q);
1283}
1284
1291inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1292 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1293 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1294 double roll, pitch, yaw;
1295 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1296 return yaw;
1297}
1298
1306inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1307 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1308 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1309 yaw_confidence *= M_PI / 180.0; // convert to radians
1310 return yaw_confidence;
1311}
1312
1319inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1320 gm::Pose pose;
1321 pose.position = getPositionOfPerceivedObject(object);
1322 pose.orientation = getOrientationOfPerceivedObject(object);
1323 return pose;
1324}
1325
1333inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1334 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1335 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1336}
1337
1347inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1348 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1349 auto val = object.z_angular_velocity.confidence.value;
1350 static const std::map<uint8_t, double> confidence_map = {
1351 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1352 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1353 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1354 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1355 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1356 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1357 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1358 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1359 };
1360 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1361}
1362
1369inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1370
1377inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1378 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1379}
1380
1388inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1389 if (!object.velocity_is_present) {
1390 throw std::invalid_argument("No velocity present in PerceivedObject");
1391 }
1392 gm::Vector3 velocity;
1393 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1394 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1395 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1396 velocity.x = speed * cos(angle);
1397 velocity.y = speed * sin(angle);
1398 if (object.velocity.polar_velocity.z_velocity_is_present) {
1399 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1400 }
1401 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1402 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1403 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1404 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1405 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1406 }
1407 } else {
1408 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1409 }
1410 return velocity;
1411}
1412
1421inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1422 if (!object.velocity_is_present) {
1423 throw std::invalid_argument("No velocity present in PerceivedObject");
1424 }
1425 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1426 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1427 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1428 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1429 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1430 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1431 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1432 + lateral_confidence * sin(angle) * sin(angle);
1433 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1434 + lateral_confidence * cos(angle) * cos(angle);
1435 // neglect xy covariance, as it is not present in the output of this function
1436 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1437 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1438 : std::numeric_limits<double>::infinity();
1439 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1440 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1441 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1442 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1443 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1444 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1445 : std::numeric_limits<double>::infinity();
1446 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1447 } else {
1448 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1449 }
1450
1451}
1452
1459inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1460 return double(acceleration.value.value) / 10.0;
1461}
1462
1469inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1470 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1471}
1472
1480inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1481 if (!object.acceleration_is_present) {
1482 throw std::invalid_argument("No acceleration present in PerceivedObject");
1483 }
1484 gm::Vector3 acceleration;
1485
1486 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1487 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1488 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1489 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1490 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1491 }
1492 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1493 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1494 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1495 acceleration.x = magnitude * cos(angle);
1496 acceleration.y = magnitude * sin(angle);
1497 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1498 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1499 }
1500 } else {
1501 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1502 }
1503
1504 return acceleration;
1505}
1506
1515inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1516 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1517
1518 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1519 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1520 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1521 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1522 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1523 : std::numeric_limits<double>::infinity();
1524 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1525 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1526 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1527 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1528 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1529 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1530 double lateral_confidence = magnitude * angle_confidence; // best approximation
1531 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1532 + lateral_confidence * sin(angle) * sin(angle);
1533 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1534 + lateral_confidence * cos(angle) * cos(angle);
1535 // neglect xy covariance, as it is not present in the output of this function
1536 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1537 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1538 : std::numeric_limits<double>::infinity();
1539 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1540 } else {
1541 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1542 }
1543}
1544
1556inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1557 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1558 return object.object_dimension_x.value.value;
1559}
1560
1568inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1569 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1570 return object.object_dimension_x.confidence.value;
1571}
1572
1584inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1585 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1586 return object.object_dimension_y.value.value;
1587}
1588
1596inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1597 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1598 return object.object_dimension_y.confidence.value;
1599}
1600
1612inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1613 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1614 return object.object_dimension_z.value.value;
1615}
1616
1624inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1625 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1626 return object.object_dimension_z.confidence.value;
1627}
1628
1638inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1639 gm::Vector3 dimensions;
1640 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1641 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1642 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1643 return dimensions;
1644}
1645
1652inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1653 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1654 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1655 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1656 return {conf_x, conf_y, conf_z};
1657}
1658
1669inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1670 const PerceivedObject &object) {
1671 gm::PointStamped utm_position;
1672 gm::PointStamped reference_position = getUTMPosition(cpm);
1673 gm::Point relative_position = getPositionOfPerceivedObject(object);
1674
1675 utm_position.header.frame_id = reference_position.header.frame_id;
1676 utm_position.point.x = reference_position.point.x + relative_position.x;
1677 utm_position.point.y = reference_position.point.y + relative_position.y;
1678 utm_position.point.z = reference_position.point.z + relative_position.z;
1679
1680 return utm_position;
1681}
1682
1692inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1693 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1694}
1695
1702inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1703 return sensor_information.sensor_id.value;
1704}
1705
1712inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1713 return sensor_information.sensor_type.value;
1714}
1715
1716} // namespace etsi_its_cpm_ts_msgs::access

◆ getYawConfidenceOfPerceivedObject()

double etsi_its_cpm_ts_msgs::access::getYawConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Yaw Confidence Of Perceived Object object.

Parameters
objectPerceivedObject to get the yaw confidence from
Returns
double The standard deviation of the yaw angle in radians
Exceptions
std::invalid_argumentIf the angles are not present in the object.

Definition at line 704 of file cpm_ts_getters.h.

◆ getYawOfPerceivedObject()

double etsi_its_cpm_ts_msgs::access::getYawOfPerceivedObject ( const PerceivedObject & object)
inline

Get the yaw of the PerceivedObject.

Parameters
objectPerceivedObject to get the yaw from
Returns
double yaw of the PerceivedObject in radians from -pi to pi

Definition at line 689 of file cpm_ts_getters.h.

◆ getYawRateCDD()

template<typename YawRate>
double etsi_its_cpm_ts_msgs::access::getYawRateCDD ( const YawRate & yaw_rate)
inline

Get the Yaw Rate value.

Parameters
yaw_rateThe YawRate object to get the yaw rate from
Returns
double The yaw rate in degrees per second

Definition at line 179 of file cpm_ts_getters.h.

181 {

◆ getYawRateConfidenceCDD()

template<typename YawRate, typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
double etsi_its_cpm_ts_msgs::access::getYawRateConfidenceCDD ( const YawRate & yaw_rate)
inline

Get the Yaw Rate Confidence.

Parameters
yaw_rateThe YawRate object to get the yaw rate confidence from
Returns
double The yaw rate standard deviation in degrees per second

Definition at line 190 of file cpm_ts_getters.h.

195 {
196 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
197}
198
202

◆ getYawRateConfidenceOfPerceivedObject()

double etsi_its_cpm_ts_msgs::access::getYawRateConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Get the Yaw Rate Confidence Of Perceived Object.

Parameters
objectThe PerceivedObject to get the yaw rate confidence from
Returns
double The standard deviation of the yaw rate in rad/s As defined in the TS, this is rounded up to the next possible value
Exceptions
std::invalid_argumentIf the yaw rate is not present in the object.

Definition at line 745 of file cpm_ts_getters.h.

759inline uint8_t getSensorType(const SensorInformation &sensor_information) {

◆ getYawRateOfPerceivedObject()

double etsi_its_cpm_ts_msgs::access::getYawRateOfPerceivedObject ( const PerceivedObject & object)
inline

Get the yaw rate of the PerceivedObject.

Parameters
objectPerceivedObject to get the yaw rate from
Returns
double yaw rate of the PerceivedObject in rad/s
Exceptions
std::invalid_argumentIf the yaw rate is not present in the object.

Definition at line 731 of file cpm_ts_getters.h.

◆ getYDimensionConfidenceOfPerceivedObject()

uint8_t etsi_its_cpm_ts_msgs::access::getYDimensionConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Gets the confidence of the y-dimension of a perceived object.

Parameters
objectThe PerceivedObject from which to retrieve the y-dimension confidence.
Returns
uint8_t The confidence of the y-dimension of the perceived object in decimeters.
Exceptions
std::invalid_argumentif the y-dimension is not present in the PerceivedObject.

Definition at line 994 of file cpm_ts_getters.h.

1029 {
1030
1031#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1032
1041inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1042
1049inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1050 return cpm.payload.management_container.reference_time;
1051}
1052
1059inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1060
1067inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1068 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1069}
1070
1077inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1078 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1079}
1080
1087inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1088 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1089}
1090
1102inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1103 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1104}
1105
1115inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1116 int zone;
1117 bool northp;
1118 return getUTMPosition(cpm, zone, northp);
1119}
1120
1130inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1131 std::vector<uint8_t> container_ids;
1132 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1133 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1134 }
1135 return container_ids;
1136}
1137
1146inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1147 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1148 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1149 return cpm.payload.cpm_containers.value.array[i];
1150 }
1151 }
1152 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1153}
1154
1163inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1164 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1165}
1166
1174inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1175 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1176 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1177 }
1178 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1179 return number;
1180}
1181
1188inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1189 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1190}
1191
1195
1204inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1205 if (i >= getNumberOfPerceivedObjects(container)) {
1206 throw std::invalid_argument("Index out of range");
1207 }
1208 return container.container_data_perceived_object_container.perceived_objects.array[i];
1209}
1210
1219inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1220 if (!object.object_id_is_present) {
1221 throw std::invalid_argument("No object_id present in PerceivedObject");
1222 }
1223 return object.object_id.value;
1224}
1225
1232inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1233 return coordinate.value.value;
1234}
1235
1242inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1243 return coordinate.confidence.value;
1244}
1245
1252inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1253 gm::Point point;
1254 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1255 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1256 if (object.position.z_coordinate_is_present) {
1257 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1258 }
1259 return point;
1260}
1261
1269inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1270 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1271 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1272 double z_confidence = object.position.z_coordinate_is_present
1273 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1274 : std::numeric_limits<double>::infinity();
1275 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1276}
1277
1284inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1285
1292inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1293
1304inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1305 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1306 tf2::Quaternion q;
1307 double roll{0}, pitch{0}, yaw{0};
1308
1309 if (object.angles.x_angle_is_present) {
1310 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1311 M_PI;
1312 }
1313 if (object.angles.y_angle_is_present) {
1314 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1315 M_PI;
1316 }
1317 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1318 M_PI;
1319 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1320 q.setRPY(roll, pitch, yaw);
1321
1322 return tf2::toMsg(q);
1323}
1324
1331inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1332 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1333 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1334 double roll, pitch, yaw;
1335 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1336 return yaw;
1337}
1338
1346inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1347 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1348 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1349 yaw_confidence *= M_PI / 180.0; // convert to radians
1350 return yaw_confidence;
1351}
1352
1359inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1360 gm::Pose pose;
1361 pose.position = getPositionOfPerceivedObject(object);
1362 pose.orientation = getOrientationOfPerceivedObject(object);
1363 return pose;
1364}
1365
1373inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1374 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1375 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1376}
1377
1387inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1388 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1389 auto val = object.z_angular_velocity.confidence.value;
1390 static const std::map<uint8_t, double> confidence_map = {
1391 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1392 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1393 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1394 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1395 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1396 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1397 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1398 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1399 };
1400 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1401}
1402
1409inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1410
1417inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1418 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1419}
1420
1428inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1429 if (!object.velocity_is_present) {
1430 throw std::invalid_argument("No velocity present in PerceivedObject");
1431 }
1432 gm::Vector3 velocity;
1433 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1434 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1435 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1436 velocity.x = speed * cos(angle);
1437 velocity.y = speed * sin(angle);
1438 if (object.velocity.polar_velocity.z_velocity_is_present) {
1439 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1440 }
1441 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1442 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1443 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1444 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1445 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1446 }
1447 } else {
1448 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1449 }
1450 return velocity;
1451}
1452
1461inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1462 if (!object.velocity_is_present) {
1463 throw std::invalid_argument("No velocity present in PerceivedObject");
1464 }
1465 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1466 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1467 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1468 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1469 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1470 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1471 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1472 + lateral_confidence * sin(angle) * sin(angle);
1473 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1474 + lateral_confidence * cos(angle) * cos(angle);
1475 // neglect xy covariance, as it is not present in the output of this function
1476 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1477 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1478 : std::numeric_limits<double>::infinity();
1479 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1480 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1481 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1482 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1483 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1484 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1485 : std::numeric_limits<double>::infinity();
1486 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1487 } else {
1488 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1489 }
1490
1491}
1492
1499inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1500 return double(acceleration.value.value) / 10.0;
1501}
1502
1509inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1510 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1511}
1512
1520inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1521 if (!object.acceleration_is_present) {
1522 throw std::invalid_argument("No acceleration present in PerceivedObject");
1523 }
1524 gm::Vector3 acceleration;
1525
1526 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1527 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1528 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1529 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1530 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1531 }
1532 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1533 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1534 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1535 acceleration.x = magnitude * cos(angle);
1536 acceleration.y = magnitude * sin(angle);
1537 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1538 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1539 }
1540 } else {
1541 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1542 }
1543
1544 return acceleration;
1545}
1546
1555inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1556 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1557
1558 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1559 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1560 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1561 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1562 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1563 : std::numeric_limits<double>::infinity();
1564 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1565 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1566 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1567 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1568 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1569 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1570 double lateral_confidence = magnitude * angle_confidence; // best approximation
1571 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1572 + lateral_confidence * sin(angle) * sin(angle);
1573 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1574 + lateral_confidence * cos(angle) * cos(angle);
1575 // neglect xy covariance, as it is not present in the output of this function
1576 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1577 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1578 : std::numeric_limits<double>::infinity();
1579 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1580 } else {
1581 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1582 }
1583}
1584
1596inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1597 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1598 return object.object_dimension_x.value.value;
1599}
1600
1608inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1609 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1610 return object.object_dimension_x.confidence.value;
1611}
1612
1624inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1625 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1626 return object.object_dimension_y.value.value;
1627}
1628
1636inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1637 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1638 return object.object_dimension_y.confidence.value;
1639}
1640
1652inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1653 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1654 return object.object_dimension_z.value.value;
1655}
1656
1664inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1665 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1666 return object.object_dimension_z.confidence.value;
1667}
1668
1678inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1679 gm::Vector3 dimensions;
1680 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1681 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1682 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1683 return dimensions;
1684}
1685
1692inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1693 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1694 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1695 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1696 return {conf_x, conf_y, conf_z};
1697}
1698
1709inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1710 const PerceivedObject &object) {
1711 gm::PointStamped utm_position;
1712 gm::PointStamped reference_position = getUTMPosition(cpm);
1713 gm::Point relative_position = getPositionOfPerceivedObject(object);
1714
1715 utm_position.header.frame_id = reference_position.header.frame_id;
1716 utm_position.point.x = reference_position.point.x + relative_position.x;
1717 utm_position.point.y = reference_position.point.y + relative_position.y;
1718 utm_position.point.z = reference_position.point.z + relative_position.z;
1719
1720 return utm_position;
1721}
1722
1732inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1733 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1734}
1735
1742inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1743 return sensor_information.sensor_id.value;
1744}
1745
1752inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1753 return sensor_information.sensor_type.value;
1754}
1755
1756} // namespace etsi_its_cpm_ts_msgs::access

◆ getYDimensionOfPerceivedObject()

uint16_t etsi_its_cpm_ts_msgs::access::getYDimensionOfPerceivedObject ( const PerceivedObject & object)
inline

Retrieves the y-dimension of a perceived object.

This function extracts the y-dimension from a given PerceivedObject. If the y-dimension is not present in the object, it throws an std::invalid_argument exception.

Parameters
objectThe PerceivedObject from which to retrieve the y-dimension.
Returns
uint16_t y-dimension of the perceived object in in decimeters.
Exceptions
std::invalid_argumentif the y-dimension is not present in the PerceivedObject.

Definition at line 982 of file cpm_ts_getters.h.

1017 {
1018
1019#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1020
1029inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1030
1037inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1038 return cpm.payload.management_container.reference_time;
1039}
1040
1047inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1048
1055inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1056 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1057}
1058
1065inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1066 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1067}
1068
1075inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1076 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1077}
1078
1090inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1091 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1092}
1093
1103inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1104 int zone;
1105 bool northp;
1106 return getUTMPosition(cpm, zone, northp);
1107}
1108
1118inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1119 std::vector<uint8_t> container_ids;
1120 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1121 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1122 }
1123 return container_ids;
1124}
1125
1134inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1135 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1136 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1137 return cpm.payload.cpm_containers.value.array[i];
1138 }
1139 }
1140 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1141}
1142
1151inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1152 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1153}
1154
1162inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1163 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1164 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1165 }
1166 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1167 return number;
1168}
1169
1176inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1177 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1178}
1179
1183
1192inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1193 if (i >= getNumberOfPerceivedObjects(container)) {
1194 throw std::invalid_argument("Index out of range");
1195 }
1196 return container.container_data_perceived_object_container.perceived_objects.array[i];
1197}
1198
1207inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1208 if (!object.object_id_is_present) {
1209 throw std::invalid_argument("No object_id present in PerceivedObject");
1210 }
1211 return object.object_id.value;
1212}
1213
1220inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1221 return coordinate.value.value;
1222}
1223
1230inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1231 return coordinate.confidence.value;
1232}
1233
1240inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1241 gm::Point point;
1242 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1243 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1244 if (object.position.z_coordinate_is_present) {
1245 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1246 }
1247 return point;
1248}
1249
1257inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1258 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1259 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1260 double z_confidence = object.position.z_coordinate_is_present
1261 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1262 : std::numeric_limits<double>::infinity();
1263 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1264}
1265
1272inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1273
1280inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1281
1292inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1293 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1294 tf2::Quaternion q;
1295 double roll{0}, pitch{0}, yaw{0};
1296
1297 if (object.angles.x_angle_is_present) {
1298 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1299 M_PI;
1300 }
1301 if (object.angles.y_angle_is_present) {
1302 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1303 M_PI;
1304 }
1305 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1306 M_PI;
1307 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1308 q.setRPY(roll, pitch, yaw);
1309
1310 return tf2::toMsg(q);
1311}
1312
1319inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1320 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1321 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1322 double roll, pitch, yaw;
1323 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1324 return yaw;
1325}
1326
1334inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1335 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1336 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1337 yaw_confidence *= M_PI / 180.0; // convert to radians
1338 return yaw_confidence;
1339}
1340
1347inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1348 gm::Pose pose;
1349 pose.position = getPositionOfPerceivedObject(object);
1350 pose.orientation = getOrientationOfPerceivedObject(object);
1351 return pose;
1352}
1353
1361inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1362 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1363 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1364}
1365
1375inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1376 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1377 auto val = object.z_angular_velocity.confidence.value;
1378 static const std::map<uint8_t, double> confidence_map = {
1379 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1380 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1381 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1382 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1383 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1384 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1385 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1386 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1387 };
1388 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1389}
1390
1397inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1398
1405inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1406 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1407}
1408
1416inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1417 if (!object.velocity_is_present) {
1418 throw std::invalid_argument("No velocity present in PerceivedObject");
1419 }
1420 gm::Vector3 velocity;
1421 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1422 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1423 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1424 velocity.x = speed * cos(angle);
1425 velocity.y = speed * sin(angle);
1426 if (object.velocity.polar_velocity.z_velocity_is_present) {
1427 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1428 }
1429 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1430 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1431 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1432 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1433 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1434 }
1435 } else {
1436 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1437 }
1438 return velocity;
1439}
1440
1449inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1450 if (!object.velocity_is_present) {
1451 throw std::invalid_argument("No velocity present in PerceivedObject");
1452 }
1453 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1454 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1455 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1456 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1457 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1458 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1459 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1460 + lateral_confidence * sin(angle) * sin(angle);
1461 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1462 + lateral_confidence * cos(angle) * cos(angle);
1463 // neglect xy covariance, as it is not present in the output of this function
1464 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1465 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1466 : std::numeric_limits<double>::infinity();
1467 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1468 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1469 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1470 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1471 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1472 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1473 : std::numeric_limits<double>::infinity();
1474 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1475 } else {
1476 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1477 }
1478
1479}
1480
1487inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1488 return double(acceleration.value.value) / 10.0;
1489}
1490
1497inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1498 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1499}
1500
1508inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1509 if (!object.acceleration_is_present) {
1510 throw std::invalid_argument("No acceleration present in PerceivedObject");
1511 }
1512 gm::Vector3 acceleration;
1513
1514 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1515 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1516 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1517 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1518 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1519 }
1520 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1521 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1522 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1523 acceleration.x = magnitude * cos(angle);
1524 acceleration.y = magnitude * sin(angle);
1525 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1526 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1527 }
1528 } else {
1529 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1530 }
1531
1532 return acceleration;
1533}
1534
1543inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1544 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1545
1546 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1547 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1548 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1549 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1550 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1551 : std::numeric_limits<double>::infinity();
1552 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1553 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1554 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1555 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1556 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1557 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1558 double lateral_confidence = magnitude * angle_confidence; // best approximation
1559 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1560 + lateral_confidence * sin(angle) * sin(angle);
1561 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1562 + lateral_confidence * cos(angle) * cos(angle);
1563 // neglect xy covariance, as it is not present in the output of this function
1564 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1565 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1566 : std::numeric_limits<double>::infinity();
1567 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1568 } else {
1569 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1570 }
1571}
1572
1584inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1585 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1586 return object.object_dimension_x.value.value;
1587}
1588
1596inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1597 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1598 return object.object_dimension_x.confidence.value;
1599}
1600
1612inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1613 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1614 return object.object_dimension_y.value.value;
1615}
1616
1624inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1625 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1626 return object.object_dimension_y.confidence.value;
1627}
1628
1640inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1641 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1642 return object.object_dimension_z.value.value;
1643}
1644
1652inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1653 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1654 return object.object_dimension_z.confidence.value;
1655}
1656
1666inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1667 gm::Vector3 dimensions;
1668 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1669 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1670 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1671 return dimensions;
1672}
1673
1680inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1681 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1682 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1683 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1684 return {conf_x, conf_y, conf_z};
1685}
1686
1697inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1698 const PerceivedObject &object) {
1699 gm::PointStamped utm_position;
1700 gm::PointStamped reference_position = getUTMPosition(cpm);
1701 gm::Point relative_position = getPositionOfPerceivedObject(object);
1702
1703 utm_position.header.frame_id = reference_position.header.frame_id;
1704 utm_position.point.x = reference_position.point.x + relative_position.x;
1705 utm_position.point.y = reference_position.point.y + relative_position.y;
1706 utm_position.point.z = reference_position.point.z + relative_position.z;
1707
1708 return utm_position;
1709}
1710
1720inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1721 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1722}
1723
1730inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1731 return sensor_information.sensor_id.value;
1732}
1733
1740inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1741 return sensor_information.sensor_type.value;
1742}
1743
1744} // namespace etsi_its_cpm_ts_msgs::access

◆ getZDimensionConfidenceOfPerceivedObject()

uint8_t etsi_its_cpm_ts_msgs::access::getZDimensionConfidenceOfPerceivedObject ( const PerceivedObject & object)
inline

Gets the confidence of the z-dimension of a perceived object.

Parameters
objectThe PerceivedObject from which to retrieve the z-dimension confidence.
Returns
uint8_t The confidence of the z-dimension of the perceived object in decimeters.
Exceptions
std::invalid_argumentif the z-dimension is not present in the PerceivedObject.

Definition at line 1022 of file cpm_ts_getters.h.

1057 {
1058
1059#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1060
1069inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1070
1077inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1078 return cpm.payload.management_container.reference_time;
1079}
1080
1087inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1088
1095inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1096 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1097}
1098
1105inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1106 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1107}
1108
1115inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1116 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1117}
1118
1130inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1131 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1132}
1133
1143inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1144 int zone;
1145 bool northp;
1146 return getUTMPosition(cpm, zone, northp);
1147}
1148
1158inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1159 std::vector<uint8_t> container_ids;
1160 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1161 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1162 }
1163 return container_ids;
1164}
1165
1174inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1175 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1176 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1177 return cpm.payload.cpm_containers.value.array[i];
1178 }
1179 }
1180 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1181}
1182
1191inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1192 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1193}
1194
1202inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1203 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1204 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1205 }
1206 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1207 return number;
1208}
1209
1216inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1217 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1218}
1219
1223
1232inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1233 if (i >= getNumberOfPerceivedObjects(container)) {
1234 throw std::invalid_argument("Index out of range");
1235 }
1236 return container.container_data_perceived_object_container.perceived_objects.array[i];
1237}
1238
1247inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1248 if (!object.object_id_is_present) {
1249 throw std::invalid_argument("No object_id present in PerceivedObject");
1250 }
1251 return object.object_id.value;
1252}
1253
1260inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1261 return coordinate.value.value;
1262}
1263
1270inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1271 return coordinate.confidence.value;
1272}
1273
1280inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1281 gm::Point point;
1282 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1283 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1284 if (object.position.z_coordinate_is_present) {
1285 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1286 }
1287 return point;
1288}
1289
1297inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1298 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1299 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1300 double z_confidence = object.position.z_coordinate_is_present
1301 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1302 : std::numeric_limits<double>::infinity();
1303 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1304}
1305
1312inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1313
1320inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1321
1332inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1333 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1334 tf2::Quaternion q;
1335 double roll{0}, pitch{0}, yaw{0};
1336
1337 if (object.angles.x_angle_is_present) {
1338 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1339 M_PI;
1340 }
1341 if (object.angles.y_angle_is_present) {
1342 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1343 M_PI;
1344 }
1345 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1346 M_PI;
1347 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1348 q.setRPY(roll, pitch, yaw);
1349
1350 return tf2::toMsg(q);
1351}
1352
1359inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1360 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1361 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1362 double roll, pitch, yaw;
1363 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1364 return yaw;
1365}
1366
1374inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1375 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1376 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1377 yaw_confidence *= M_PI / 180.0; // convert to radians
1378 return yaw_confidence;
1379}
1380
1387inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1388 gm::Pose pose;
1389 pose.position = getPositionOfPerceivedObject(object);
1390 pose.orientation = getOrientationOfPerceivedObject(object);
1391 return pose;
1392}
1393
1401inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1402 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1403 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1404}
1405
1415inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1416 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1417 auto val = object.z_angular_velocity.confidence.value;
1418 static const std::map<uint8_t, double> confidence_map = {
1419 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1420 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1421 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1422 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1423 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1424 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1425 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1426 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1427 };
1428 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1429}
1430
1437inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1438
1445inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1446 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1447}
1448
1456inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1457 if (!object.velocity_is_present) {
1458 throw std::invalid_argument("No velocity present in PerceivedObject");
1459 }
1460 gm::Vector3 velocity;
1461 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1462 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1463 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1464 velocity.x = speed * cos(angle);
1465 velocity.y = speed * sin(angle);
1466 if (object.velocity.polar_velocity.z_velocity_is_present) {
1467 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1468 }
1469 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1470 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1471 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1472 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1473 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1474 }
1475 } else {
1476 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1477 }
1478 return velocity;
1479}
1480
1489inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1490 if (!object.velocity_is_present) {
1491 throw std::invalid_argument("No velocity present in PerceivedObject");
1492 }
1493 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1494 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1495 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1496 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1497 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1498 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1499 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1500 + lateral_confidence * sin(angle) * sin(angle);
1501 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1502 + lateral_confidence * cos(angle) * cos(angle);
1503 // neglect xy covariance, as it is not present in the output of this function
1504 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1505 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1506 : std::numeric_limits<double>::infinity();
1507 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1508 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1509 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1510 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1511 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1512 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1513 : std::numeric_limits<double>::infinity();
1514 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1515 } else {
1516 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1517 }
1518
1519}
1520
1527inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1528 return double(acceleration.value.value) / 10.0;
1529}
1530
1537inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1538 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1539}
1540
1548inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1549 if (!object.acceleration_is_present) {
1550 throw std::invalid_argument("No acceleration present in PerceivedObject");
1551 }
1552 gm::Vector3 acceleration;
1553
1554 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1555 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1556 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1557 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1558 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1559 }
1560 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1561 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1562 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1563 acceleration.x = magnitude * cos(angle);
1564 acceleration.y = magnitude * sin(angle);
1565 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1566 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1567 }
1568 } else {
1569 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1570 }
1571
1572 return acceleration;
1573}
1574
1583inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1584 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1585
1586 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1587 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1588 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1589 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1590 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1591 : std::numeric_limits<double>::infinity();
1592 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1593 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1594 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1595 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1596 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1597 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1598 double lateral_confidence = magnitude * angle_confidence; // best approximation
1599 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1600 + lateral_confidence * sin(angle) * sin(angle);
1601 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1602 + lateral_confidence * cos(angle) * cos(angle);
1603 // neglect xy covariance, as it is not present in the output of this function
1604 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1605 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1606 : std::numeric_limits<double>::infinity();
1607 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1608 } else {
1609 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1610 }
1611}
1612
1624inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1625 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1626 return object.object_dimension_x.value.value;
1627}
1628
1636inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1637 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1638 return object.object_dimension_x.confidence.value;
1639}
1640
1652inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1653 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1654 return object.object_dimension_y.value.value;
1655}
1656
1664inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1665 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1666 return object.object_dimension_y.confidence.value;
1667}
1668
1680inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1681 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1682 return object.object_dimension_z.value.value;
1683}
1684
1692inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1693 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1694 return object.object_dimension_z.confidence.value;
1695}
1696
1706inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1707 gm::Vector3 dimensions;
1708 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1709 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1710 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1711 return dimensions;
1712}
1713
1720inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1721 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1722 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1723 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1724 return {conf_x, conf_y, conf_z};
1725}
1726
1737inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1738 const PerceivedObject &object) {
1739 gm::PointStamped utm_position;
1740 gm::PointStamped reference_position = getUTMPosition(cpm);
1741 gm::Point relative_position = getPositionOfPerceivedObject(object);
1742
1743 utm_position.header.frame_id = reference_position.header.frame_id;
1744 utm_position.point.x = reference_position.point.x + relative_position.x;
1745 utm_position.point.y = reference_position.point.y + relative_position.y;
1746 utm_position.point.z = reference_position.point.z + relative_position.z;
1747
1748 return utm_position;
1749}
1750
1760inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1761 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1762}
1763
1770inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1771 return sensor_information.sensor_id.value;
1772}
1773
1780inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1781 return sensor_information.sensor_type.value;
1782}
1783
1784} // namespace etsi_its_cpm_ts_msgs::access

◆ getZDimensionOfPerceivedObject()

uint16_t etsi_its_cpm_ts_msgs::access::getZDimensionOfPerceivedObject ( const PerceivedObject & object)
inline

Retrieves the z-dimension of a perceived object.

This function extracts the z-dimension from a given PerceivedObject. If the z-dimension is not present in the object, it throws an std::invalid_argument exception.

Parameters
objectThe PerceivedObject from which to retrieve the z-dimension.
Returns
uint16_t z-dimension of the perceived object in decimeters.
Exceptions
std::invalid_argumentIf the z-dimension is not present in the object.

Definition at line 1010 of file cpm_ts_getters.h.

1045 {
1046
1047#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1048
1057inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1058
1065inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1066 return cpm.payload.management_container.reference_time;
1067}
1068
1075inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1076
1083inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1084 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1085}
1086
1093inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1094 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1095}
1096
1103inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1104 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1105}
1106
1118inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1119 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1120}
1121
1131inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1132 int zone;
1133 bool northp;
1134 return getUTMPosition(cpm, zone, northp);
1135}
1136
1146inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1147 std::vector<uint8_t> container_ids;
1148 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1149 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1150 }
1151 return container_ids;
1152}
1153
1162inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1163 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1164 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1165 return cpm.payload.cpm_containers.value.array[i];
1166 }
1167 }
1168 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1169}
1170
1179inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1180 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1181}
1182
1190inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1191 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1192 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1193 }
1194 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1195 return number;
1196}
1197
1204inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1205 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1206}
1207
1211
1220inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1221 if (i >= getNumberOfPerceivedObjects(container)) {
1222 throw std::invalid_argument("Index out of range");
1223 }
1224 return container.container_data_perceived_object_container.perceived_objects.array[i];
1225}
1226
1235inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
1236 if (!object.object_id_is_present) {
1237 throw std::invalid_argument("No object_id present in PerceivedObject");
1238 }
1239 return object.object_id.value;
1240}
1241
1248inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1249 return coordinate.value.value;
1250}
1251
1258inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1259 return coordinate.confidence.value;
1260}
1261
1268inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1269 gm::Point point;
1270 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1271 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1272 if (object.position.z_coordinate_is_present) {
1273 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1274 }
1275 return point;
1276}
1277
1285inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1286 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1287 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1288 double z_confidence = object.position.z_coordinate_is_present
1289 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1290 : std::numeric_limits<double>::infinity();
1291 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1292}
1293
1300inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1301
1308inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1309
1320inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1321 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1322 tf2::Quaternion q;
1323 double roll{0}, pitch{0}, yaw{0};
1324
1325 if (object.angles.x_angle_is_present) {
1326 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1327 M_PI;
1328 }
1329 if (object.angles.y_angle_is_present) {
1330 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1331 M_PI;
1332 }
1333 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1334 M_PI;
1335 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1336 q.setRPY(roll, pitch, yaw);
1337
1338 return tf2::toMsg(q);
1339}
1340
1347inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1348 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1349 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1350 double roll, pitch, yaw;
1351 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1352 return yaw;
1353}
1354
1362inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1363 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1364 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1365 yaw_confidence *= M_PI / 180.0; // convert to radians
1366 return yaw_confidence;
1367}
1368
1375inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1376 gm::Pose pose;
1377 pose.position = getPositionOfPerceivedObject(object);
1378 pose.orientation = getOrientationOfPerceivedObject(object);
1379 return pose;
1380}
1381
1389inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1390 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1391 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1392}
1393
1403inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1404 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1405 auto val = object.z_angular_velocity.confidence.value;
1406 static const std::map<uint8_t, double> confidence_map = {
1407 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1408 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1409 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1410 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1411 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1412 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1413 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1414 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1415 };
1416 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1417}
1418
1425inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1426
1433inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1434 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1435}
1436
1444inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1445 if (!object.velocity_is_present) {
1446 throw std::invalid_argument("No velocity present in PerceivedObject");
1447 }
1448 gm::Vector3 velocity;
1449 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1450 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1451 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1452 velocity.x = speed * cos(angle);
1453 velocity.y = speed * sin(angle);
1454 if (object.velocity.polar_velocity.z_velocity_is_present) {
1455 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1456 }
1457 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1458 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1459 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1460 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1461 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1462 }
1463 } else {
1464 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1465 }
1466 return velocity;
1467}
1468
1477inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1478 if (!object.velocity_is_present) {
1479 throw std::invalid_argument("No velocity present in PerceivedObject");
1480 }
1481 if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
1482 double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1483 double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1484 double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
1485 double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
1486 double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
1487 double x_confidence = speed_confidence * cos(angle) * cos(angle)
1488 + lateral_confidence * sin(angle) * sin(angle);
1489 double y_confidence = speed_confidence * sin(angle) * sin(angle)
1490 + lateral_confidence * cos(angle) * cos(angle);
1491 // neglect xy covariance, as it is not present in the output of this function
1492 double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
1493 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1494 : std::numeric_limits<double>::infinity();
1495 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1496 } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1497 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1498 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1499 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1500 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1501 : std::numeric_limits<double>::infinity();
1502 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1503 } else {
1504 throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
1505 }
1506
1507}
1508
1515inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1516 return double(acceleration.value.value) / 10.0;
1517}
1518
1525inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1526 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1527}
1528
1536inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1537 if (!object.acceleration_is_present) {
1538 throw std::invalid_argument("No acceleration present in PerceivedObject");
1539 }
1540 gm::Vector3 acceleration;
1541
1542 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1543 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1544 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1545 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1546 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1547 }
1548 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1549 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1550 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1551 acceleration.x = magnitude * cos(angle);
1552 acceleration.y = magnitude * sin(angle);
1553 if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
1554 acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
1555 }
1556 } else {
1557 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1558 }
1559
1560 return acceleration;
1561}
1562
1571inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1572 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1573
1574 if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1575 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1576 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1577 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1578 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1579 : std::numeric_limits<double>::infinity();
1580 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1581 } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
1582 double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
1583 double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
1584 double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
1585 double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
1586 double lateral_confidence = magnitude * angle_confidence; // best approximation
1587 double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
1588 + lateral_confidence * sin(angle) * sin(angle);
1589 double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
1590 + lateral_confidence * cos(angle) * cos(angle);
1591 // neglect xy covariance, as it is not present in the output of this function
1592 double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
1593 ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
1594 : std::numeric_limits<double>::infinity();
1595 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1596 } else {
1597 throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
1598 }
1599}
1600
1612inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1613 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1614 return object.object_dimension_x.value.value;
1615}
1616
1624inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1625 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1626 return object.object_dimension_x.confidence.value;
1627}
1628
1640inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1641 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1642 return object.object_dimension_y.value.value;
1643}
1644
1652inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1653 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1654 return object.object_dimension_y.confidence.value;
1655}
1656
1668inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1669 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1670 return object.object_dimension_z.value.value;
1671}
1672
1680inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1681 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1682 return object.object_dimension_z.confidence.value;
1683}
1684
1694inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1695 gm::Vector3 dimensions;
1696 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1697 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1698 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1699 return dimensions;
1700}
1701
1708inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1709 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1710 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1711 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1712 return {conf_x, conf_y, conf_z};
1713}
1714
1725inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1726 const PerceivedObject &object) {
1727 gm::PointStamped utm_position;
1728 gm::PointStamped reference_position = getUTMPosition(cpm);
1729 gm::Point relative_position = getPositionOfPerceivedObject(object);
1730
1731 utm_position.header.frame_id = reference_position.header.frame_id;
1732 utm_position.point.x = reference_position.point.x + relative_position.x;
1733 utm_position.point.y = reference_position.point.y + relative_position.y;
1734 utm_position.point.z = reference_position.point.z + relative_position.z;
1735
1736 return utm_position;
1737}
1738
1748inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1749 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1750}
1751
1758inline uint8_t getSensorID(const SensorInformation &sensor_information) {
1759 return sensor_information.sensor_id.value;
1760}
1761
1768inline uint8_t getSensorType(const SensorInformation &sensor_information) {
1769 return sensor_information.sensor_type.value;
1770}
1771
1772} // namespace etsi_its_cpm_ts_msgs::access

◆ WGSCovMatrixFromConfidenceEllipse()

std::array< double, 4 > etsi_its_cpm_ts_msgs::access::WGSCovMatrixFromConfidenceEllipse ( double semi_major,
double semi_minor,
double major_orientation )
inline

Convert the confidence ellipse to a covariance matrix.

Note that the major_orientation is given in degrees, while the object_heading is given in radians!

Parameters
semi_majorSemi major axis length in meters
semi_minorSemi minor axis length in meters
major_orientationOrientation of the major axis in degrees, relative to WGS84
Returns
std::array<double, 4> The covariance matrix in WGS coordinates (x = North, y = East)

Definition at line 273 of file cpm_ts_getters.h.

276 {