etsi_its_messages v3.1.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 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 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.
 

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 182 of file cpm_ts_getters.h.

182 {
183 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
184 }
185 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
186 return number;
187}
188
195inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
196 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
197}
198

◆ 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 760 of file cpm_ts_getters.h.

795 {
796
797#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
798
807inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
808
815inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
816 return cpm.payload.management_container.reference_time;
817}
818
825inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
826
833inline double getLatitude(const CollectivePerceptionMessage &cpm) {
834 return getLatitude(cpm.payload.management_container.reference_position.latitude);
835}
836
843inline double getLongitude(const CollectivePerceptionMessage &cpm) {
844 return getLongitude(cpm.payload.management_container.reference_position.longitude);
845}
846
853inline double getAltitude(const CollectivePerceptionMessage &cpm) {
854 return getAltitude(cpm.payload.management_container.reference_position.altitude);
855}
856
868inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
869 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
870}
871
881inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
882 int zone;
883 bool northp;
884 return getUTMPosition(cpm, zone, northp);
885}
886
896inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
897 std::vector<uint8_t> container_ids;
898 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
899 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
900 }
901 return container_ids;
902}
903
912inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
913 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
914 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
915 return cpm.payload.cpm_containers.value.array[i];
916 }
917 }
918 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
919}
920
929inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
930 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
931}
932
940inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
941 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
942 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
943 }
944 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
945 return number;
946}
947
954inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
955 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
956}
957
961
970inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
971 if (i >= getNumberOfPerceivedObjects(container)) {
972 throw std::invalid_argument("Index out of range");
973 }
974 return container.container_data_perceived_object_container.perceived_objects.array[i];
975}
976
985inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
986
993inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
994 return coordinate.value.value;
995}
996
1003inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1004 return coordinate.confidence.value;
1005}
1006
1013inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1014 gm::Point point;
1015 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1016 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1017 if (object.position.z_coordinate_is_present) {
1018 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1019 }
1020 return point;
1021}
1022
1030inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1031 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1032 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1033 double z_confidence = object.position.z_coordinate_is_present
1034 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1035 : std::numeric_limits<double>::infinity();
1036 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1037}
1038
1045inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1046
1053inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1054
1065inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1066 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1067 tf2::Quaternion q;
1068 double roll{0}, pitch{0}, yaw{0};
1069
1070 if (object.angles.x_angle_is_present) {
1071 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1072 M_PI;
1073 }
1074 if (object.angles.y_angle_is_present) {
1075 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1076 M_PI;
1077 }
1078 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1079 M_PI;
1080 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1081 q.setRPY(roll, pitch, yaw);
1082
1083 return tf2::toMsg(q);
1084}
1085
1092inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1093 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1094 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1095 double roll, pitch, yaw;
1096 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1097 return yaw;
1098}
1099
1107inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1108 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1109 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1110 yaw_confidence *= M_PI / 180.0; // convert to radians
1111 return yaw_confidence;
1112}
1113
1120inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1121 gm::Pose pose;
1122 pose.position = getPositionOfPerceivedObject(object);
1123 pose.orientation = getOrientationOfPerceivedObject(object);
1124 return pose;
1125}
1126
1134inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1135 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1136 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1137}
1138
1148inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1149 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1150 auto val = object.z_angular_velocity.confidence.value;
1151 static const std::map<uint8_t, double> confidence_map = {
1152 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1153 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1154 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1155 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1156 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1157 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1158 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1159 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1160 };
1161 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1162}
1163
1170inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1171
1178inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1179 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1180}
1181
1189inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1190 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1191 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1192 throw std::invalid_argument("Velocity is not Cartesian");
1193 }
1194 gm::Vector3 velocity;
1195 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1196 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1197 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1198 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1199 }
1200 return velocity;
1201}
1202
1211inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1212 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1213 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1214 throw std::invalid_argument("Velocity is not Cartesian");
1215 }
1216 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1217 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1218 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1219 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1220 : std::numeric_limits<double>::infinity();
1221 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1222}
1223
1230inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1231 return double(acceleration.value.value) / 10.0;
1232}
1233
1240inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1241 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1242}
1243
1251inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1252 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1253 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1254 throw std::invalid_argument("Acceleration is not Cartesian");
1255 }
1256 gm::Vector3 acceleration;
1257 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1258 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1259 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1260 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1261 }
1262 return acceleration;
1263}
1264
1273inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1274 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1275 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1276 throw std::invalid_argument("Acceleration is not Cartesian");
1277 }
1278 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1279 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1280 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1281 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1282 : std::numeric_limits<double>::infinity();
1283 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1284}
1285
1297inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1298 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1299 return object.object_dimension_x.value.value;
1300}
1301
1309inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1310 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1311 return object.object_dimension_x.confidence.value;
1312}
1313
1325inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1326 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1327 return object.object_dimension_y.value.value;
1328}
1329
1337inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1338 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1339 return object.object_dimension_y.confidence.value;
1340}
1341
1353inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1354 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1355 return object.object_dimension_z.value.value;
1356}
1357
1365inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1366 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1367 return object.object_dimension_z.confidence.value;
1368}
1369
1379inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1380 gm::Vector3 dimensions;
1381 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1382 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1383 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1384 return dimensions;
1385}
1386
1393inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1394 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1395 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1396 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1397 return {conf_x, conf_y, conf_z};
1398}
1399
1410inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1411 const PerceivedObject &object) {
1412 gm::PointStamped utm_position;
1413 gm::PointStamped reference_position = getUTMPosition(cpm);
1414 gm::Point relative_position = getPositionOfPerceivedObject(object);
1415
1416 utm_position.header.frame_id = reference_position.header.frame_id;
1417 utm_position.point.x = reference_position.point.x + relative_position.x;
1418 utm_position.point.y = reference_position.point.y + relative_position.y;
1419 utm_position.point.z = reference_position.point.z + relative_position.z;
1420
1421 return utm_position;
1422}
1423
1433inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1434 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1435}
1436
1437} // 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:678
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:923
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:803
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:637
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:523
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:883
etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects
uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container)
Definition cpm_ts_getters.h:470
etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject
gm::Point getPositionOfPerceivedObject(const PerceivedObject &object)
Definition cpm_ts_getters.h:543
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:533
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:583
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:855
etsi_its_cpm_ts_msgs::access::getVelocityComponent
double getVelocityComponent(const VelocityComponent &velocity)
Get the velocity component of the PerceivedObject.
Definition cpm_ts_getters.h:700
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:459
etsi_its_cpm_ts_msgs::access::getYawRateOfPerceivedObject
double getYawRateOfPerceivedObject(const PerceivedObject &object)
Get the yaw rate of the PerceivedObject.
Definition cpm_ts_getters.h:664
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:909
etsi_its_cpm_ts_msgs::access::getPoseOfPerceivedObject
gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object)
Get the pose of the PerceivedObject.
Definition cpm_ts_getters.h:650
etsi_its_cpm_ts_msgs::access::getYawOfPerceivedObject
double getYawOfPerceivedObject(const PerceivedObject &object)
Get the yaw of the PerceivedObject.
Definition cpm_ts_getters.h:622
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:867
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:940
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:741
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:895
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:575
etsi_its_cpm_ts_msgs::access::getVelocityComponentConfidence
double getVelocityComponentConfidence(const VelocityComponent &velocity)
Get the confidence of the velocity component.
Definition cpm_ts_getters.h:708
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:237
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:839
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:515
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:719
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:827
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:426
etsi_its_cpm_ts_msgs::access::getAccelerationComponentConfidence
double getAccelerationComponentConfidence(const AccelerationComponent &acceleration)
Get the confidence of the acceleration component.
Definition cpm_ts_getters.h:770
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:963
etsi_its_cpm_ts_msgs::access::getCpmContainer
WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id)
Definition cpm_ts_getters.h:442
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:500
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:781
etsi_its_cpm_ts_msgs::access::getAccelerationComponent
double getAccelerationComponent(const AccelerationComponent &acceleration)
Get the acceleration component of the PerceivedObject.
Definition cpm_ts_getters.h:760
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:560
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:595
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:103

◆ 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 770 of file cpm_ts_getters.h.

805 {
806
807#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
808
817inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
818
825inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
826 return cpm.payload.management_container.reference_time;
827}
828
835inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
836
843inline double getLatitude(const CollectivePerceptionMessage &cpm) {
844 return getLatitude(cpm.payload.management_container.reference_position.latitude);
845}
846
853inline double getLongitude(const CollectivePerceptionMessage &cpm) {
854 return getLongitude(cpm.payload.management_container.reference_position.longitude);
855}
856
863inline double getAltitude(const CollectivePerceptionMessage &cpm) {
864 return getAltitude(cpm.payload.management_container.reference_position.altitude);
865}
866
878inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
879 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
880}
881
891inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
892 int zone;
893 bool northp;
894 return getUTMPosition(cpm, zone, northp);
895}
896
906inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
907 std::vector<uint8_t> container_ids;
908 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
909 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
910 }
911 return container_ids;
912}
913
922inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
923 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
924 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
925 return cpm.payload.cpm_containers.value.array[i];
926 }
927 }
928 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
929}
930
939inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
940 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
941}
942
950inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
951 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
952 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
953 }
954 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
955 return number;
956}
957
964inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
965 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
966}
967
971
980inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
981 if (i >= getNumberOfPerceivedObjects(container)) {
982 throw std::invalid_argument("Index out of range");
983 }
984 return container.container_data_perceived_object_container.perceived_objects.array[i];
985}
986
995inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
996
1003inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1004 return coordinate.value.value;
1005}
1006
1013inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1014 return coordinate.confidence.value;
1015}
1016
1023inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1024 gm::Point point;
1025 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1026 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1027 if (object.position.z_coordinate_is_present) {
1028 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1029 }
1030 return point;
1031}
1032
1040inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1041 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1042 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1043 double z_confidence = object.position.z_coordinate_is_present
1044 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1045 : std::numeric_limits<double>::infinity();
1046 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1047}
1048
1055inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1056
1063inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1064
1075inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1076 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1077 tf2::Quaternion q;
1078 double roll{0}, pitch{0}, yaw{0};
1079
1080 if (object.angles.x_angle_is_present) {
1081 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1082 M_PI;
1083 }
1084 if (object.angles.y_angle_is_present) {
1085 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1086 M_PI;
1087 }
1088 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1089 M_PI;
1090 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1091 q.setRPY(roll, pitch, yaw);
1092
1093 return tf2::toMsg(q);
1094}
1095
1102inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1103 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1104 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1105 double roll, pitch, yaw;
1106 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1107 return yaw;
1108}
1109
1117inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1118 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1119 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1120 yaw_confidence *= M_PI / 180.0; // convert to radians
1121 return yaw_confidence;
1122}
1123
1130inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1131 gm::Pose pose;
1132 pose.position = getPositionOfPerceivedObject(object);
1133 pose.orientation = getOrientationOfPerceivedObject(object);
1134 return pose;
1135}
1136
1144inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1145 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1146 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1147}
1148
1158inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1159 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1160 auto val = object.z_angular_velocity.confidence.value;
1161 static const std::map<uint8_t, double> confidence_map = {
1162 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1163 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1164 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1165 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1166 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1167 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1168 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1169 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1170 };
1171 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1172}
1173
1180inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1181
1188inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1189 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1190}
1191
1199inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1200 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1201 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1202 throw std::invalid_argument("Velocity is not Cartesian");
1203 }
1204 gm::Vector3 velocity;
1205 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1206 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1207 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1208 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1209 }
1210 return velocity;
1211}
1212
1221inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1222 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1223 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1224 throw std::invalid_argument("Velocity is not Cartesian");
1225 }
1226 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1227 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1228 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1229 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1230 : std::numeric_limits<double>::infinity();
1231 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1232}
1233
1240inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1241 return double(acceleration.value.value) / 10.0;
1242}
1243
1250inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1251 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1252}
1253
1261inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1262 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1263 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1264 throw std::invalid_argument("Acceleration is not Cartesian");
1265 }
1266 gm::Vector3 acceleration;
1267 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1268 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1269 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1270 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1271 }
1272 return acceleration;
1273}
1274
1283inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1284 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1285 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1286 throw std::invalid_argument("Acceleration is not Cartesian");
1287 }
1288 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1289 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1290 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1291 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1292 : std::numeric_limits<double>::infinity();
1293 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1294}
1295
1307inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1308 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1309 return object.object_dimension_x.value.value;
1310}
1311
1319inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1320 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1321 return object.object_dimension_x.confidence.value;
1322}
1323
1335inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1336 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1337 return object.object_dimension_y.value.value;
1338}
1339
1347inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1348 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1349 return object.object_dimension_y.confidence.value;
1350}
1351
1363inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1364 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1365 return object.object_dimension_z.value.value;
1366}
1367
1375inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1376 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1377 return object.object_dimension_z.confidence.value;
1378}
1379
1389inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1390 gm::Vector3 dimensions;
1391 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1392 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1393 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1394 return dimensions;
1395}
1396
1403inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1404 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1405 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1406 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1407 return {conf_x, conf_y, conf_z};
1408}
1409
1420inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1421 const PerceivedObject &object) {
1422 gm::PointStamped utm_position;
1423 gm::PointStamped reference_position = getUTMPosition(cpm);
1424 gm::Point relative_position = getPositionOfPerceivedObject(object);
1425
1426 utm_position.header.frame_id = reference_position.header.frame_id;
1427 utm_position.point.x = reference_position.point.x + relative_position.x;
1428 utm_position.point.y = reference_position.point.y + relative_position.y;
1429 utm_position.point.z = reference_position.point.z + relative_position.z;
1430
1431 return utm_position;
1432}
1433
1443inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1444 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1445}
1446
1447} // namespace etsi_its_cpm_ts_msgs::access

◆ 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

Definition at line 71 of file cpm_ts_getters.h.

◆ 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 383 of file cpm_ts_getters.h.

◆ 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 803 of file cpm_ts_getters.h.

838 {
839
840#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
841
850inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
851
858inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
859 return cpm.payload.management_container.reference_time;
860}
861
868inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
869
876inline double getLatitude(const CollectivePerceptionMessage &cpm) {
877 return getLatitude(cpm.payload.management_container.reference_position.latitude);
878}
879
886inline double getLongitude(const CollectivePerceptionMessage &cpm) {
887 return getLongitude(cpm.payload.management_container.reference_position.longitude);
888}
889
896inline double getAltitude(const CollectivePerceptionMessage &cpm) {
897 return getAltitude(cpm.payload.management_container.reference_position.altitude);
898}
899
911inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
912 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
913}
914
924inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
925 int zone;
926 bool northp;
927 return getUTMPosition(cpm, zone, northp);
928}
929
939inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
940 std::vector<uint8_t> container_ids;
941 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
942 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
943 }
944 return container_ids;
945}
946
955inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
956 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
957 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
958 return cpm.payload.cpm_containers.value.array[i];
959 }
960 }
961 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
962}
963
972inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
973 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
974}
975
983inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
984 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
985 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
986 }
987 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
988 return number;
989}
990
997inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
998 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
999}
1000
1004
1013inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1014 if (i >= getNumberOfPerceivedObjects(container)) {
1015 throw std::invalid_argument("Index out of range");
1016 }
1017 return container.container_data_perceived_object_container.perceived_objects.array[i];
1018}
1019
1028inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1029
1036inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1037 return coordinate.value.value;
1038}
1039
1046inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1047 return coordinate.confidence.value;
1048}
1049
1056inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1057 gm::Point point;
1058 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1059 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1060 if (object.position.z_coordinate_is_present) {
1061 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1062 }
1063 return point;
1064}
1065
1073inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1074 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1075 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1076 double z_confidence = object.position.z_coordinate_is_present
1077 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1078 : std::numeric_limits<double>::infinity();
1079 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1080}
1081
1088inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1089
1096inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1097
1108inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1109 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1110 tf2::Quaternion q;
1111 double roll{0}, pitch{0}, yaw{0};
1112
1113 if (object.angles.x_angle_is_present) {
1114 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1115 M_PI;
1116 }
1117 if (object.angles.y_angle_is_present) {
1118 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1119 M_PI;
1120 }
1121 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1122 M_PI;
1123 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1124 q.setRPY(roll, pitch, yaw);
1125
1126 return tf2::toMsg(q);
1127}
1128
1135inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1136 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1137 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1138 double roll, pitch, yaw;
1139 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1140 return yaw;
1141}
1142
1150inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1151 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1152 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1153 yaw_confidence *= M_PI / 180.0; // convert to radians
1154 return yaw_confidence;
1155}
1156
1163inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1164 gm::Pose pose;
1165 pose.position = getPositionOfPerceivedObject(object);
1166 pose.orientation = getOrientationOfPerceivedObject(object);
1167 return pose;
1168}
1169
1177inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1178 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1179 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1180}
1181
1191inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1192 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1193 auto val = object.z_angular_velocity.confidence.value;
1194 static const std::map<uint8_t, double> confidence_map = {
1195 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1196 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1197 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1198 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1199 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1200 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1201 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1202 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1203 };
1204 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1205}
1206
1213inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1214
1221inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1222 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1223}
1224
1232inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1233 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1234 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1235 throw std::invalid_argument("Velocity is not Cartesian");
1236 }
1237 gm::Vector3 velocity;
1238 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1239 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1240 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1241 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1242 }
1243 return velocity;
1244}
1245
1254inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1255 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1256 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1257 throw std::invalid_argument("Velocity is not Cartesian");
1258 }
1259 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1260 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1261 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1262 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1263 : std::numeric_limits<double>::infinity();
1264 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1265}
1266
1273inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1274 return double(acceleration.value.value) / 10.0;
1275}
1276
1283inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1284 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1285}
1286
1294inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1295 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1296 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1297 throw std::invalid_argument("Acceleration is not Cartesian");
1298 }
1299 gm::Vector3 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 return acceleration;
1306}
1307
1316inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1317 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1318 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1319 throw std::invalid_argument("Acceleration is not Cartesian");
1320 }
1321 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1322 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1323 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1324 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1325 : std::numeric_limits<double>::infinity();
1326 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1327}
1328
1340inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1341 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1342 return object.object_dimension_x.value.value;
1343}
1344
1352inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1353 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1354 return object.object_dimension_x.confidence.value;
1355}
1356
1368inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1369 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1370 return object.object_dimension_y.value.value;
1371}
1372
1380inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1381 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1382 return object.object_dimension_y.confidence.value;
1383}
1384
1396inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1397 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1398 return object.object_dimension_z.value.value;
1399}
1400
1408inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1409 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1410 return object.object_dimension_z.confidence.value;
1411}
1412
1422inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1423 gm::Vector3 dimensions;
1424 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1425 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1426 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1427 return dimensions;
1428}
1429
1436inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1437 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1438 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1439 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1440 return {conf_x, conf_y, conf_z};
1441}
1442
1453inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1454 const PerceivedObject &object) {
1455 gm::PointStamped utm_position;
1456 gm::PointStamped reference_position = getUTMPosition(cpm);
1457 gm::Point relative_position = getPositionOfPerceivedObject(object);
1458
1459 utm_position.header.frame_id = reference_position.header.frame_id;
1460 utm_position.point.x = reference_position.point.x + relative_position.x;
1461 utm_position.point.y = reference_position.point.y + relative_position.y;
1462 utm_position.point.z = reference_position.point.z + relative_position.z;
1463
1464 return utm_position;
1465}
1466
1476inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1477 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1478}
1479
1480} // 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 781 of file cpm_ts_getters.h.

816 {
817
818#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
819
828inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
829
836inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
837 return cpm.payload.management_container.reference_time;
838}
839
846inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
847
854inline double getLatitude(const CollectivePerceptionMessage &cpm) {
855 return getLatitude(cpm.payload.management_container.reference_position.latitude);
856}
857
864inline double getLongitude(const CollectivePerceptionMessage &cpm) {
865 return getLongitude(cpm.payload.management_container.reference_position.longitude);
866}
867
874inline double getAltitude(const CollectivePerceptionMessage &cpm) {
875 return getAltitude(cpm.payload.management_container.reference_position.altitude);
876}
877
889inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
890 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
891}
892
902inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
903 int zone;
904 bool northp;
905 return getUTMPosition(cpm, zone, northp);
906}
907
917inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
918 std::vector<uint8_t> container_ids;
919 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
920 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
921 }
922 return container_ids;
923}
924
933inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
934 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
935 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
936 return cpm.payload.cpm_containers.value.array[i];
937 }
938 }
939 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
940}
941
950inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
951 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
952}
953
961inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
962 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
963 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
964 }
965 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
966 return number;
967}
968
975inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
976 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
977}
978
982
991inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
992 if (i >= getNumberOfPerceivedObjects(container)) {
993 throw std::invalid_argument("Index out of range");
994 }
995 return container.container_data_perceived_object_container.perceived_objects.array[i];
996}
997
1006inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1007
1014inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1015 return coordinate.value.value;
1016}
1017
1024inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1025 return coordinate.confidence.value;
1026}
1027
1034inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1035 gm::Point point;
1036 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1037 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1038 if (object.position.z_coordinate_is_present) {
1039 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1040 }
1041 return point;
1042}
1043
1051inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1052 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1053 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1054 double z_confidence = object.position.z_coordinate_is_present
1055 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1056 : std::numeric_limits<double>::infinity();
1057 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1058}
1059
1066inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1067
1074inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1075
1086inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1087 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1088 tf2::Quaternion q;
1089 double roll{0}, pitch{0}, yaw{0};
1090
1091 if (object.angles.x_angle_is_present) {
1092 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1093 M_PI;
1094 }
1095 if (object.angles.y_angle_is_present) {
1096 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1097 M_PI;
1098 }
1099 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1100 M_PI;
1101 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1102 q.setRPY(roll, pitch, yaw);
1103
1104 return tf2::toMsg(q);
1105}
1106
1113inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1114 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1115 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1116 double roll, pitch, yaw;
1117 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1118 return yaw;
1119}
1120
1128inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1129 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1130 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1131 yaw_confidence *= M_PI / 180.0; // convert to radians
1132 return yaw_confidence;
1133}
1134
1141inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1142 gm::Pose pose;
1143 pose.position = getPositionOfPerceivedObject(object);
1144 pose.orientation = getOrientationOfPerceivedObject(object);
1145 return pose;
1146}
1147
1155inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1156 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1157 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1158}
1159
1169inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1170 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1171 auto val = object.z_angular_velocity.confidence.value;
1172 static const std::map<uint8_t, double> confidence_map = {
1173 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1174 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1175 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1176 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1177 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1178 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1179 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1180 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1181 };
1182 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1183}
1184
1191inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1192
1199inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1200 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1201}
1202
1210inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1211 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1212 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1213 throw std::invalid_argument("Velocity is not Cartesian");
1214 }
1215 gm::Vector3 velocity;
1216 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1217 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1218 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1219 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1220 }
1221 return velocity;
1222}
1223
1232inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1233 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1234 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1235 throw std::invalid_argument("Velocity is not Cartesian");
1236 }
1237 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1238 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1239 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1240 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1241 : std::numeric_limits<double>::infinity();
1242 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1243}
1244
1251inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1252 return double(acceleration.value.value) / 10.0;
1253}
1254
1261inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1262 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1263}
1264
1272inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1273 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1274 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1275 throw std::invalid_argument("Acceleration is not Cartesian");
1276 }
1277 gm::Vector3 acceleration;
1278 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1279 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1280 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1281 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1282 }
1283 return acceleration;
1284}
1285
1294inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1295 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1296 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1297 throw std::invalid_argument("Acceleration is not Cartesian");
1298 }
1299 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1300 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1301 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1302 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1303 : std::numeric_limits<double>::infinity();
1304 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1305}
1306
1318inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1319 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1320 return object.object_dimension_x.value.value;
1321}
1322
1330inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1331 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1332 return object.object_dimension_x.confidence.value;
1333}
1334
1346inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1347 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1348 return object.object_dimension_y.value.value;
1349}
1350
1358inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1359 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1360 return object.object_dimension_y.confidence.value;
1361}
1362
1374inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1375 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1376 return object.object_dimension_z.value.value;
1377}
1378
1386inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1387 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1388 return object.object_dimension_z.confidence.value;
1389}
1390
1400inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1401 gm::Vector3 dimensions;
1402 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1403 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1404 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1405 return dimensions;
1406}
1407
1414inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1415 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1416 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1417 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1418 return {conf_x, conf_y, conf_z};
1419}
1420
1431inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1432 const PerceivedObject &object) {
1433 gm::PointStamped utm_position;
1434 gm::PointStamped reference_position = getUTMPosition(cpm);
1435 gm::Point relative_position = getPositionOfPerceivedObject(object);
1436
1437 utm_position.header.frame_id = reference_position.header.frame_id;
1438 utm_position.point.x = reference_position.point.x + relative_position.x;
1439 utm_position.point.y = reference_position.point.y + relative_position.y;
1440 utm_position.point.z = reference_position.point.z + relative_position.z;
1441
1442 return utm_position;
1443}
1444
1454inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1455 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1456}
1457
1458} // 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 575 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 583 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 523 of file cpm_ts_getters.h.

523 : std::numeric_limits<double>::infinity();
524 return std::make_tuple(x_confidence, y_confidence, z_confidence);
525}

◆ 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 533 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 741 of file cpm_ts_getters.h.

776 {
777
778#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
779
788inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
789
796inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
797 return cpm.payload.management_container.reference_time;
798}
799
806inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
807
814inline double getLatitude(const CollectivePerceptionMessage &cpm) {
815 return getLatitude(cpm.payload.management_container.reference_position.latitude);
816}
817
824inline double getLongitude(const CollectivePerceptionMessage &cpm) {
825 return getLongitude(cpm.payload.management_container.reference_position.longitude);
826}
827
834inline double getAltitude(const CollectivePerceptionMessage &cpm) {
835 return getAltitude(cpm.payload.management_container.reference_position.altitude);
836}
837
849inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
850 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
851}
852
862inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
863 int zone;
864 bool northp;
865 return getUTMPosition(cpm, zone, northp);
866}
867
877inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
878 std::vector<uint8_t> container_ids;
879 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
880 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
881 }
882 return container_ids;
883}
884
893inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
894 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
895 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
896 return cpm.payload.cpm_containers.value.array[i];
897 }
898 }
899 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
900}
901
910inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
911 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
912}
913
921inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
922 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
923 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
924 }
925 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
926 return number;
927}
928
935inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
936 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
937}
938
942
951inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
952 if (i >= getNumberOfPerceivedObjects(container)) {
953 throw std::invalid_argument("Index out of range");
954 }
955 return container.container_data_perceived_object_container.perceived_objects.array[i];
956}
957
966inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
967
974inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
975 return coordinate.value.value;
976}
977
984inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
985 return coordinate.confidence.value;
986}
987
994inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
995 gm::Point point;
996 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
997 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
998 if (object.position.z_coordinate_is_present) {
999 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1000 }
1001 return point;
1002}
1003
1011inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1012 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1013 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1014 double z_confidence = object.position.z_coordinate_is_present
1015 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1016 : std::numeric_limits<double>::infinity();
1017 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1018}
1019
1026inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1027
1034inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1035
1046inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1047 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1048 tf2::Quaternion q;
1049 double roll{0}, pitch{0}, yaw{0};
1050
1051 if (object.angles.x_angle_is_present) {
1052 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1053 M_PI;
1054 }
1055 if (object.angles.y_angle_is_present) {
1056 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1057 M_PI;
1058 }
1059 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1060 M_PI;
1061 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1062 q.setRPY(roll, pitch, yaw);
1063
1064 return tf2::toMsg(q);
1065}
1066
1073inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1074 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1075 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1076 double roll, pitch, yaw;
1077 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1078 return yaw;
1079}
1080
1088inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1089 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1090 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1091 yaw_confidence *= M_PI / 180.0; // convert to radians
1092 return yaw_confidence;
1093}
1094
1101inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1102 gm::Pose pose;
1103 pose.position = getPositionOfPerceivedObject(object);
1104 pose.orientation = getOrientationOfPerceivedObject(object);
1105 return pose;
1106}
1107
1115inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1116 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1117 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1118}
1119
1129inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1130 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1131 auto val = object.z_angular_velocity.confidence.value;
1132 static const std::map<uint8_t, double> confidence_map = {
1133 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1134 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1135 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1136 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1137 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1138 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1139 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1140 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1141 };
1142 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1143}
1144
1151inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1152
1159inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1160 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1161}
1162
1170inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1171 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1172 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1173 throw std::invalid_argument("Velocity is not Cartesian");
1174 }
1175 gm::Vector3 velocity;
1176 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1177 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1178 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1179 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1180 }
1181 return velocity;
1182}
1183
1192inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1193 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1194 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1195 throw std::invalid_argument("Velocity is not Cartesian");
1196 }
1197 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1198 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1199 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1200 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1201 : std::numeric_limits<double>::infinity();
1202 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1203}
1204
1211inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1212 return double(acceleration.value.value) / 10.0;
1213}
1214
1221inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1222 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1223}
1224
1232inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1233 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1234 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1235 throw std::invalid_argument("Acceleration is not Cartesian");
1236 }
1237 gm::Vector3 acceleration;
1238 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1239 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1240 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1241 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1242 }
1243 return acceleration;
1244}
1245
1254inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1255 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1256 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1257 throw std::invalid_argument("Acceleration is not Cartesian");
1258 }
1259 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1260 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1261 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1262 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1263 : std::numeric_limits<double>::infinity();
1264 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1265}
1266
1278inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1279 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1280 return object.object_dimension_x.value.value;
1281}
1282
1290inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1291 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1292 return object.object_dimension_x.confidence.value;
1293}
1294
1306inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1307 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1308 return object.object_dimension_y.value.value;
1309}
1310
1318inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1319 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1320 return object.object_dimension_y.confidence.value;
1321}
1322
1334inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1335 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1336 return object.object_dimension_z.value.value;
1337}
1338
1346inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1347 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1348 return object.object_dimension_z.confidence.value;
1349}
1350
1360inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1361 gm::Vector3 dimensions;
1362 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1363 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1364 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1365 return dimensions;
1366}
1367
1374inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1375 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1376 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1377 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1378 return {conf_x, conf_y, conf_z};
1379}
1380
1391inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1392 const PerceivedObject &object) {
1393 gm::PointStamped utm_position;
1394 gm::PointStamped reference_position = getUTMPosition(cpm);
1395 gm::Point relative_position = getPositionOfPerceivedObject(object);
1396
1397 utm_position.header.frame_id = reference_position.header.frame_id;
1398 utm_position.point.x = reference_position.point.x + relative_position.x;
1399 utm_position.point.y = reference_position.point.y + relative_position.y;
1400 utm_position.point.z = reference_position.point.z + relative_position.z;
1401
1402 return utm_position;
1403}
1404
1414inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1415 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1416}
1417
1418} // 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 719 of file cpm_ts_getters.h.

754 {
755
756#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
757
766inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
767
774inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
775 return cpm.payload.management_container.reference_time;
776}
777
784inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
785
792inline double getLatitude(const CollectivePerceptionMessage &cpm) {
793 return getLatitude(cpm.payload.management_container.reference_position.latitude);
794}
795
802inline double getLongitude(const CollectivePerceptionMessage &cpm) {
803 return getLongitude(cpm.payload.management_container.reference_position.longitude);
804}
805
812inline double getAltitude(const CollectivePerceptionMessage &cpm) {
813 return getAltitude(cpm.payload.management_container.reference_position.altitude);
814}
815
827inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
828 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
829}
830
840inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
841 int zone;
842 bool northp;
843 return getUTMPosition(cpm, zone, northp);
844}
845
855inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
856 std::vector<uint8_t> container_ids;
857 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
858 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
859 }
860 return container_ids;
861}
862
871inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
872 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
873 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
874 return cpm.payload.cpm_containers.value.array[i];
875 }
876 }
877 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
878}
879
888inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
889 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
890}
891
899inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
900 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
901 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
902 }
903 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
904 return number;
905}
906
913inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
914 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
915}
916
920
929inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
930 if (i >= getNumberOfPerceivedObjects(container)) {
931 throw std::invalid_argument("Index out of range");
932 }
933 return container.container_data_perceived_object_container.perceived_objects.array[i];
934}
935
944inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
945
952inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
953 return coordinate.value.value;
954}
955
962inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
963 return coordinate.confidence.value;
964}
965
972inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
973 gm::Point point;
974 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
975 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
976 if (object.position.z_coordinate_is_present) {
977 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
978 }
979 return point;
980}
981
989inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
990 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
991 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
992 double z_confidence = object.position.z_coordinate_is_present
993 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
994 : std::numeric_limits<double>::infinity();
995 return std::make_tuple(x_confidence, y_confidence, z_confidence);
996}
997
1004inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1005
1012inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1013
1024inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1025 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1026 tf2::Quaternion q;
1027 double roll{0}, pitch{0}, yaw{0};
1028
1029 if (object.angles.x_angle_is_present) {
1030 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1031 M_PI;
1032 }
1033 if (object.angles.y_angle_is_present) {
1034 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1035 M_PI;
1036 }
1037 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1038 M_PI;
1039 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1040 q.setRPY(roll, pitch, yaw);
1041
1042 return tf2::toMsg(q);
1043}
1044
1051inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1052 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1053 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1054 double roll, pitch, yaw;
1055 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1056 return yaw;
1057}
1058
1066inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1067 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1068 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1069 yaw_confidence *= M_PI / 180.0; // convert to radians
1070 return yaw_confidence;
1071}
1072
1079inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1080 gm::Pose pose;
1081 pose.position = getPositionOfPerceivedObject(object);
1082 pose.orientation = getOrientationOfPerceivedObject(object);
1083 return pose;
1084}
1085
1093inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1094 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1095 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1096}
1097
1107inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1108 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1109 auto val = object.z_angular_velocity.confidence.value;
1110 static const std::map<uint8_t, double> confidence_map = {
1111 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1112 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1113 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1114 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1115 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1116 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1117 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1118 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1119 };
1120 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1121}
1122
1129inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1130
1137inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1138 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1139}
1140
1148inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1149 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1150 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1151 throw std::invalid_argument("Velocity is not Cartesian");
1152 }
1153 gm::Vector3 velocity;
1154 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1155 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1156 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1157 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1158 }
1159 return velocity;
1160}
1161
1170inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1171 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1172 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1173 throw std::invalid_argument("Velocity is not Cartesian");
1174 }
1175 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1176 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1177 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1178 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1179 : std::numeric_limits<double>::infinity();
1180 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1181}
1182
1189inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1190 return double(acceleration.value.value) / 10.0;
1191}
1192
1199inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1200 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1201}
1202
1210inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1211 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1212 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1213 throw std::invalid_argument("Acceleration is not Cartesian");
1214 }
1215 gm::Vector3 acceleration;
1216 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1217 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1218 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1219 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1220 }
1221 return acceleration;
1222}
1223
1232inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1233 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1234 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1235 throw std::invalid_argument("Acceleration is not Cartesian");
1236 }
1237 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1238 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1239 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1240 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1241 : std::numeric_limits<double>::infinity();
1242 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1243}
1244
1256inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1257 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1258 return object.object_dimension_x.value.value;
1259}
1260
1268inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1269 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1270 return object.object_dimension_x.confidence.value;
1271}
1272
1284inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1285 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1286 return object.object_dimension_y.value.value;
1287}
1288
1296inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1297 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1298 return object.object_dimension_y.confidence.value;
1299}
1300
1312inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1313 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1314 return object.object_dimension_z.value.value;
1315}
1316
1324inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1325 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1326 return object.object_dimension_z.confidence.value;
1327}
1328
1338inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1339 gm::Vector3 dimensions;
1340 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1341 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1342 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1343 return dimensions;
1344}
1345
1352inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1353 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1354 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1355 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1356 return {conf_x, conf_y, conf_z};
1357}
1358
1369inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1370 const PerceivedObject &object) {
1371 gm::PointStamped utm_position;
1372 gm::PointStamped reference_position = getUTMPosition(cpm);
1373 gm::Point relative_position = getPositionOfPerceivedObject(object);
1374
1375 utm_position.header.frame_id = reference_position.header.frame_id;
1376 utm_position.point.x = reference_position.point.x + relative_position.x;
1377 utm_position.point.y = reference_position.point.y + relative_position.y;
1378 utm_position.point.z = reference_position.point.z + relative_position.z;
1379
1380 return utm_position;
1381}
1382
1392inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1393 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1394}
1395
1396} // 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 442 of file cpm_ts_getters.h.

◆ 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 426 of file cpm_ts_getters.h.

430 {
431 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
432 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {

◆ 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 923 of file cpm_ts_getters.h.

958 {
959
960#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
961
970inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
971
978inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
979 return cpm.payload.management_container.reference_time;
980}
981
988inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
989
996inline double getLatitude(const CollectivePerceptionMessage &cpm) {
997 return getLatitude(cpm.payload.management_container.reference_position.latitude);
998}
999
1006inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1007 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1008}
1009
1016inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1017 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1018}
1019
1031inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1032 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1033}
1034
1044inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1045 int zone;
1046 bool northp;
1047 return getUTMPosition(cpm, zone, northp);
1048}
1049
1059inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1060 std::vector<uint8_t> container_ids;
1061 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1062 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1063 }
1064 return container_ids;
1065}
1066
1075inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1076 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1077 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1078 return cpm.payload.cpm_containers.value.array[i];
1079 }
1080 }
1081 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1082}
1083
1092inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1093 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1094}
1095
1103inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1104 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1105 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1106 }
1107 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1108 return number;
1109}
1110
1117inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1118 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1119}
1120
1124
1133inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1134 if (i >= getNumberOfPerceivedObjects(container)) {
1135 throw std::invalid_argument("Index out of range");
1136 }
1137 return container.container_data_perceived_object_container.perceived_objects.array[i];
1138}
1139
1148inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1149
1156inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1157 return coordinate.value.value;
1158}
1159
1166inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1167 return coordinate.confidence.value;
1168}
1169
1176inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1177 gm::Point point;
1178 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1179 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1180 if (object.position.z_coordinate_is_present) {
1181 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1182 }
1183 return point;
1184}
1185
1193inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1194 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1195 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1196 double z_confidence = object.position.z_coordinate_is_present
1197 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1198 : std::numeric_limits<double>::infinity();
1199 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1200}
1201
1208inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1209
1216inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1217
1228inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1229 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1230 tf2::Quaternion q;
1231 double roll{0}, pitch{0}, yaw{0};
1232
1233 if (object.angles.x_angle_is_present) {
1234 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1235 M_PI;
1236 }
1237 if (object.angles.y_angle_is_present) {
1238 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1239 M_PI;
1240 }
1241 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1242 M_PI;
1243 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1244 q.setRPY(roll, pitch, yaw);
1245
1246 return tf2::toMsg(q);
1247}
1248
1255inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1256 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1257 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1258 double roll, pitch, yaw;
1259 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1260 return yaw;
1261}
1262
1270inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1271 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1272 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1273 yaw_confidence *= M_PI / 180.0; // convert to radians
1274 return yaw_confidence;
1275}
1276
1283inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1284 gm::Pose pose;
1285 pose.position = getPositionOfPerceivedObject(object);
1286 pose.orientation = getOrientationOfPerceivedObject(object);
1287 return pose;
1288}
1289
1297inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1298 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1299 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1300}
1301
1311inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1312 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1313 auto val = object.z_angular_velocity.confidence.value;
1314 static const std::map<uint8_t, double> confidence_map = {
1315 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1316 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1317 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1318 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1319 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1320 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1321 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1322 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1323 };
1324 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1325}
1326
1333inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1334
1341inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1342 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1343}
1344
1352inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1353 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1354 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1355 throw std::invalid_argument("Velocity is not Cartesian");
1356 }
1357 gm::Vector3 velocity;
1358 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1359 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1360 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1361 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1362 }
1363 return velocity;
1364}
1365
1374inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1375 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1376 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1377 throw std::invalid_argument("Velocity is not Cartesian");
1378 }
1379 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1380 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1381 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1382 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1383 : std::numeric_limits<double>::infinity();
1384 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1385}
1386
1393inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1394 return double(acceleration.value.value) / 10.0;
1395}
1396
1403inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1404 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1405}
1406
1414inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1415 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1416 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1417 throw std::invalid_argument("Acceleration is not Cartesian");
1418 }
1419 gm::Vector3 acceleration;
1420 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1421 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1422 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1423 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1424 }
1425 return acceleration;
1426}
1427
1436inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1437 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1438 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1439 throw std::invalid_argument("Acceleration is not Cartesian");
1440 }
1441 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1442 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1443 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1444 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1445 : std::numeric_limits<double>::infinity();
1446 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1447}
1448
1460inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1461 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1462 return object.object_dimension_x.value.value;
1463}
1464
1472inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1473 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1474 return object.object_dimension_x.confidence.value;
1475}
1476
1488inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1489 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1490 return object.object_dimension_y.value.value;
1491}
1492
1500inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1501 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1502 return object.object_dimension_y.confidence.value;
1503}
1504
1516inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1517 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1518 return object.object_dimension_z.value.value;
1519}
1520
1528inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1529 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1530 return object.object_dimension_z.confidence.value;
1531}
1532
1542inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1543 gm::Vector3 dimensions;
1544 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1545 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1546 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1547 return dimensions;
1548}
1549
1556inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1557 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1558 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1559 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1560 return {conf_x, conf_y, conf_z};
1561}
1562
1573inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1574 const PerceivedObject &object) {
1575 gm::PointStamped utm_position;
1576 gm::PointStamped reference_position = getUTMPosition(cpm);
1577 gm::Point relative_position = getPositionOfPerceivedObject(object);
1578
1579 utm_position.header.frame_id = reference_position.header.frame_id;
1580 utm_position.point.x = reference_position.point.x + relative_position.x;
1581 utm_position.point.y = reference_position.point.y + relative_position.y;
1582 utm_position.point.z = reference_position.point.z + relative_position.z;
1583
1584 return utm_position;
1585}
1586
1596inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1597 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1598}
1599
1600} // 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 909 of file cpm_ts_getters.h.

944 {
945
946#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
947
956inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
957
964inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
965 return cpm.payload.management_container.reference_time;
966}
967
974inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
975
982inline double getLatitude(const CollectivePerceptionMessage &cpm) {
983 return getLatitude(cpm.payload.management_container.reference_position.latitude);
984}
985
992inline double getLongitude(const CollectivePerceptionMessage &cpm) {
993 return getLongitude(cpm.payload.management_container.reference_position.longitude);
994}
995
1002inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1003 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1004}
1005
1017inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1018 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1019}
1020
1030inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1031 int zone;
1032 bool northp;
1033 return getUTMPosition(cpm, zone, northp);
1034}
1035
1045inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1046 std::vector<uint8_t> container_ids;
1047 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1048 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1049 }
1050 return container_ids;
1051}
1052
1061inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1062 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1063 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1064 return cpm.payload.cpm_containers.value.array[i];
1065 }
1066 }
1067 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1068}
1069
1078inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1079 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1080}
1081
1089inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1090 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1091 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1092 }
1093 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1094 return number;
1095}
1096
1103inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1104 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1105}
1106
1110
1119inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1120 if (i >= getNumberOfPerceivedObjects(container)) {
1121 throw std::invalid_argument("Index out of range");
1122 }
1123 return container.container_data_perceived_object_container.perceived_objects.array[i];
1124}
1125
1134inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1135
1142inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1143 return coordinate.value.value;
1144}
1145
1152inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1153 return coordinate.confidence.value;
1154}
1155
1162inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1163 gm::Point point;
1164 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1165 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1166 if (object.position.z_coordinate_is_present) {
1167 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1168 }
1169 return point;
1170}
1171
1179inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1180 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1181 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1182 double z_confidence = object.position.z_coordinate_is_present
1183 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1184 : std::numeric_limits<double>::infinity();
1185 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1186}
1187
1194inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1195
1202inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1203
1214inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1215 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1216 tf2::Quaternion q;
1217 double roll{0}, pitch{0}, yaw{0};
1218
1219 if (object.angles.x_angle_is_present) {
1220 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1221 M_PI;
1222 }
1223 if (object.angles.y_angle_is_present) {
1224 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1225 M_PI;
1226 }
1227 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1228 M_PI;
1229 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1230 q.setRPY(roll, pitch, yaw);
1231
1232 return tf2::toMsg(q);
1233}
1234
1241inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1242 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1243 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1244 double roll, pitch, yaw;
1245 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1246 return yaw;
1247}
1248
1256inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1257 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1258 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1259 yaw_confidence *= M_PI / 180.0; // convert to radians
1260 return yaw_confidence;
1261}
1262
1269inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1270 gm::Pose pose;
1271 pose.position = getPositionOfPerceivedObject(object);
1272 pose.orientation = getOrientationOfPerceivedObject(object);
1273 return pose;
1274}
1275
1283inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1284 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1285 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1286}
1287
1297inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1298 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1299 auto val = object.z_angular_velocity.confidence.value;
1300 static const std::map<uint8_t, double> confidence_map = {
1301 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1302 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1303 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1304 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1305 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1306 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1307 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1308 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1309 };
1310 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1311}
1312
1319inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1320
1327inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1328 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1329}
1330
1338inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1339 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1340 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1341 throw std::invalid_argument("Velocity is not Cartesian");
1342 }
1343 gm::Vector3 velocity;
1344 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1345 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1346 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1347 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1348 }
1349 return velocity;
1350}
1351
1360inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1361 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1362 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1363 throw std::invalid_argument("Velocity is not Cartesian");
1364 }
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}
1372
1379inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1380 return double(acceleration.value.value) / 10.0;
1381}
1382
1389inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1390 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1391}
1392
1400inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1401 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1402 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1403 throw std::invalid_argument("Acceleration is not Cartesian");
1404 }
1405 gm::Vector3 acceleration;
1406 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1407 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1408 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1409 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1410 }
1411 return acceleration;
1412}
1413
1422inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1423 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1424 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1425 throw std::invalid_argument("Acceleration is not Cartesian");
1426 }
1427 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1428 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1429 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1430 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1431 : std::numeric_limits<double>::infinity();
1432 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1433}
1434
1446inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1447 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1448 return object.object_dimension_x.value.value;
1449}
1450
1458inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1459 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1460 return object.object_dimension_x.confidence.value;
1461}
1462
1474inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1475 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1476 return object.object_dimension_y.value.value;
1477}
1478
1486inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1487 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1488 return object.object_dimension_y.confidence.value;
1489}
1490
1502inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1503 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1504 return object.object_dimension_z.value.value;
1505}
1506
1514inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1515 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1516 return object.object_dimension_z.confidence.value;
1517}
1518
1528inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1529 gm::Vector3 dimensions;
1530 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1531 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1532 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1533 return dimensions;
1534}
1535
1542inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1543 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1544 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1545 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1546 return {conf_x, conf_y, conf_z};
1547}
1548
1559inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1560 const PerceivedObject &object) {
1561 gm::PointStamped utm_position;
1562 gm::PointStamped reference_position = getUTMPosition(cpm);
1563 gm::Point relative_position = getPositionOfPerceivedObject(object);
1564
1565 utm_position.header.frame_id = reference_position.header.frame_id;
1566 utm_position.point.x = reference_position.point.x + relative_position.x;
1567 utm_position.point.y = reference_position.point.y + relative_position.y;
1568 utm_position.point.z = reference_position.point.z + relative_position.z;
1569
1570 return utm_position;
1571}
1572
1582inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1583 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1584}
1585
1586} // 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 132 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 143 of file cpm_ts_getters.h.

◆ 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 515 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 270 of file cpm_ts_getters.h.

271 {
272 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;

◆ 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 280 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 363 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 373 of file cpm_ts_getters.h.

375 {

◆ 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 250 of file cpm_ts_getters.h.

◆ 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 260 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 484 of file cpm_ts_getters.h.

◆ 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 470 of file cpm_ts_getters.h.

471 {
472 return double(acceleration.value.value) / 10.0;
473}
474

◆ 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 595 of file cpm_ts_getters.h.

◆ 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 500 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 459 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 163 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 224 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 650 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 308 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 292 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 560 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 543 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 345 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 355 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 152 of file cpm_ts_getters.h.

◆ 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 79 of file cpm_ts_getters.h.

◆ 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 87 of file cpm_ts_getters.h.

◆ 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 337 of file cpm_ts_getters.h.

◆ 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 411 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 398 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 103 of file cpm_ts_getters.h.

◆ 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 940 of file cpm_ts_getters.h.

975 {
976
977#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
978
987inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
988
995inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
996 return cpm.payload.management_container.reference_time;
997}
998
1005inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1006
1013inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1014 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1015}
1016
1023inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1024 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1025}
1026
1033inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1034 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1035}
1036
1048inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1049 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1050}
1051
1061inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1062 int zone;
1063 bool northp;
1064 return getUTMPosition(cpm, zone, northp);
1065}
1066
1076inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1077 std::vector<uint8_t> container_ids;
1078 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1079 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1080 }
1081 return container_ids;
1082}
1083
1092inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1093 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1094 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1095 return cpm.payload.cpm_containers.value.array[i];
1096 }
1097 }
1098 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1099}
1100
1109inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1110 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1111}
1112
1120inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1121 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1122 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1123 }
1124 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1125 return number;
1126}
1127
1134inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1135 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1136}
1137
1141
1150inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1151 if (i >= getNumberOfPerceivedObjects(container)) {
1152 throw std::invalid_argument("Index out of range");
1153 }
1154 return container.container_data_perceived_object_container.perceived_objects.array[i];
1155}
1156
1165inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1166
1173inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1174 return coordinate.value.value;
1175}
1176
1183inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1184 return coordinate.confidence.value;
1185}
1186
1193inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1194 gm::Point point;
1195 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1196 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1197 if (object.position.z_coordinate_is_present) {
1198 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1199 }
1200 return point;
1201}
1202
1210inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1211 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1212 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1213 double z_confidence = object.position.z_coordinate_is_present
1214 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1215 : std::numeric_limits<double>::infinity();
1216 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1217}
1218
1225inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1226
1233inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1234
1245inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1246 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1247 tf2::Quaternion q;
1248 double roll{0}, pitch{0}, yaw{0};
1249
1250 if (object.angles.x_angle_is_present) {
1251 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1252 M_PI;
1253 }
1254 if (object.angles.y_angle_is_present) {
1255 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1256 M_PI;
1257 }
1258 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1259 M_PI;
1260 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1261 q.setRPY(roll, pitch, yaw);
1262
1263 return tf2::toMsg(q);
1264}
1265
1272inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1273 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1274 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1275 double roll, pitch, yaw;
1276 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1277 return yaw;
1278}
1279
1287inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1288 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1289 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1290 yaw_confidence *= M_PI / 180.0; // convert to radians
1291 return yaw_confidence;
1292}
1293
1300inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1301 gm::Pose pose;
1302 pose.position = getPositionOfPerceivedObject(object);
1303 pose.orientation = getOrientationOfPerceivedObject(object);
1304 return pose;
1305}
1306
1314inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1315 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1316 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1317}
1318
1328inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1329 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1330 auto val = object.z_angular_velocity.confidence.value;
1331 static const std::map<uint8_t, double> confidence_map = {
1332 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1333 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1334 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1335 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1336 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1337 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1338 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1339 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1340 };
1341 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1342}
1343
1350inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1351
1358inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1359 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1360}
1361
1369inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1370 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1371 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1372 throw std::invalid_argument("Velocity is not Cartesian");
1373 }
1374 gm::Vector3 velocity;
1375 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1376 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1377 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1378 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1379 }
1380 return velocity;
1381}
1382
1391inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1392 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1393 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1394 throw std::invalid_argument("Velocity is not Cartesian");
1395 }
1396 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1397 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1398 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1399 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1400 : std::numeric_limits<double>::infinity();
1401 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1402}
1403
1410inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1411 return double(acceleration.value.value) / 10.0;
1412}
1413
1420inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1421 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1422}
1423
1431inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1432 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1433 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1434 throw std::invalid_argument("Acceleration is not Cartesian");
1435 }
1436 gm::Vector3 acceleration;
1437 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1438 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1439 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1440 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1441 }
1442 return acceleration;
1443}
1444
1453inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1454 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1455 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1456 throw std::invalid_argument("Acceleration is not Cartesian");
1457 }
1458 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1459 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1460 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1461 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1462 : std::numeric_limits<double>::infinity();
1463 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1464}
1465
1477inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1478 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1479 return object.object_dimension_x.value.value;
1480}
1481
1489inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1490 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1491 return object.object_dimension_x.confidence.value;
1492}
1493
1505inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1506 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1507 return object.object_dimension_y.value.value;
1508}
1509
1517inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1518 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1519 return object.object_dimension_y.confidence.value;
1520}
1521
1533inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1534 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1535 return object.object_dimension_z.value.value;
1536}
1537
1545inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1546 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1547 return object.object_dimension_z.confidence.value;
1548}
1549
1559inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1560 gm::Vector3 dimensions;
1561 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1562 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1563 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1564 return dimensions;
1565}
1566
1573inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1574 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1575 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1576 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1577 return {conf_x, conf_y, conf_z};
1578}
1579
1590inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1591 const PerceivedObject &object) {
1592 gm::PointStamped utm_position;
1593 gm::PointStamped reference_position = getUTMPosition(cpm);
1594 gm::Point relative_position = getPositionOfPerceivedObject(object);
1595
1596 utm_position.header.frame_id = reference_position.header.frame_id;
1597 utm_position.point.x = reference_position.point.x + relative_position.x;
1598 utm_position.point.y = reference_position.point.y + relative_position.y;
1599 utm_position.point.z = reference_position.point.z + relative_position.z;
1600
1601 return utm_position;
1602}
1603
1613inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1614 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1615}
1616
1617} // 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 700 of file cpm_ts_getters.h.

735{
736
737#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
738
747inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
748
755inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
756 return cpm.payload.management_container.reference_time;
757}
758
765inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
766
773inline double getLatitude(const CollectivePerceptionMessage &cpm) {
774 return getLatitude(cpm.payload.management_container.reference_position.latitude);
775}
776
783inline double getLongitude(const CollectivePerceptionMessage &cpm) {
784 return getLongitude(cpm.payload.management_container.reference_position.longitude);
785}
786
793inline double getAltitude(const CollectivePerceptionMessage &cpm) {
794 return getAltitude(cpm.payload.management_container.reference_position.altitude);
795}
796
808inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
809 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
810}
811
821inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
822 int zone;
823 bool northp;
824 return getUTMPosition(cpm, zone, northp);
825}
826
836inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
837 std::vector<uint8_t> container_ids;
838 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
839 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
840 }
841 return container_ids;
842}
843
852inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
853 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
854 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
855 return cpm.payload.cpm_containers.value.array[i];
856 }
857 }
858 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
859}
860
869inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
870 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
871}
872
880inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
881 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
882 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
883 }
884 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
885 return number;
886}
887
894inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
895 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
896}
897
901
910inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
911 if (i >= getNumberOfPerceivedObjects(container)) {
912 throw std::invalid_argument("Index out of range");
913 }
914 return container.container_data_perceived_object_container.perceived_objects.array[i];
915}
916
925inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
926
933inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
934 return coordinate.value.value;
935}
936
943inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
944 return coordinate.confidence.value;
945}
946
953inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
954 gm::Point point;
955 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
956 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
957 if (object.position.z_coordinate_is_present) {
958 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
959 }
960 return point;
961}
962
970inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
971 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
972 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
973 double z_confidence = object.position.z_coordinate_is_present
974 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
975 : std::numeric_limits<double>::infinity();
976 return std::make_tuple(x_confidence, y_confidence, z_confidence);
977}
978
985inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
986
993inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
994
1005inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1006 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1007 tf2::Quaternion q;
1008 double roll{0}, pitch{0}, yaw{0};
1009
1010 if (object.angles.x_angle_is_present) {
1011 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1012 M_PI;
1013 }
1014 if (object.angles.y_angle_is_present) {
1015 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1016 M_PI;
1017 }
1018 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1019 M_PI;
1020 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1021 q.setRPY(roll, pitch, yaw);
1022
1023 return tf2::toMsg(q);
1024}
1025
1032inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1033 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1034 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1035 double roll, pitch, yaw;
1036 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1037 return yaw;
1038}
1039
1047inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1048 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1049 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1050 yaw_confidence *= M_PI / 180.0; // convert to radians
1051 return yaw_confidence;
1052}
1053
1060inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1061 gm::Pose pose;
1062 pose.position = getPositionOfPerceivedObject(object);
1063 pose.orientation = getOrientationOfPerceivedObject(object);
1064 return pose;
1065}
1066
1074inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1075 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1076 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1077}
1078
1088inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1089 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1090 auto val = object.z_angular_velocity.confidence.value;
1091 static const std::map<uint8_t, double> confidence_map = {
1092 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1093 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1094 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1095 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1096 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1097 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1098 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1099 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1100 };
1101 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1102}
1103
1110inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1111
1118inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1119 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1120}
1121
1129inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1130 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1131 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1132 throw std::invalid_argument("Velocity is not Cartesian");
1133 }
1134 gm::Vector3 velocity;
1135 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1136 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1137 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1138 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1139 }
1140 return velocity;
1141}
1142
1151inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1152 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1153 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1154 throw std::invalid_argument("Velocity is not Cartesian");
1155 }
1156 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1157 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1158 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1159 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1160 : std::numeric_limits<double>::infinity();
1161 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1162}
1163
1170inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1171 return double(acceleration.value.value) / 10.0;
1172}
1173
1180inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1181 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1182}
1183
1191inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1192 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1193 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1194 throw std::invalid_argument("Acceleration is not Cartesian");
1195 }
1196 gm::Vector3 acceleration;
1197 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1198 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1199 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1200 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1201 }
1202 return acceleration;
1203}
1204
1213inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1214 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1215 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1216 throw std::invalid_argument("Acceleration is not Cartesian");
1217 }
1218 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1219 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1220 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1221 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1222 : std::numeric_limits<double>::infinity();
1223 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1224}
1225
1237inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1238 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1239 return object.object_dimension_x.value.value;
1240}
1241
1249inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1250 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1251 return object.object_dimension_x.confidence.value;
1252}
1253
1265inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1266 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1267 return object.object_dimension_y.value.value;
1268}
1269
1277inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1278 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1279 return object.object_dimension_y.confidence.value;
1280}
1281
1293inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1294 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1295 return object.object_dimension_z.value.value;
1296}
1297
1305inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1306 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1307 return object.object_dimension_z.confidence.value;
1308}
1309
1319inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1320 gm::Vector3 dimensions;
1321 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1322 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1323 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1324 return dimensions;
1325}
1326
1333inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1334 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1335 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1336 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1337 return {conf_x, conf_y, conf_z};
1338}
1339
1350inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1351 const PerceivedObject &object) {
1352 gm::PointStamped utm_position;
1353 gm::PointStamped reference_position = getUTMPosition(cpm);
1354 gm::Point relative_position = getPositionOfPerceivedObject(object);
1355
1356 utm_position.header.frame_id = reference_position.header.frame_id;
1357 utm_position.point.x = reference_position.point.x + relative_position.x;
1358 utm_position.point.y = reference_position.point.y + relative_position.y;
1359 utm_position.point.z = reference_position.point.z + relative_position.z;
1360
1361 return utm_position;
1362}
1363
1373inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1374 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1375}
1376
1377} // 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 708 of file cpm_ts_getters.h.

743 {
744
745#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
746
755inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
756
763inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
764 return cpm.payload.management_container.reference_time;
765}
766
773inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
774
781inline double getLatitude(const CollectivePerceptionMessage &cpm) {
782 return getLatitude(cpm.payload.management_container.reference_position.latitude);
783}
784
791inline double getLongitude(const CollectivePerceptionMessage &cpm) {
792 return getLongitude(cpm.payload.management_container.reference_position.longitude);
793}
794
801inline double getAltitude(const CollectivePerceptionMessage &cpm) {
802 return getAltitude(cpm.payload.management_container.reference_position.altitude);
803}
804
816inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
817 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
818}
819
829inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
830 int zone;
831 bool northp;
832 return getUTMPosition(cpm, zone, northp);
833}
834
844inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
845 std::vector<uint8_t> container_ids;
846 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
847 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
848 }
849 return container_ids;
850}
851
860inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
861 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
862 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
863 return cpm.payload.cpm_containers.value.array[i];
864 }
865 }
866 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
867}
868
877inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
878 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
879}
880
888inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
889 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
890 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
891 }
892 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
893 return number;
894}
895
902inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
903 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
904}
905
909
918inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
919 if (i >= getNumberOfPerceivedObjects(container)) {
920 throw std::invalid_argument("Index out of range");
921 }
922 return container.container_data_perceived_object_container.perceived_objects.array[i];
923}
924
933inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
934
941inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
942 return coordinate.value.value;
943}
944
951inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
952 return coordinate.confidence.value;
953}
954
961inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
962 gm::Point point;
963 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
964 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
965 if (object.position.z_coordinate_is_present) {
966 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
967 }
968 return point;
969}
970
978inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
979 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
980 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
981 double z_confidence = object.position.z_coordinate_is_present
982 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
983 : std::numeric_limits<double>::infinity();
984 return std::make_tuple(x_confidence, y_confidence, z_confidence);
985}
986
993inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
994
1001inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1002
1013inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1014 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1015 tf2::Quaternion q;
1016 double roll{0}, pitch{0}, yaw{0};
1017
1018 if (object.angles.x_angle_is_present) {
1019 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1020 M_PI;
1021 }
1022 if (object.angles.y_angle_is_present) {
1023 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1024 M_PI;
1025 }
1026 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1027 M_PI;
1028 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1029 q.setRPY(roll, pitch, yaw);
1030
1031 return tf2::toMsg(q);
1032}
1033
1040inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1041 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1042 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1043 double roll, pitch, yaw;
1044 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1045 return yaw;
1046}
1047
1055inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1056 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1057 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1058 yaw_confidence *= M_PI / 180.0; // convert to radians
1059 return yaw_confidence;
1060}
1061
1068inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1069 gm::Pose pose;
1070 pose.position = getPositionOfPerceivedObject(object);
1071 pose.orientation = getOrientationOfPerceivedObject(object);
1072 return pose;
1073}
1074
1082inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1083 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1084 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1085}
1086
1096inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1097 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1098 auto val = object.z_angular_velocity.confidence.value;
1099 static const std::map<uint8_t, double> confidence_map = {
1100 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1101 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1102 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1103 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1104 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1105 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1106 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1107 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1108 };
1109 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1110}
1111
1118inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1119
1126inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1127 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1128}
1129
1137inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1138 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1139 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1140 throw std::invalid_argument("Velocity is not Cartesian");
1141 }
1142 gm::Vector3 velocity;
1143 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1144 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1145 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1146 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1147 }
1148 return velocity;
1149}
1150
1159inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1160 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1161 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1162 throw std::invalid_argument("Velocity is not Cartesian");
1163 }
1164 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1165 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1166 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1167 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1168 : std::numeric_limits<double>::infinity();
1169 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1170}
1171
1178inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1179 return double(acceleration.value.value) / 10.0;
1180}
1181
1188inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1189 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1190}
1191
1199inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1200 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1201 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1202 throw std::invalid_argument("Acceleration is not Cartesian");
1203 }
1204 gm::Vector3 acceleration;
1205 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1206 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1207 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1208 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1209 }
1210 return acceleration;
1211}
1212
1221inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1222 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1223 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1224 throw std::invalid_argument("Acceleration is not Cartesian");
1225 }
1226 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1227 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1228 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1229 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1230 : std::numeric_limits<double>::infinity();
1231 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1232}
1233
1245inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1246 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1247 return object.object_dimension_x.value.value;
1248}
1249
1257inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1258 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1259 return object.object_dimension_x.confidence.value;
1260}
1261
1273inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1274 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1275 return object.object_dimension_y.value.value;
1276}
1277
1285inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1286 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1287 return object.object_dimension_y.confidence.value;
1288}
1289
1301inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1302 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1303 return object.object_dimension_z.value.value;
1304}
1305
1313inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1314 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1315 return object.object_dimension_z.confidence.value;
1316}
1317
1327inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1328 gm::Vector3 dimensions;
1329 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1330 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1331 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1332 return dimensions;
1333}
1334
1341inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1342 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1343 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1344 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1345 return {conf_x, conf_y, conf_z};
1346}
1347
1358inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1359 const PerceivedObject &object) {
1360 gm::PointStamped utm_position;
1361 gm::PointStamped reference_position = getUTMPosition(cpm);
1362 gm::Point relative_position = getPositionOfPerceivedObject(object);
1363
1364 utm_position.header.frame_id = reference_position.header.frame_id;
1365 utm_position.point.x = reference_position.point.x + relative_position.x;
1366 utm_position.point.y = reference_position.point.y + relative_position.y;
1367 utm_position.point.z = reference_position.point.z + relative_position.z;
1368
1369 return utm_position;
1370}
1371
1381inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1382 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1383}
1384
1385} // 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 237 of file cpm_ts_getters.h.

◆ 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 321 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 963 of file cpm_ts_getters.h.

998 {
999
1000#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
1001
1010inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
1011
1018inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
1019 return cpm.payload.management_container.reference_time;
1020}
1021
1028inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
1029
1036inline double getLatitude(const CollectivePerceptionMessage &cpm) {
1037 return getLatitude(cpm.payload.management_container.reference_position.latitude);
1038}
1039
1046inline double getLongitude(const CollectivePerceptionMessage &cpm) {
1047 return getLongitude(cpm.payload.management_container.reference_position.longitude);
1048}
1049
1056inline double getAltitude(const CollectivePerceptionMessage &cpm) {
1057 return getAltitude(cpm.payload.management_container.reference_position.altitude);
1058}
1059
1071inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1072 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1073}
1074
1084inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1085 int zone;
1086 bool northp;
1087 return getUTMPosition(cpm, zone, northp);
1088}
1089
1099inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1100 std::vector<uint8_t> container_ids;
1101 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1102 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1103 }
1104 return container_ids;
1105}
1106
1115inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1116 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1117 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1118 return cpm.payload.cpm_containers.value.array[i];
1119 }
1120 }
1121 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1122}
1123
1132inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1133 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1134}
1135
1143inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1144 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1145 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1146 }
1147 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1148 return number;
1149}
1150
1157inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1158 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1159}
1160
1164
1173inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1174 if (i >= getNumberOfPerceivedObjects(container)) {
1175 throw std::invalid_argument("Index out of range");
1176 }
1177 return container.container_data_perceived_object_container.perceived_objects.array[i];
1178}
1179
1188inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1189
1196inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1197 return coordinate.value.value;
1198}
1199
1206inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1207 return coordinate.confidence.value;
1208}
1209
1216inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1217 gm::Point point;
1218 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1219 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1220 if (object.position.z_coordinate_is_present) {
1221 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1222 }
1223 return point;
1224}
1225
1233inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1234 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1235 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1236 double z_confidence = object.position.z_coordinate_is_present
1237 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1238 : std::numeric_limits<double>::infinity();
1239 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1240}
1241
1248inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1249
1256inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1257
1268inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1269 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1270 tf2::Quaternion q;
1271 double roll{0}, pitch{0}, yaw{0};
1272
1273 if (object.angles.x_angle_is_present) {
1274 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1275 M_PI;
1276 }
1277 if (object.angles.y_angle_is_present) {
1278 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1279 M_PI;
1280 }
1281 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1282 M_PI;
1283 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1284 q.setRPY(roll, pitch, yaw);
1285
1286 return tf2::toMsg(q);
1287}
1288
1295inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1296 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1297 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1298 double roll, pitch, yaw;
1299 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1300 return yaw;
1301}
1302
1310inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1311 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1312 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1313 yaw_confidence *= M_PI / 180.0; // convert to radians
1314 return yaw_confidence;
1315}
1316
1323inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1324 gm::Pose pose;
1325 pose.position = getPositionOfPerceivedObject(object);
1326 pose.orientation = getOrientationOfPerceivedObject(object);
1327 return pose;
1328}
1329
1337inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1338 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1339 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1340}
1341
1351inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1352 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1353 auto val = object.z_angular_velocity.confidence.value;
1354 static const std::map<uint8_t, double> confidence_map = {
1355 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1356 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1357 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1358 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1359 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1360 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1361 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1362 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1363 };
1364 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1365}
1366
1373inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1374
1381inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1382 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1383}
1384
1392inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1393 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1394 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1395 throw std::invalid_argument("Velocity is not Cartesian");
1396 }
1397 gm::Vector3 velocity;
1398 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1399 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1400 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1401 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1402 }
1403 return velocity;
1404}
1405
1414inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1415 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1416 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1417 throw std::invalid_argument("Velocity is not Cartesian");
1418 }
1419 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1420 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1421 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1422 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1423 : std::numeric_limits<double>::infinity();
1424 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1425}
1426
1433inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1434 return double(acceleration.value.value) / 10.0;
1435}
1436
1443inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1444 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1445}
1446
1454inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1455 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1456 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1457 throw std::invalid_argument("Acceleration is not Cartesian");
1458 }
1459 gm::Vector3 acceleration;
1460 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1461 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1462 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1463 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1464 }
1465 return acceleration;
1466}
1467
1476inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1477 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1478 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1479 throw std::invalid_argument("Acceleration is not Cartesian");
1480 }
1481 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1482 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1483 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1484 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1485 : std::numeric_limits<double>::infinity();
1486 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1487}
1488
1500inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1501 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1502 return object.object_dimension_x.value.value;
1503}
1504
1512inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1513 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1514 return object.object_dimension_x.confidence.value;
1515}
1516
1528inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1529 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1530 return object.object_dimension_y.value.value;
1531}
1532
1540inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1541 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1542 return object.object_dimension_y.confidence.value;
1543}
1544
1556inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1557 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1558 return object.object_dimension_z.value.value;
1559}
1560
1568inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1569 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1570 return object.object_dimension_z.confidence.value;
1571}
1572
1582inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1583 gm::Vector3 dimensions;
1584 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1585 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1586 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1587 return dimensions;
1588}
1589
1596inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1597 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1598 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1599 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1600 return {conf_x, conf_y, conf_z};
1601}
1602
1613inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1614 const PerceivedObject &object) {
1615 gm::PointStamped utm_position;
1616 gm::PointStamped reference_position = getUTMPosition(cpm);
1617 gm::Point relative_position = getPositionOfPerceivedObject(object);
1618
1619 utm_position.header.frame_id = reference_position.header.frame_id;
1620 utm_position.point.x = reference_position.point.x + relative_position.x;
1621 utm_position.point.y = reference_position.point.y + relative_position.y;
1622 utm_position.point.z = reference_position.point.z + relative_position.z;
1623
1624 return utm_position;
1625}
1626
1636inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1637 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1638}
1639
1640} // 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 839 of file cpm_ts_getters.h.

874 {
875
876#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
877
886inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
887
894inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
895 return cpm.payload.management_container.reference_time;
896}
897
904inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
905
912inline double getLatitude(const CollectivePerceptionMessage &cpm) {
913 return getLatitude(cpm.payload.management_container.reference_position.latitude);
914}
915
922inline double getLongitude(const CollectivePerceptionMessage &cpm) {
923 return getLongitude(cpm.payload.management_container.reference_position.longitude);
924}
925
932inline double getAltitude(const CollectivePerceptionMessage &cpm) {
933 return getAltitude(cpm.payload.management_container.reference_position.altitude);
934}
935
947inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
948 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
949}
950
960inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
961 int zone;
962 bool northp;
963 return getUTMPosition(cpm, zone, northp);
964}
965
975inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
976 std::vector<uint8_t> container_ids;
977 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
978 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
979 }
980 return container_ids;
981}
982
991inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
992 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
993 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
994 return cpm.payload.cpm_containers.value.array[i];
995 }
996 }
997 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
998}
999
1008inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1009 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1010}
1011
1019inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1020 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1021 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1022 }
1023 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1024 return number;
1025}
1026
1033inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1034 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1035}
1036
1040
1049inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1050 if (i >= getNumberOfPerceivedObjects(container)) {
1051 throw std::invalid_argument("Index out of range");
1052 }
1053 return container.container_data_perceived_object_container.perceived_objects.array[i];
1054}
1055
1064inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1065
1072inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1073 return coordinate.value.value;
1074}
1075
1082inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1083 return coordinate.confidence.value;
1084}
1085
1092inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1093 gm::Point point;
1094 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1095 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1096 if (object.position.z_coordinate_is_present) {
1097 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1098 }
1099 return point;
1100}
1101
1109inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1110 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1111 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1112 double z_confidence = object.position.z_coordinate_is_present
1113 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1114 : std::numeric_limits<double>::infinity();
1115 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1116}
1117
1124inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1125
1132inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1133
1144inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1145 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1146 tf2::Quaternion q;
1147 double roll{0}, pitch{0}, yaw{0};
1148
1149 if (object.angles.x_angle_is_present) {
1150 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1151 M_PI;
1152 }
1153 if (object.angles.y_angle_is_present) {
1154 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1155 M_PI;
1156 }
1157 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1158 M_PI;
1159 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1160 q.setRPY(roll, pitch, yaw);
1161
1162 return tf2::toMsg(q);
1163}
1164
1171inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1172 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1173 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1174 double roll, pitch, yaw;
1175 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1176 return yaw;
1177}
1178
1186inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1187 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1188 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1189 yaw_confidence *= M_PI / 180.0; // convert to radians
1190 return yaw_confidence;
1191}
1192
1199inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1200 gm::Pose pose;
1201 pose.position = getPositionOfPerceivedObject(object);
1202 pose.orientation = getOrientationOfPerceivedObject(object);
1203 return pose;
1204}
1205
1213inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1214 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1215 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1216}
1217
1227inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1228 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1229 auto val = object.z_angular_velocity.confidence.value;
1230 static const std::map<uint8_t, double> confidence_map = {
1231 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1232 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1233 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1234 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1235 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1236 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1237 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1238 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1239 };
1240 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1241}
1242
1249inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1250
1257inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1258 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1259}
1260
1268inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1269 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1270 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1271 throw std::invalid_argument("Velocity is not Cartesian");
1272 }
1273 gm::Vector3 velocity;
1274 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1275 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1276 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1277 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1278 }
1279 return velocity;
1280}
1281
1290inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1291 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1292 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1293 throw std::invalid_argument("Velocity is not Cartesian");
1294 }
1295 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1296 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1297 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1298 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1299 : std::numeric_limits<double>::infinity();
1300 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1301}
1302
1309inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1310 return double(acceleration.value.value) / 10.0;
1311}
1312
1319inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1320 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1321}
1322
1330inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1331 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1332 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1333 throw std::invalid_argument("Acceleration is not Cartesian");
1334 }
1335 gm::Vector3 acceleration;
1336 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1337 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1338 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1339 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1340 }
1341 return acceleration;
1342}
1343
1352inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1353 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1354 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1355 throw std::invalid_argument("Acceleration is not Cartesian");
1356 }
1357 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1358 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1359 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1360 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1361 : std::numeric_limits<double>::infinity();
1362 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1363}
1364
1376inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1377 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1378 return object.object_dimension_x.value.value;
1379}
1380
1388inline uint8_t getXDimensionConfidenceOfPerceivedObject(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.confidence.value;
1391}
1392
1404inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1405 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1406 return object.object_dimension_y.value.value;
1407}
1408
1416inline uint8_t getYDimensionConfidenceOfPerceivedObject(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.confidence.value;
1419}
1420
1432inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1433 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1434 return object.object_dimension_z.value.value;
1435}
1436
1444inline uint8_t getZDimensionConfidenceOfPerceivedObject(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.confidence.value;
1447}
1448
1458inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1459 gm::Vector3 dimensions;
1460 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1461 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1462 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1463 return dimensions;
1464}
1465
1472inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1473 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1474 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1475 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1476 return {conf_x, conf_y, conf_z};
1477}
1478
1489inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1490 const PerceivedObject &object) {
1491 gm::PointStamped utm_position;
1492 gm::PointStamped reference_position = getUTMPosition(cpm);
1493 gm::Point relative_position = getPositionOfPerceivedObject(object);
1494
1495 utm_position.header.frame_id = reference_position.header.frame_id;
1496 utm_position.point.x = reference_position.point.x + relative_position.x;
1497 utm_position.point.y = reference_position.point.y + relative_position.y;
1498 utm_position.point.z = reference_position.point.z + relative_position.z;
1499
1500 return utm_position;
1501}
1502
1512inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1513 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1514}
1515
1516} // 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 827 of file cpm_ts_getters.h.

862 {
863
864#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
865
874inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
875
882inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
883 return cpm.payload.management_container.reference_time;
884}
885
892inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
893
900inline double getLatitude(const CollectivePerceptionMessage &cpm) {
901 return getLatitude(cpm.payload.management_container.reference_position.latitude);
902}
903
910inline double getLongitude(const CollectivePerceptionMessage &cpm) {
911 return getLongitude(cpm.payload.management_container.reference_position.longitude);
912}
913
920inline double getAltitude(const CollectivePerceptionMessage &cpm) {
921 return getAltitude(cpm.payload.management_container.reference_position.altitude);
922}
923
935inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
936 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
937}
938
948inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
949 int zone;
950 bool northp;
951 return getUTMPosition(cpm, zone, northp);
952}
953
963inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
964 std::vector<uint8_t> container_ids;
965 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
966 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
967 }
968 return container_ids;
969}
970
979inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
980 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
981 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
982 return cpm.payload.cpm_containers.value.array[i];
983 }
984 }
985 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
986}
987
996inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
997 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
998}
999
1007inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1008 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1009 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1010 }
1011 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1012 return number;
1013}
1014
1021inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1022 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1023}
1024
1028
1037inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1038 if (i >= getNumberOfPerceivedObjects(container)) {
1039 throw std::invalid_argument("Index out of range");
1040 }
1041 return container.container_data_perceived_object_container.perceived_objects.array[i];
1042}
1043
1052inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1053
1060inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1061 return coordinate.value.value;
1062}
1063
1070inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1071 return coordinate.confidence.value;
1072}
1073
1080inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1081 gm::Point point;
1082 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1083 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1084 if (object.position.z_coordinate_is_present) {
1085 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1086 }
1087 return point;
1088}
1089
1097inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1098 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1099 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1100 double z_confidence = object.position.z_coordinate_is_present
1101 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1102 : std::numeric_limits<double>::infinity();
1103 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1104}
1105
1112inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1113
1120inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1121
1132inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1133 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1134 tf2::Quaternion q;
1135 double roll{0}, pitch{0}, yaw{0};
1136
1137 if (object.angles.x_angle_is_present) {
1138 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1139 M_PI;
1140 }
1141 if (object.angles.y_angle_is_present) {
1142 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1143 M_PI;
1144 }
1145 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1146 M_PI;
1147 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1148 q.setRPY(roll, pitch, yaw);
1149
1150 return tf2::toMsg(q);
1151}
1152
1159inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1160 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1161 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1162 double roll, pitch, yaw;
1163 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1164 return yaw;
1165}
1166
1174inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1175 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1176 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1177 yaw_confidence *= M_PI / 180.0; // convert to radians
1178 return yaw_confidence;
1179}
1180
1187inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1188 gm::Pose pose;
1189 pose.position = getPositionOfPerceivedObject(object);
1190 pose.orientation = getOrientationOfPerceivedObject(object);
1191 return pose;
1192}
1193
1201inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1202 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1203 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1204}
1205
1215inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1216 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1217 auto val = object.z_angular_velocity.confidence.value;
1218 static const std::map<uint8_t, double> confidence_map = {
1219 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1220 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1221 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1222 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1223 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1224 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1225 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1226 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1227 };
1228 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1229}
1230
1237inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1238
1245inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1246 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1247}
1248
1256inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1257 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1258 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1259 throw std::invalid_argument("Velocity is not Cartesian");
1260 }
1261 gm::Vector3 velocity;
1262 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1263 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1264 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1265 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1266 }
1267 return velocity;
1268}
1269
1278inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1279 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1280 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1281 throw std::invalid_argument("Velocity is not Cartesian");
1282 }
1283 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1284 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1285 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1286 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1287 : std::numeric_limits<double>::infinity();
1288 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1289}
1290
1297inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1298 return double(acceleration.value.value) / 10.0;
1299}
1300
1307inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1308 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1309}
1310
1318inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1319 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1320 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1321 throw std::invalid_argument("Acceleration is not Cartesian");
1322 }
1323 gm::Vector3 acceleration;
1324 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1325 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1326 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1327 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1328 }
1329 return acceleration;
1330}
1331
1340inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1341 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1342 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1343 throw std::invalid_argument("Acceleration is not Cartesian");
1344 }
1345 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1346 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1347 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1348 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1349 : std::numeric_limits<double>::infinity();
1350 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1351}
1352
1364inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1365 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1366 return object.object_dimension_x.value.value;
1367}
1368
1376inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1377 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1378 return object.object_dimension_x.confidence.value;
1379}
1380
1392inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1393 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1394 return object.object_dimension_y.value.value;
1395}
1396
1404inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1405 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1406 return object.object_dimension_y.confidence.value;
1407}
1408
1420inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1421 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1422 return object.object_dimension_z.value.value;
1423}
1424
1432inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1433 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1434 return object.object_dimension_z.confidence.value;
1435}
1436
1446inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1447 gm::Vector3 dimensions;
1448 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1449 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1450 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1451 return dimensions;
1452}
1453
1460inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1461 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1462 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1463 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1464 return {conf_x, conf_y, conf_z};
1465}
1466
1477inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1478 const PerceivedObject &object) {
1479 gm::PointStamped utm_position;
1480 gm::PointStamped reference_position = getUTMPosition(cpm);
1481 gm::Point relative_position = getPositionOfPerceivedObject(object);
1482
1483 utm_position.header.frame_id = reference_position.header.frame_id;
1484 utm_position.point.x = reference_position.point.x + relative_position.x;
1485 utm_position.point.y = reference_position.point.y + relative_position.y;
1486 utm_position.point.z = reference_position.point.z + relative_position.z;
1487
1488 return utm_position;
1489}
1490
1500inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1501 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1502}
1503
1504} // 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 637 of file cpm_ts_getters.h.

638 {conf_x, conf_y, conf_z};
639}
640

◆ 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 622 of file cpm_ts_getters.h.

◆ 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 678 of file cpm_ts_getters.h.

◆ 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 664 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 867 of file cpm_ts_getters.h.

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) { return object.object_id.value; }
1093
1100inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1101 return coordinate.value.value;
1102}
1103
1110inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1111 return coordinate.confidence.value;
1112}
1113
1120inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1121 gm::Point point;
1122 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1123 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1124 if (object.position.z_coordinate_is_present) {
1125 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1126 }
1127 return point;
1128}
1129
1137inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1138 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1139 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1140 double z_confidence = object.position.z_coordinate_is_present
1141 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1142 : std::numeric_limits<double>::infinity();
1143 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1144}
1145
1152inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1153
1160inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1161
1172inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1173 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1174 tf2::Quaternion q;
1175 double roll{0}, pitch{0}, yaw{0};
1176
1177 if (object.angles.x_angle_is_present) {
1178 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1179 M_PI;
1180 }
1181 if (object.angles.y_angle_is_present) {
1182 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1183 M_PI;
1184 }
1185 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1186 M_PI;
1187 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1188 q.setRPY(roll, pitch, yaw);
1189
1190 return tf2::toMsg(q);
1191}
1192
1199inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1200 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1201 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1202 double roll, pitch, yaw;
1203 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1204 return yaw;
1205}
1206
1214inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1215 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1216 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1217 yaw_confidence *= M_PI / 180.0; // convert to radians
1218 return yaw_confidence;
1219}
1220
1227inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1228 gm::Pose pose;
1229 pose.position = getPositionOfPerceivedObject(object);
1230 pose.orientation = getOrientationOfPerceivedObject(object);
1231 return pose;
1232}
1233
1241inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1242 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1243 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1244}
1245
1255inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1256 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1257 auto val = object.z_angular_velocity.confidence.value;
1258 static const std::map<uint8_t, double> confidence_map = {
1259 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1260 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1261 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1262 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1263 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1264 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1265 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1266 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1267 };
1268 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1269}
1270
1277inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1278
1285inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1286 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1287}
1288
1296inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1297 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1298 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1299 throw std::invalid_argument("Velocity is not Cartesian");
1300 }
1301 gm::Vector3 velocity;
1302 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1303 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1304 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1305 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1306 }
1307 return velocity;
1308}
1309
1318inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1319 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1320 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1321 throw std::invalid_argument("Velocity is not Cartesian");
1322 }
1323 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1324 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1325 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1326 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1327 : std::numeric_limits<double>::infinity();
1328 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1329}
1330
1337inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1338 return double(acceleration.value.value) / 10.0;
1339}
1340
1347inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1348 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1349}
1350
1358inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1359 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1360 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1361 throw std::invalid_argument("Acceleration is not Cartesian");
1362 }
1363 gm::Vector3 acceleration;
1364 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1365 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1366 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1367 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
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 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1383 throw std::invalid_argument("Acceleration is not Cartesian");
1384 }
1385 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1386 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1387 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1388 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1389 : std::numeric_limits<double>::infinity();
1390 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1391}
1392
1404inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1405 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1406 return object.object_dimension_x.value.value;
1407}
1408
1416inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1417 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1418 return object.object_dimension_x.confidence.value;
1419}
1420
1432inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1433 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1434 return object.object_dimension_y.value.value;
1435}
1436
1444inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1445 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1446 return object.object_dimension_y.confidence.value;
1447}
1448
1460inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1461 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1462 return object.object_dimension_z.value.value;
1463}
1464
1472inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1473 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1474 return object.object_dimension_z.confidence.value;
1475}
1476
1486inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1487 gm::Vector3 dimensions;
1488 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1489 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1490 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1491 return dimensions;
1492}
1493
1500inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1501 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1502 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1503 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1504 return {conf_x, conf_y, conf_z};
1505}
1506
1517inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1518 const PerceivedObject &object) {
1519 gm::PointStamped utm_position;
1520 gm::PointStamped reference_position = getUTMPosition(cpm);
1521 gm::Point relative_position = getPositionOfPerceivedObject(object);
1522
1523 utm_position.header.frame_id = reference_position.header.frame_id;
1524 utm_position.point.x = reference_position.point.x + relative_position.x;
1525 utm_position.point.y = reference_position.point.y + relative_position.y;
1526 utm_position.point.z = reference_position.point.z + relative_position.z;
1527
1528 return utm_position;
1529}
1530
1540inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1541 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1542}
1543
1544} // 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 855 of file cpm_ts_getters.h.

890 {
891
892#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
893
902inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
903
910inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
911 return cpm.payload.management_container.reference_time;
912}
913
920inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
921
928inline double getLatitude(const CollectivePerceptionMessage &cpm) {
929 return getLatitude(cpm.payload.management_container.reference_position.latitude);
930}
931
938inline double getLongitude(const CollectivePerceptionMessage &cpm) {
939 return getLongitude(cpm.payload.management_container.reference_position.longitude);
940}
941
948inline double getAltitude(const CollectivePerceptionMessage &cpm) {
949 return getAltitude(cpm.payload.management_container.reference_position.altitude);
950}
951
963inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
964 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
965}
966
976inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
977 int zone;
978 bool northp;
979 return getUTMPosition(cpm, zone, northp);
980}
981
991inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
992 std::vector<uint8_t> container_ids;
993 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
994 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
995 }
996 return container_ids;
997}
998
1007inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1008 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1009 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1010 return cpm.payload.cpm_containers.value.array[i];
1011 }
1012 }
1013 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1014}
1015
1024inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1025 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1026}
1027
1035inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1036 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1037 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1038 }
1039 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1040 return number;
1041}
1042
1049inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1050 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1051}
1052
1056
1065inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1066 if (i >= getNumberOfPerceivedObjects(container)) {
1067 throw std::invalid_argument("Index out of range");
1068 }
1069 return container.container_data_perceived_object_container.perceived_objects.array[i];
1070}
1071
1080inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1081
1088inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1089 return coordinate.value.value;
1090}
1091
1098inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1099 return coordinate.confidence.value;
1100}
1101
1108inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1109 gm::Point point;
1110 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1111 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1112 if (object.position.z_coordinate_is_present) {
1113 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1114 }
1115 return point;
1116}
1117
1125inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1126 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1127 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1128 double z_confidence = object.position.z_coordinate_is_present
1129 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1130 : std::numeric_limits<double>::infinity();
1131 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1132}
1133
1140inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1141
1148inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1149
1160inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1161 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1162 tf2::Quaternion q;
1163 double roll{0}, pitch{0}, yaw{0};
1164
1165 if (object.angles.x_angle_is_present) {
1166 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1167 M_PI;
1168 }
1169 if (object.angles.y_angle_is_present) {
1170 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1171 M_PI;
1172 }
1173 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1174 M_PI;
1175 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1176 q.setRPY(roll, pitch, yaw);
1177
1178 return tf2::toMsg(q);
1179}
1180
1187inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1188 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1189 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1190 double roll, pitch, yaw;
1191 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1192 return yaw;
1193}
1194
1202inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1203 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1204 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1205 yaw_confidence *= M_PI / 180.0; // convert to radians
1206 return yaw_confidence;
1207}
1208
1215inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1216 gm::Pose pose;
1217 pose.position = getPositionOfPerceivedObject(object);
1218 pose.orientation = getOrientationOfPerceivedObject(object);
1219 return pose;
1220}
1221
1229inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1230 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1231 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1232}
1233
1243inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1244 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1245 auto val = object.z_angular_velocity.confidence.value;
1246 static const std::map<uint8_t, double> confidence_map = {
1247 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1248 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1249 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1250 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1251 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1252 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1253 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1254 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1255 };
1256 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1257}
1258
1265inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1266
1273inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1274 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1275}
1276
1284inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1285 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1286 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1287 throw std::invalid_argument("Velocity is not Cartesian");
1288 }
1289 gm::Vector3 velocity;
1290 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1291 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1292 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1293 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1294 }
1295 return velocity;
1296}
1297
1306inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1307 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1308 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1309 throw std::invalid_argument("Velocity is not Cartesian");
1310 }
1311 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1312 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1313 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1314 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1315 : std::numeric_limits<double>::infinity();
1316 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1317}
1318
1325inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1326 return double(acceleration.value.value) / 10.0;
1327}
1328
1335inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1336 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1337}
1338
1346inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1347 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1348 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1349 throw std::invalid_argument("Acceleration is not Cartesian");
1350 }
1351 gm::Vector3 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 return acceleration;
1358}
1359
1368inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1369 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1370 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1371 throw std::invalid_argument("Acceleration is not Cartesian");
1372 }
1373 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1374 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1375 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1376 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1377 : std::numeric_limits<double>::infinity();
1378 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1379}
1380
1392inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1393 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1394 return object.object_dimension_x.value.value;
1395}
1396
1404inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1405 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1406 return object.object_dimension_x.confidence.value;
1407}
1408
1420inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1421 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1422 return object.object_dimension_y.value.value;
1423}
1424
1432inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1433 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1434 return object.object_dimension_y.confidence.value;
1435}
1436
1448inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1449 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1450 return object.object_dimension_z.value.value;
1451}
1452
1460inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1461 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1462 return object.object_dimension_z.confidence.value;
1463}
1464
1474inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1475 gm::Vector3 dimensions;
1476 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1477 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1478 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1479 return dimensions;
1480}
1481
1488inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1489 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1490 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1491 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1492 return {conf_x, conf_y, conf_z};
1493}
1494
1505inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1506 const PerceivedObject &object) {
1507 gm::PointStamped utm_position;
1508 gm::PointStamped reference_position = getUTMPosition(cpm);
1509 gm::Point relative_position = getPositionOfPerceivedObject(object);
1510
1511 utm_position.header.frame_id = reference_position.header.frame_id;
1512 utm_position.point.x = reference_position.point.x + relative_position.x;
1513 utm_position.point.y = reference_position.point.y + relative_position.y;
1514 utm_position.point.z = reference_position.point.z + relative_position.z;
1515
1516 return utm_position;
1517}
1518
1528inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1529 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1530}
1531
1532} // 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 895 of file cpm_ts_getters.h.

930 {
931
932#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
933
942inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
943
950inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
951 return cpm.payload.management_container.reference_time;
952}
953
960inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
961
968inline double getLatitude(const CollectivePerceptionMessage &cpm) {
969 return getLatitude(cpm.payload.management_container.reference_position.latitude);
970}
971
978inline double getLongitude(const CollectivePerceptionMessage &cpm) {
979 return getLongitude(cpm.payload.management_container.reference_position.longitude);
980}
981
988inline double getAltitude(const CollectivePerceptionMessage &cpm) {
989 return getAltitude(cpm.payload.management_container.reference_position.altitude);
990}
991
1003inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
1004 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
1005}
1006
1016inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1017 int zone;
1018 bool northp;
1019 return getUTMPosition(cpm, zone, northp);
1020}
1021
1031inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1032 std::vector<uint8_t> container_ids;
1033 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1034 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1035 }
1036 return container_ids;
1037}
1038
1047inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1048 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1049 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1050 return cpm.payload.cpm_containers.value.array[i];
1051 }
1052 }
1053 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1054}
1055
1064inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1065 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1066}
1067
1075inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1076 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1077 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1078 }
1079 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1080 return number;
1081}
1082
1089inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1090 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1091}
1092
1096
1105inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1106 if (i >= getNumberOfPerceivedObjects(container)) {
1107 throw std::invalid_argument("Index out of range");
1108 }
1109 return container.container_data_perceived_object_container.perceived_objects.array[i];
1110}
1111
1120inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
1121
1128inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
1129 return coordinate.value.value;
1130}
1131
1138inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
1139 return coordinate.confidence.value;
1140}
1141
1148inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
1149 gm::Point point;
1150 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
1151 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
1152 if (object.position.z_coordinate_is_present) {
1153 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
1154 }
1155 return point;
1156}
1157
1165inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1166 double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1167 double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1168 double z_confidence = object.position.z_coordinate_is_present
1169 ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
1170 : std::numeric_limits<double>::infinity();
1171 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1172}
1173
1180inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
1181
1188inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
1189
1200inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
1201 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1202 tf2::Quaternion q;
1203 double roll{0}, pitch{0}, yaw{0};
1204
1205 if (object.angles.x_angle_is_present) {
1206 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
1207 M_PI;
1208 }
1209 if (object.angles.y_angle_is_present) {
1210 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
1211 M_PI;
1212 }
1213 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
1214 M_PI;
1215 // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
1216 q.setRPY(roll, pitch, yaw);
1217
1218 return tf2::toMsg(q);
1219}
1220
1227inline double getYawOfPerceivedObject(const PerceivedObject &object) {
1228 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
1229 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
1230 double roll, pitch, yaw;
1231 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
1232 return yaw;
1233}
1234
1242inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
1243 if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
1244 double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1245 yaw_confidence *= M_PI / 180.0; // convert to radians
1246 return yaw_confidence;
1247}
1248
1255inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
1256 gm::Pose pose;
1257 pose.position = getPositionOfPerceivedObject(object);
1258 pose.orientation = getOrientationOfPerceivedObject(object);
1259 return pose;
1260}
1261
1269inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
1270 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1271 return object.z_angular_velocity.value.value * M_PI / 180.0; // convert to rad/s
1272}
1273
1283inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
1284 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
1285 auto val = object.z_angular_velocity.confidence.value;
1286 static const std::map<uint8_t, double> confidence_map = {
1287 {AngularSpeedConfidence::UNAVAILABLE, 0.0},
1288 {AngularSpeedConfidence::DEG_SEC_01, 1.0},
1289 {AngularSpeedConfidence::DEG_SEC_02, 2.0},
1290 {AngularSpeedConfidence::DEG_SEC_05, 5.0},
1291 {AngularSpeedConfidence::DEG_SEC_10, 10.0},
1292 {AngularSpeedConfidence::DEG_SEC_20, 20.0},
1293 {AngularSpeedConfidence::DEG_SEC_50, 50.0},
1294 {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
1295 };
1296 return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to rad/s
1297}
1298
1305inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
1306
1313inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
1314 return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1315}
1316
1324inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1325 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1326 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1327 throw std::invalid_argument("Velocity is not Cartesian");
1328 }
1329 gm::Vector3 velocity;
1330 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1331 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1332 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1333 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1334 }
1335 return velocity;
1336}
1337
1346inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1347 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1348 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1349 throw std::invalid_argument("Velocity is not Cartesian");
1350 }
1351 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1352 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1353 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1354 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1355 : std::numeric_limits<double>::infinity();
1356 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1357}
1358
1365inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1366 return double(acceleration.value.value) / 10.0;
1367}
1368
1375inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1376 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1377}
1378
1386inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1387 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1388 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1389 throw std::invalid_argument("Acceleration is not Cartesian");
1390 }
1391 gm::Vector3 acceleration;
1392 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1393 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1394 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1395 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1396 }
1397 return acceleration;
1398}
1399
1408inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1409 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1410 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1411 throw std::invalid_argument("Acceleration is not Cartesian");
1412 }
1413 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1414 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1415 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1416 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1417 : std::numeric_limits<double>::infinity();
1418 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1419}
1420
1432inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1433 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1434 return object.object_dimension_x.value.value;
1435}
1436
1444inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1445 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1446 return object.object_dimension_x.confidence.value;
1447}
1448
1460inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1461 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1462 return object.object_dimension_y.value.value;
1463}
1464
1472inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1473 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1474 return object.object_dimension_y.confidence.value;
1475}
1476
1488inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1489 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1490 return object.object_dimension_z.value.value;
1491}
1492
1500inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1501 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1502 return object.object_dimension_z.confidence.value;
1503}
1504
1514inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1515 gm::Vector3 dimensions;
1516 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1517 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1518 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1519 return dimensions;
1520}
1521
1528inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1529 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1530 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1531 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1532 return {conf_x, conf_y, conf_z};
1533}
1534
1545inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1546 const PerceivedObject &object) {
1547 gm::PointStamped utm_position;
1548 gm::PointStamped reference_position = getUTMPosition(cpm);
1549 gm::Point relative_position = getPositionOfPerceivedObject(object);
1550
1551 utm_position.header.frame_id = reference_position.header.frame_id;
1552 utm_position.point.x = reference_position.point.x + relative_position.x;
1553 utm_position.point.y = reference_position.point.y + relative_position.y;
1554 utm_position.point.z = reference_position.point.z + relative_position.z;
1555
1556 return utm_position;
1557}
1558
1568inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1569 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1570}
1571
1572} // 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 883 of file cpm_ts_getters.h.

918 {
919
920#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
921
930inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
931
938inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
939 return cpm.payload.management_container.reference_time;
940}
941
948inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
949
956inline double getLatitude(const CollectivePerceptionMessage &cpm) {
957 return getLatitude(cpm.payload.management_container.reference_position.latitude);
958}
959
966inline double getLongitude(const CollectivePerceptionMessage &cpm) {
967 return getLongitude(cpm.payload.management_container.reference_position.longitude);
968}
969
976inline double getAltitude(const CollectivePerceptionMessage &cpm) {
977 return getAltitude(cpm.payload.management_container.reference_position.altitude);
978}
979
991inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
992 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
993}
994
1004inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
1005 int zone;
1006 bool northp;
1007 return getUTMPosition(cpm, zone, northp);
1008}
1009
1019inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
1020 std::vector<uint8_t> container_ids;
1021 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1022 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
1023 }
1024 return container_ids;
1025}
1026
1035inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
1036 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
1037 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
1038 return cpm.payload.cpm_containers.value.array[i];
1039 }
1040 }
1041 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
1042}
1043
1052inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
1053 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
1054}
1055
1063inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
1064 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1065 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1066 }
1067 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
1068 return number;
1069}
1070
1077inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
1078 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
1079}
1080
1084
1093inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
1094 if (i >= getNumberOfPerceivedObjects(container)) {
1095 throw std::invalid_argument("Index out of range");
1096 }
1097 return container.container_data_perceived_object_container.perceived_objects.array[i];
1098}
1099
1108inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
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) throw std::invalid_argument("No velocity present in PerceivedObject");
1314 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1315 throw std::invalid_argument("Velocity is not Cartesian");
1316 }
1317 gm::Vector3 velocity;
1318 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1319 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1320 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1321 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1322 }
1323 return velocity;
1324}
1325
1334inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
1335 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1336 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1337 throw std::invalid_argument("Velocity is not Cartesian");
1338 }
1339 double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
1340 double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
1341 double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
1342 ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
1343 : std::numeric_limits<double>::infinity();
1344 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1345}
1346
1353inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1354 return double(acceleration.value.value) / 10.0;
1355}
1356
1363inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1364 return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1365}
1366
1374inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1375 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1376 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1377 throw std::invalid_argument("Acceleration is not Cartesian");
1378 }
1379 gm::Vector3 acceleration;
1380 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1381 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1382 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1383 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1384 }
1385 return acceleration;
1386}
1387
1396inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
1397 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1398 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1399 throw std::invalid_argument("Acceleration is not Cartesian");
1400 }
1401 double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
1402 double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
1403 double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
1404 ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
1405 : std::numeric_limits<double>::infinity();
1406 return std::make_tuple(x_confidence, y_confidence, z_confidence);
1407}
1408
1420inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1421 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1422 return object.object_dimension_x.value.value;
1423}
1424
1432inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1433 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1434 return object.object_dimension_x.confidence.value;
1435}
1436
1448inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1449 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1450 return object.object_dimension_y.value.value;
1451}
1452
1460inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1461 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1462 return object.object_dimension_y.confidence.value;
1463}
1464
1476inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1477 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1478 return object.object_dimension_z.value.value;
1479}
1480
1488inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
1489 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1490 return object.object_dimension_z.confidence.value;
1491}
1492
1502inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1503 gm::Vector3 dimensions;
1504 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1505 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1506 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1507 return dimensions;
1508}
1509
1516inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
1517 double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1518 double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1519 double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
1520 return {conf_x, conf_y, conf_z};
1521}
1522
1533inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1534 const PerceivedObject &object) {
1535 gm::PointStamped utm_position;
1536 gm::PointStamped reference_position = getUTMPosition(cpm);
1537 gm::Point relative_position = getPositionOfPerceivedObject(object);
1538
1539 utm_position.header.frame_id = reference_position.header.frame_id;
1540 utm_position.point.x = reference_position.point.x + relative_position.x;
1541 utm_position.point.y = reference_position.point.y + relative_position.y;
1542 utm_position.point.z = reference_position.point.z + relative_position.z;
1543
1544 return utm_position;
1545}
1546
1556inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
1557 return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
1558}
1559
1560} // 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 211 of file cpm_ts_getters.h.

211 {
212 if (i >= getNumberOfPerceivedObjects(container)) {
213 throw std::invalid_argument("Index out of range");
214 }