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

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

#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.
 
std::vector< bool > etsi_its_cpm_ts_msgs::access::getBitString (const std::vector< uint8_t > &buffer, const int bits_unused)
 Get a Bit String in form of bool vector.
 
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.
 
double etsi_its_cpm_ts_msgs::access::getLongitudinalAcceleration (const AccelerationComponent &longitudinal_acceleration)
 Get the longitudinal acceleration.
 
double etsi_its_cpm_ts_msgs::access::getLateralAcceleration (const AccelerationComponent &lateral_acceleration)
 Get the lateral acceleration.
 
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)
 
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.
 
gm::Pose etsi_its_cpm_ts_msgs::access::getPoseOfPerceivedObject (const PerceivedObject &object)
 Get the pose of the PerceivedObject.
 
int16_t etsi_its_cpm_ts_msgs::access::getYawRateOfPerceivedObject (const PerceivedObject &object)
 Get the yaw rate of the PerceivedObject.
 
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.
 
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.
 
uint16_t etsi_its_cpm_ts_msgs::access::getXDimensionOfPerceivedObject (const PerceivedObject &object)
 Gets 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.
 
uint16_t etsi_its_cpm_ts_msgs::access::getZDimensionOfPerceivedObject (const PerceivedObject &object)
 Retrieves 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.
 
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.
 

Detailed Description

Getter functions for the ETSI ITS CPM (TS)

Definition in file cpm_ts_getters.h.

Function Documentation

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

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

562 {
563
564#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
565
574inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
575
582inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
583 return cpm.payload.management_container.reference_time;
584}
585
592inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
593
600inline double getLatitude(const CollectivePerceptionMessage &cpm) {
601 return getLatitude(cpm.payload.management_container.reference_position.latitude);
602}
603
610inline double getLongitude(const CollectivePerceptionMessage &cpm) {
611 return getLongitude(cpm.payload.management_container.reference_position.longitude);
612}
613
620inline double getAltitude(const CollectivePerceptionMessage &cpm) {
621 return getAltitude(cpm.payload.management_container.reference_position.altitude);
622}
623
635inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
636 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
637}
638
648inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
649 int zone;
650 bool northp;
651 return getUTMPosition(cpm, zone, northp);
652}
653
663inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
664 std::vector<uint8_t> container_ids;
665 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
666 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
667 }
668 return container_ids;
669}
670
679inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
680 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
681 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
682 return cpm.payload.cpm_containers.value.array[i];
683 }
684 }
685 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
686}
687
696inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
697 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
698}
699
707inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
708 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
709 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
710 }
711 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
712 return number;
713}
714
721inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
722 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
723}
724
728
737inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
738 if (i >= getNumberOfPerceivedObjects(container)) {
739 throw std::invalid_argument("Index out of range");
740 }
741 return container.container_data_perceived_object_container.perceived_objects.array[i];
742}
743
752inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
753
760inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
761 return coordinate.value.value;
762}
763
770inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
771 return coordinate.confidence.value;
772}
773
780inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
781 gm::Point point;
782 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
783 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
784 if (object.position.z_coordinate_is_present) {
785 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
786 }
787 return point;
788}
789
796inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
797
804inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
805
816inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
817 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
818 tf2::Quaternion q;
819 double roll{0}, pitch{0}, yaw{0};
820
821 if (object.angles.x_angle_is_present) {
822 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
823 M_PI; // TODO: check if 0-360 -> -180-180 is correct
824 }
825 if (object.angles.y_angle_is_present) {
826 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
827 M_PI; // TODO: check if 0-360 -> -180-180 is correct
828 }
829 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
830 M_PI; // TODO: check if 0-360 -> -180-180 is correct
831 q.setRPY(roll, pitch, yaw);
832
833 return tf2::toMsg(q);
834}
835
842inline double getYawOfPerceivedObject(const PerceivedObject &object) {
843 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
844 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
845 double roll, pitch, yaw;
846 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
847 return yaw;
848}
849
856inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
857 gm::Pose pose;
858 pose.position = getPositionOfPerceivedObject(object);
859 pose.orientation = getOrientationOfPerceivedObject(object);
860 return pose;
861}
862
870inline int16_t getYawRateOfPerceivedObject(const PerceivedObject &object) {
871 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
872 return object.z_angular_velocity.value.value;
873}
874
881inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
882
889inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
890 return double(velocity.confidence.value) / 100.0;
891}
892
900inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
901 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
902 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
903 throw std::invalid_argument("Velocity is not Cartesian");
904 }
905 gm::Vector3 velocity;
906 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
907 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
908 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
909 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
910 }
911 return velocity;
912}
913
920inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
921 return double(acceleration.value.value) / 10.0;
922}
923
930inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
931 return double(acceleration.confidence.value) / 10.0;
932}
933
941inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
942 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
943 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
944 throw std::invalid_argument("Acceleration is not Cartesian");
945 }
946 gm::Vector3 acceleration;
947 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
948 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
949 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
950 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
951 }
952 return acceleration;
953}
954
966inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
967 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
968 return object.object_dimension_x.value.value;
969}
970
982inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
983 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
984 return object.object_dimension_y.value.value;
985}
986
998inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
999 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1000 return object.object_dimension_z.value.value;
1001}
1002
1012inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1013 gm::Vector3 dimensions;
1014 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1015 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1016 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1017 return dimensions;
1018}
1019
1030inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1031 const PerceivedObject &object) {
1032 gm::PointStamped utm_position;
1033 gm::PointStamped reference_position = getUTMPosition(cpm);
1034 gm::Point relative_position = getPositionOfPerceivedObject(object);
1035
1036 utm_position.header.frame_id = reference_position.header.frame_id;
1037 utm_position.point.x = reference_position.point.x + relative_position.x;
1038 utm_position.point.y = reference_position.point.y + relative_position.y;
1039 utm_position.point.z = reference_position.point.z + relative_position.z;
1040
1041 return utm_position;
1042}
1043
1044} // namespace etsi_its_cpm_ts_msgs::access
getLongitude
double getLongitude(const CAM &cam)
Get the Longitude value of CAM.
Definition cam_getters_common.h:83
getUTMPosition
gm::PointStamped getUTMPosition(const CAM &cam, int &zone, bool &northp)
Get the UTM Position defined within the BasicContainer of the CAM.
Definition cam_getters_common.h:207
getStationID
uint32_t getStationID(const CAM &cam)
Get the Station ID object.
Definition cam_getters_common.h:41
getLatitude
double getLatitude(const CAM &cam)
Get the Latitude value of CAM.
Definition cam_getters_common.h:73
getAltitude
double getAltitude(const CAM &cam)
Get the Altitude value of CAM.
Definition cam_getters_common.h:93
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::getCartesianCoordinate
int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate)
Retrieves the Cartesian coordinate value from a CartesianCoordinateWithConfidence object.
Definition cpm_ts_getters.h:359
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:597
etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects
uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container)
Definition cpm_ts_getters.h:306
etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject
gm::Point getPositionOfPerceivedObject(const PerceivedObject &object)
Definition cpm_ts_getters.h:379
etsi_its_cpm_ts_msgs::access::getReferenceTimeValue
uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm)
Get the ReferenceTime-Value.
Definition cpm_ts_getters.h:191
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:369
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:403
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:581
etsi_its_cpm_ts_msgs::access::getVelocityComponent
double getVelocityComponent(const VelocityComponent &velocity)
Get the velocity component of the PerceivedObject.
Definition cpm_ts_getters.h:480
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:295
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:611
etsi_its_cpm_ts_msgs::access::getPoseOfPerceivedObject
gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object)
Get the pose of the PerceivedObject.
Definition cpm_ts_getters.h:455
etsi_its_cpm_ts_msgs::access::getYawRateOfPerceivedObject
int16_t getYawRateOfPerceivedObject(const PerceivedObject &object)
Get the yaw rate of the PerceivedObject.
Definition cpm_ts_getters.h:469
etsi_its_cpm_ts_msgs::access::getYawOfPerceivedObject
double getYawOfPerceivedObject(const PerceivedObject &object)
Get the yaw of the PerceivedObject.
Definition cpm_ts_getters.h:441
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:629
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:395
etsi_its_cpm_ts_msgs::access::getVelocityComponentConfidence
double getVelocityComponentConfidence(const VelocityComponent &velocity)
Get the confidence of the velocity component.
Definition cpm_ts_getters.h:488
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:351
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:499
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:565
etsi_its_cpm_ts_msgs::access::getReferenceTime
TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm)
Get the Reference Time object.
Definition cpm_ts_getters.h:181
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:262
etsi_its_cpm_ts_msgs::access::getAccelerationComponentConfidence
double getAccelerationComponentConfidence(const AccelerationComponent &acceleration)
Get the confidence of the acceleration component.
Definition cpm_ts_getters.h:529
etsi_its_cpm_ts_msgs::access::getCpmContainer
WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id)
Definition cpm_ts_getters.h:278
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:336
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:540
etsi_its_cpm_ts_msgs::access::getAccelerationComponent
double getAccelerationComponent(const AccelerationComponent &acceleration)
Get the acceleration component of the PerceivedObject.
Definition cpm_ts_getters.h:519
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:415

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

◆ getBitString()

std::vector< bool > etsi_its_cpm_ts_msgs::access::getBitString ( const std::vector< uint8_t > & buffer,
const int bits_unused )
inline

Get a Bit String in form of bool vector.

Parameters
bufferas uint8_t vector
bits_unusednumber of bits to ignore at the end of the bit string
Returns
std::vector<bool>

Definition at line 85 of file cpm_ts_getters.h.

92 {
93 return getAltitude(cpm.payload.management_container.reference_position.altitude);
94}
95
107inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {

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

573 {
574
575#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
576
585inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
586
593inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
594 return cpm.payload.management_container.reference_time;
595}
596
603inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
604
611inline double getLatitude(const CollectivePerceptionMessage &cpm) {
612 return getLatitude(cpm.payload.management_container.reference_position.latitude);
613}
614
621inline double getLongitude(const CollectivePerceptionMessage &cpm) {
622 return getLongitude(cpm.payload.management_container.reference_position.longitude);
623}
624
631inline double getAltitude(const CollectivePerceptionMessage &cpm) {
632 return getAltitude(cpm.payload.management_container.reference_position.altitude);
633}
634
646inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
647 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
648}
649
659inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
660 int zone;
661 bool northp;
662 return getUTMPosition(cpm, zone, northp);
663}
664
674inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
675 std::vector<uint8_t> container_ids;
676 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
677 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
678 }
679 return container_ids;
680}
681
690inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
691 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
692 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
693 return cpm.payload.cpm_containers.value.array[i];
694 }
695 }
696 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
697}
698
707inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
708 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
709}
710
718inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
719 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
720 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
721 }
722 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
723 return number;
724}
725
732inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
733 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
734}
735
739
748inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
749 if (i >= getNumberOfPerceivedObjects(container)) {
750 throw std::invalid_argument("Index out of range");
751 }
752 return container.container_data_perceived_object_container.perceived_objects.array[i];
753}
754
763inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
764
771inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
772 return coordinate.value.value;
773}
774
781inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
782 return coordinate.confidence.value;
783}
784
791inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
792 gm::Point point;
793 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
794 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
795 if (object.position.z_coordinate_is_present) {
796 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
797 }
798 return point;
799}
800
807inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
808
815inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
816
827inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
828 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
829 tf2::Quaternion q;
830 double roll{0}, pitch{0}, yaw{0};
831
832 if (object.angles.x_angle_is_present) {
833 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
834 M_PI; // TODO: check if 0-360 -> -180-180 is correct
835 }
836 if (object.angles.y_angle_is_present) {
837 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
838 M_PI; // TODO: check if 0-360 -> -180-180 is correct
839 }
840 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
841 M_PI; // TODO: check if 0-360 -> -180-180 is correct
842 q.setRPY(roll, pitch, yaw);
843
844 return tf2::toMsg(q);
845}
846
853inline double getYawOfPerceivedObject(const PerceivedObject &object) {
854 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
855 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
856 double roll, pitch, yaw;
857 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
858 return yaw;
859}
860
867inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
868 gm::Pose pose;
869 pose.position = getPositionOfPerceivedObject(object);
870 pose.orientation = getOrientationOfPerceivedObject(object);
871 return pose;
872}
873
881inline int16_t getYawRateOfPerceivedObject(const PerceivedObject &object) {
882 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
883 return object.z_angular_velocity.value.value;
884}
885
892inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
893
900inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
901 return double(velocity.confidence.value) / 100.0;
902}
903
911inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
912 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
913 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
914 throw std::invalid_argument("Velocity is not Cartesian");
915 }
916 gm::Vector3 velocity;
917 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
918 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
919 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
920 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
921 }
922 return velocity;
923}
924
931inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
932 return double(acceleration.value.value) / 10.0;
933}
934
941inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
942 return double(acceleration.confidence.value) / 10.0;
943}
944
952inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
953 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
954 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
955 throw std::invalid_argument("Acceleration is not Cartesian");
956 }
957 gm::Vector3 acceleration;
958 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
959 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
960 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
961 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
962 }
963 return acceleration;
964}
965
977inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
978 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
979 return object.object_dimension_x.value.value;
980}
981
993inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
994 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
995 return object.object_dimension_y.value.value;
996}
997
1009inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1010 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1011 return object.object_dimension_z.value.value;
1012}
1013
1023inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1024 gm::Vector3 dimensions;
1025 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1026 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1027 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1028 return dimensions;
1029}
1030
1041inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1042 const PerceivedObject &object) {
1043 gm::PointStamped utm_position;
1044 gm::PointStamped reference_position = getUTMPosition(cpm);
1045 gm::Point relative_position = getPositionOfPerceivedObject(object);
1046
1047 utm_position.header.frame_id = reference_position.header.frame_id;
1048 utm_position.point.x = reference_position.point.x + relative_position.x;
1049 utm_position.point.y = reference_position.point.y + relative_position.y;
1050 utm_position.point.z = reference_position.point.z + relative_position.z;
1051
1052 return utm_position;
1053}
1054
1055} // 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 395 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 403 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 359 of file cpm_ts_getters.h.

361 {

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

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

503 {
504 gm::PointStamped utm_position;
505 gm::PointStamped reference_position = getUTMPosition(cpm);
506 gm::Point relative_position = getPositionOfPerceivedObject(object);
507
508 utm_position.header.frame_id = reference_position.header.frame_id;
509 utm_position.point.x = reference_position.point.x + relative_position.x;
510 utm_position.point.y = reference_position.point.y + relative_position.y;
511 utm_position.point.z = reference_position.point.z + relative_position.z;

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

268 { return angle.value.value; }

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

644 {
645
646#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
647
656inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
657
664inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
665 return cpm.payload.management_container.reference_time;
666}
667
674inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
675
682inline double getLatitude(const CollectivePerceptionMessage &cpm) {
683 return getLatitude(cpm.payload.management_container.reference_position.latitude);
684}
685
692inline double getLongitude(const CollectivePerceptionMessage &cpm) {
693 return getLongitude(cpm.payload.management_container.reference_position.longitude);
694}
695
702inline double getAltitude(const CollectivePerceptionMessage &cpm) {
703 return getAltitude(cpm.payload.management_container.reference_position.altitude);
704}
705
717inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
718 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
719}
720
730inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
731 int zone;
732 bool northp;
733 return getUTMPosition(cpm, zone, northp);
734}
735
745inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
746 std::vector<uint8_t> container_ids;
747 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
748 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
749 }
750 return container_ids;
751}
752
761inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
762 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
763 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
764 return cpm.payload.cpm_containers.value.array[i];
765 }
766 }
767 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
768}
769
778inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
779 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
780}
781
789inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
790 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
791 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
792 }
793 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
794 return number;
795}
796
803inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
804 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
805}
806
810
819inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
820 if (i >= getNumberOfPerceivedObjects(container)) {
821 throw std::invalid_argument("Index out of range");
822 }
823 return container.container_data_perceived_object_container.perceived_objects.array[i];
824}
825
834inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
835
842inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
843 return coordinate.value.value;
844}
845
852inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
853 return coordinate.confidence.value;
854}
855
862inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
863 gm::Point point;
864 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
865 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
866 if (object.position.z_coordinate_is_present) {
867 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
868 }
869 return point;
870}
871
878inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
879
886inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
887
898inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
899 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
900 tf2::Quaternion q;
901 double roll{0}, pitch{0}, yaw{0};
902
903 if (object.angles.x_angle_is_present) {
904 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
905 M_PI; // TODO: check if 0-360 -> -180-180 is correct
906 }
907 if (object.angles.y_angle_is_present) {
908 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
909 M_PI; // TODO: check if 0-360 -> -180-180 is correct
910 }
911 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
912 M_PI; // TODO: check if 0-360 -> -180-180 is correct
913 q.setRPY(roll, pitch, yaw);
914
915 return tf2::toMsg(q);
916}
917
924inline double getYawOfPerceivedObject(const PerceivedObject &object) {
925 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
926 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
927 double roll, pitch, yaw;
928 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
929 return yaw;
930}
931
938inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
939 gm::Pose pose;
940 pose.position = getPositionOfPerceivedObject(object);
941 pose.orientation = getOrientationOfPerceivedObject(object);
942 return pose;
943}
944
952inline int16_t getYawRateOfPerceivedObject(const PerceivedObject &object) {
953 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
954 return object.z_angular_velocity.value.value;
955}
956
963inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
964
971inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
972 return double(velocity.confidence.value) / 100.0;
973}
974
982inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
983 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
984 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
985 throw std::invalid_argument("Velocity is not Cartesian");
986 }
987 gm::Vector3 velocity;
988 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
989 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
990 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
991 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
992 }
993 return velocity;
994}
995
1002inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1003 return double(acceleration.value.value) / 10.0;
1004}
1005
1012inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1013 return double(acceleration.confidence.value) / 10.0;
1014}
1015
1023inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1024 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1025 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1026 throw std::invalid_argument("Acceleration is not Cartesian");
1027 }
1028 gm::Vector3 acceleration;
1029 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1030 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1031 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1032 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1033 }
1034 return acceleration;
1035}
1036
1048inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1049 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1050 return object.object_dimension_x.value.value;
1051}
1052
1064inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1065 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1066 return object.object_dimension_y.value.value;
1067}
1068
1080inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1081 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1082 return object.object_dimension_z.value.value;
1083}
1084
1094inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1095 gm::Vector3 dimensions;
1096 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1097 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1098 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1099 return dimensions;
1100}
1101
1112inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1113 const PerceivedObject &object) {
1114 gm::PointStamped utm_position;
1115 gm::PointStamped reference_position = getUTMPosition(cpm);
1116 gm::Point relative_position = getPositionOfPerceivedObject(object);
1117
1118 utm_position.header.frame_id = reference_position.header.frame_id;
1119 utm_position.point.x = reference_position.point.x + relative_position.x;
1120 utm_position.point.y = reference_position.point.y + relative_position.y;
1121 utm_position.point.z = reference_position.point.z + relative_position.z;
1122
1123 return utm_position;
1124}
1125
1126} // namespace etsi_its_cpm_ts_msgs::access

◆ 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 351 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 159 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 199 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 52 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 209 of file cpm_ts_getters.h.

209 {
210 if (i >= getNumberOfPerceivedObjects(container)) {
211 throw std::invalid_argument("Index out of range");

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

151 {

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

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

415 {
416 throw std::invalid_argument("Acceleration is not Cartesian");
417 }
418 gm::Vector3 acceleration;
419 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
420 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
421 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
422 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
423 }
424 return acceleration;
425}
426

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

297 {

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

380 {
381 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
382 }
383 return velocity;
384}
385

◆ 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 181 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 191 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 76 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 173 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 44 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 247 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 234 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 121 of file cpm_ts_getters.h.

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

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

662 {
663
664#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
665
674inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
675
682inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
683 return cpm.payload.management_container.reference_time;
684}
685
692inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
693
700inline double getLatitude(const CollectivePerceptionMessage &cpm) {
701 return getLatitude(cpm.payload.management_container.reference_position.latitude);
702}
703
710inline double getLongitude(const CollectivePerceptionMessage &cpm) {
711 return getLongitude(cpm.payload.management_container.reference_position.longitude);
712}
713
720inline double getAltitude(const CollectivePerceptionMessage &cpm) {
721 return getAltitude(cpm.payload.management_container.reference_position.altitude);
722}
723
735inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
736 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
737}
738
748inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
749 int zone;
750 bool northp;
751 return getUTMPosition(cpm, zone, northp);
752}
753
763inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
764 std::vector<uint8_t> container_ids;
765 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
766 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
767 }
768 return container_ids;
769}
770
779inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
780 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
781 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
782 return cpm.payload.cpm_containers.value.array[i];
783 }
784 }
785 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
786}
787
796inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
797 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
798}
799
807inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
808 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
809 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
810 }
811 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
812 return number;
813}
814
821inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
822 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
823}
824
828
837inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
838 if (i >= getNumberOfPerceivedObjects(container)) {
839 throw std::invalid_argument("Index out of range");
840 }
841 return container.container_data_perceived_object_container.perceived_objects.array[i];
842}
843
852inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
853
860inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
861 return coordinate.value.value;
862}
863
870inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
871 return coordinate.confidence.value;
872}
873
880inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
881 gm::Point point;
882 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
883 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
884 if (object.position.z_coordinate_is_present) {
885 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
886 }
887 return point;
888}
889
896inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
897
904inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
905
916inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
917 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
918 tf2::Quaternion q;
919 double roll{0}, pitch{0}, yaw{0};
920
921 if (object.angles.x_angle_is_present) {
922 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
923 M_PI; // TODO: check if 0-360 -> -180-180 is correct
924 }
925 if (object.angles.y_angle_is_present) {
926 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
927 M_PI; // TODO: check if 0-360 -> -180-180 is correct
928 }
929 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
930 M_PI; // TODO: check if 0-360 -> -180-180 is correct
931 q.setRPY(roll, pitch, yaw);
932
933 return tf2::toMsg(q);
934}
935
942inline double getYawOfPerceivedObject(const PerceivedObject &object) {
943 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
944 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
945 double roll, pitch, yaw;
946 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
947 return yaw;
948}
949
956inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
957 gm::Pose pose;
958 pose.position = getPositionOfPerceivedObject(object);
959 pose.orientation = getOrientationOfPerceivedObject(object);
960 return pose;
961}
962
970inline int16_t getYawRateOfPerceivedObject(const PerceivedObject &object) {
971 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
972 return object.z_angular_velocity.value.value;
973}
974
981inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
982
989inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
990 return double(velocity.confidence.value) / 100.0;
991}
992
1000inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
1001 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
1002 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
1003 throw std::invalid_argument("Velocity is not Cartesian");
1004 }
1005 gm::Vector3 velocity;
1006 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
1007 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
1008 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
1009 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
1010 }
1011 return velocity;
1012}
1013
1020inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
1021 return double(acceleration.value.value) / 10.0;
1022}
1023
1030inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
1031 return double(acceleration.confidence.value) / 10.0;
1032}
1033
1041inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1042 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1043 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1044 throw std::invalid_argument("Acceleration is not Cartesian");
1045 }
1046 gm::Vector3 acceleration;
1047 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1048 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1049 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1050 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1051 }
1052 return acceleration;
1053}
1054
1066inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1067 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1068 return object.object_dimension_x.value.value;
1069}
1070
1082inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1083 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1084 return object.object_dimension_y.value.value;
1085}
1086
1098inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1099 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1100 return object.object_dimension_z.value.value;
1101}
1102
1112inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1113 gm::Vector3 dimensions;
1114 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1115 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1116 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1117 return dimensions;
1118}
1119
1130inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1131 const PerceivedObject &object) {
1132 gm::PointStamped utm_position;
1133 gm::PointStamped reference_position = getUTMPosition(cpm);
1134 gm::Point relative_position = getPositionOfPerceivedObject(object);
1135
1136 utm_position.header.frame_id = reference_position.header.frame_id;
1137 utm_position.point.x = reference_position.point.x + relative_position.x;
1138 utm_position.point.y = reference_position.point.y + relative_position.y;
1139 utm_position.point.z = reference_position.point.z + relative_position.z;
1140
1141 return utm_position;
1142}
1143
1144} // 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 480 of file cpm_ts_getters.h.

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

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

598 {
599
600#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
601
610inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
611
618inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
619 return cpm.payload.management_container.reference_time;
620}
621
628inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
629
636inline double getLatitude(const CollectivePerceptionMessage &cpm) {
637 return getLatitude(cpm.payload.management_container.reference_position.latitude);
638}
639
646inline double getLongitude(const CollectivePerceptionMessage &cpm) {
647 return getLongitude(cpm.payload.management_container.reference_position.longitude);
648}
649
656inline double getAltitude(const CollectivePerceptionMessage &cpm) {
657 return getAltitude(cpm.payload.management_container.reference_position.altitude);
658}
659
671inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
672 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
673}
674
684inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
685 int zone;
686 bool northp;
687 return getUTMPosition(cpm, zone, northp);
688}
689
699inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
700 std::vector<uint8_t> container_ids;
701 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
702 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
703 }
704 return container_ids;
705}
706
715inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
716 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
717 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
718 return cpm.payload.cpm_containers.value.array[i];
719 }
720 }
721 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
722}
723
732inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
733 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
734}
735
743inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
744 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
745 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
746 }
747 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
748 return number;
749}
750
757inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
758 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
759}
760
764
773inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
774 if (i >= getNumberOfPerceivedObjects(container)) {
775 throw std::invalid_argument("Index out of range");
776 }
777 return container.container_data_perceived_object_container.perceived_objects.array[i];
778}
779
788inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
789
796inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
797 return coordinate.value.value;
798}
799
806inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
807 return coordinate.confidence.value;
808}
809
816inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
817 gm::Point point;
818 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
819 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
820 if (object.position.z_coordinate_is_present) {
821 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
822 }
823 return point;
824}
825
832inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
833
840inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
841
852inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
853 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
854 tf2::Quaternion q;
855 double roll{0}, pitch{0}, yaw{0};
856
857 if (object.angles.x_angle_is_present) {
858 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
859 M_PI; // TODO: check if 0-360 -> -180-180 is correct
860 }
861 if (object.angles.y_angle_is_present) {
862 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
863 M_PI; // TODO: check if 0-360 -> -180-180 is correct
864 }
865 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
866 M_PI; // TODO: check if 0-360 -> -180-180 is correct
867 q.setRPY(roll, pitch, yaw);
868
869 return tf2::toMsg(q);
870}
871
878inline double getYawOfPerceivedObject(const PerceivedObject &object) {
879 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
880 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
881 double roll, pitch, yaw;
882 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
883 return yaw;
884}
885
892inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
893 gm::Pose pose;
894 pose.position = getPositionOfPerceivedObject(object);
895 pose.orientation = getOrientationOfPerceivedObject(object);
896 return pose;
897}
898
906inline int16_t getYawRateOfPerceivedObject(const PerceivedObject &object) {
907 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
908 return object.z_angular_velocity.value.value;
909}
910
917inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
918
925inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
926 return double(velocity.confidence.value) / 100.0;
927}
928
936inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
937 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
938 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
939 throw std::invalid_argument("Velocity is not Cartesian");
940 }
941 gm::Vector3 velocity;
942 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
943 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
944 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
945 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
946 }
947 return velocity;
948}
949
956inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
957 return double(acceleration.value.value) / 10.0;
958}
959
966inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
967 return double(acceleration.confidence.value) / 10.0;
968}
969
977inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
978 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
979 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
980 throw std::invalid_argument("Acceleration is not Cartesian");
981 }
982 gm::Vector3 acceleration;
983 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
984 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
985 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
986 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
987 }
988 return acceleration;
989}
990
1002inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1003 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1004 return object.object_dimension_x.value.value;
1005}
1006
1018inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1019 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1020 return object.object_dimension_y.value.value;
1021}
1022
1034inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1035 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1036 return object.object_dimension_z.value.value;
1037}
1038
1048inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1049 gm::Vector3 dimensions;
1050 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1051 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1052 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1053 return dimensions;
1054}
1055
1066inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1067 const PerceivedObject &object) {
1068 gm::PointStamped utm_position;
1069 gm::PointStamped reference_position = getUTMPosition(cpm);
1070 gm::Point relative_position = getPositionOfPerceivedObject(object);
1071
1072 utm_position.header.frame_id = reference_position.header.frame_id;
1073 utm_position.point.x = reference_position.point.x + relative_position.x;
1074 utm_position.point.y = reference_position.point.y + relative_position.y;
1075 utm_position.point.z = reference_position.point.z + relative_position.z;
1076
1077 return utm_position;
1078}
1079
1080} // namespace etsi_its_cpm_ts_msgs::access

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

◆ getYawRateOfPerceivedObject()

int16_t 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
int16_t yaw rate of the PerceivedObject in deg/s
Exceptions
std::invalid_argumentIf the yaw rate is not present in the object.

Definition at line 469 of file cpm_ts_getters.h.

470 {
471 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
472 return object.object_dimension_z.value.value;

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

614 {
615
616#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
617
626inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
627
634inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
635 return cpm.payload.management_container.reference_time;
636}
637
644inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
645
652inline double getLatitude(const CollectivePerceptionMessage &cpm) {
653 return getLatitude(cpm.payload.management_container.reference_position.latitude);
654}
655
662inline double getLongitude(const CollectivePerceptionMessage &cpm) {
663 return getLongitude(cpm.payload.management_container.reference_position.longitude);
664}
665
672inline double getAltitude(const CollectivePerceptionMessage &cpm) {
673 return getAltitude(cpm.payload.management_container.reference_position.altitude);
674}
675
687inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
688 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
689}
690
700inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
701 int zone;
702 bool northp;
703 return getUTMPosition(cpm, zone, northp);
704}
705
715inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
716 std::vector<uint8_t> container_ids;
717 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
718 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
719 }
720 return container_ids;
721}
722
731inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
732 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
733 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
734 return cpm.payload.cpm_containers.value.array[i];
735 }
736 }
737 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
738}
739
748inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
749 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
750}
751
759inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
760 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
761 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
762 }
763 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
764 return number;
765}
766
773inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
774 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
775}
776
780
789inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
790 if (i >= getNumberOfPerceivedObjects(container)) {
791 throw std::invalid_argument("Index out of range");
792 }
793 return container.container_data_perceived_object_container.perceived_objects.array[i];
794}
795
804inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
805
812inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
813 return coordinate.value.value;
814}
815
822inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
823 return coordinate.confidence.value;
824}
825
832inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
833 gm::Point point;
834 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
835 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
836 if (object.position.z_coordinate_is_present) {
837 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
838 }
839 return point;
840}
841
848inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
849
856inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
857
868inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
869 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
870 tf2::Quaternion q;
871 double roll{0}, pitch{0}, yaw{0};
872
873 if (object.angles.x_angle_is_present) {
874 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
875 M_PI; // TODO: check if 0-360 -> -180-180 is correct
876 }
877 if (object.angles.y_angle_is_present) {
878 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
879 M_PI; // TODO: check if 0-360 -> -180-180 is correct
880 }
881 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
882 M_PI; // TODO: check if 0-360 -> -180-180 is correct
883 q.setRPY(roll, pitch, yaw);
884
885 return tf2::toMsg(q);
886}
887
894inline double getYawOfPerceivedObject(const PerceivedObject &object) {
895 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
896 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
897 double roll, pitch, yaw;
898 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
899 return yaw;
900}
901
908inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
909 gm::Pose pose;
910 pose.position = getPositionOfPerceivedObject(object);
911 pose.orientation = getOrientationOfPerceivedObject(object);
912 return pose;
913}
914
922inline int16_t getYawRateOfPerceivedObject(const PerceivedObject &object) {
923 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
924 return object.z_angular_velocity.value.value;
925}
926
933inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
934
941inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
942 return double(velocity.confidence.value) / 100.0;
943}
944
952inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
953 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
954 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
955 throw std::invalid_argument("Velocity is not Cartesian");
956 }
957 gm::Vector3 velocity;
958 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
959 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
960 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
961 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
962 }
963 return velocity;
964}
965
972inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
973 return double(acceleration.value.value) / 10.0;
974}
975
982inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
983 return double(acceleration.confidence.value) / 10.0;
984}
985
993inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
994 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
995 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
996 throw std::invalid_argument("Acceleration is not Cartesian");
997 }
998 gm::Vector3 acceleration;
999 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1000 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1001 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1002 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1003 }
1004 return acceleration;
1005}
1006
1018inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1019 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1020 return object.object_dimension_x.value.value;
1021}
1022
1034inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1035 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1036 return object.object_dimension_y.value.value;
1037}
1038
1050inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1051 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1052 return object.object_dimension_z.value.value;
1053}
1054
1064inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1065 gm::Vector3 dimensions;
1066 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1067 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1068 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1069 return dimensions;
1070}
1071
1082inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1083 const PerceivedObject &object) {
1084 gm::PointStamped utm_position;
1085 gm::PointStamped reference_position = getUTMPosition(cpm);
1086 gm::Point relative_position = getPositionOfPerceivedObject(object);
1087
1088 utm_position.header.frame_id = reference_position.header.frame_id;
1089 utm_position.point.x = reference_position.point.x + relative_position.x;
1090 utm_position.point.y = reference_position.point.y + relative_position.y;
1091 utm_position.point.z = reference_position.point.z + relative_position.z;
1092
1093 return utm_position;
1094}
1095
1096} // 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 597 of file cpm_ts_getters.h.

630 {
631
632#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>
633
642inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }
643
650inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
651 return cpm.payload.management_container.reference_time;
652}
653
660inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
661
668inline double getLatitude(const CollectivePerceptionMessage &cpm) {
669 return getLatitude(cpm.payload.management_container.reference_position.latitude);
670}
671
678inline double getLongitude(const CollectivePerceptionMessage &cpm) {
679 return getLongitude(cpm.payload.management_container.reference_position.longitude);
680}
681
688inline double getAltitude(const CollectivePerceptionMessage &cpm) {
689 return getAltitude(cpm.payload.management_container.reference_position.altitude);
690}
691
703inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
704 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
705}
706
716inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
717 int zone;
718 bool northp;
719 return getUTMPosition(cpm, zone, northp);
720}
721
731inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
732 std::vector<uint8_t> container_ids;
733 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
734 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
735 }
736 return container_ids;
737}
738
747inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
748 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
749 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
750 return cpm.payload.cpm_containers.value.array[i];
751 }
752 }
753 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
754}
755
764inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
765 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
766}
767
775inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
776 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
777 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
778 }
779 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
780 return number;
781}
782
789inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
790 return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
791}
792
796
805inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
806 if (i >= getNumberOfPerceivedObjects(container)) {
807 throw std::invalid_argument("Index out of range");
808 }
809 return container.container_data_perceived_object_container.perceived_objects.array[i];
810}
811
820inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; }
821
828inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
829 return coordinate.value.value;
830}
831
838inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
839 return coordinate.confidence.value;
840}
841
848inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
849 gm::Point point;
850 point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
851 point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
852 if (object.position.z_coordinate_is_present) {
853 point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
854 }
855 return point;
856}
857
864inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }
865
872inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }
873
884inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
885 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
886 tf2::Quaternion q;
887 double roll{0}, pitch{0}, yaw{0};
888
889 if (object.angles.x_angle_is_present) {
890 roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
891 M_PI; // TODO: check if 0-360 -> -180-180 is correct
892 }
893 if (object.angles.y_angle_is_present) {
894 pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
895 M_PI; // TODO: check if 0-360 -> -180-180 is correct
896 }
897 yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
898 M_PI; // TODO: check if 0-360 -> -180-180 is correct
899 q.setRPY(roll, pitch, yaw);
900
901 return tf2::toMsg(q);
902}
903
910inline double getYawOfPerceivedObject(const PerceivedObject &object) {
911 gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
912 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
913 double roll, pitch, yaw;
914 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
915 return yaw;
916}
917
924inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
925 gm::Pose pose;
926 pose.position = getPositionOfPerceivedObject(object);
927 pose.orientation = getOrientationOfPerceivedObject(object);
928 return pose;
929}
930
938inline int16_t getYawRateOfPerceivedObject(const PerceivedObject &object) {
939 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
940 return object.z_angular_velocity.value.value;
941}
942
949inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }
950
957inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
958 return double(velocity.confidence.value) / 100.0;
959}
960
968inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
969 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
970 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
971 throw std::invalid_argument("Velocity is not Cartesian");
972 }
973 gm::Vector3 velocity;
974 velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
975 velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
976 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
977 velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
978 }
979 return velocity;
980}
981
988inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
989 return double(acceleration.value.value) / 10.0;
990}
991
998inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
999 return double(acceleration.confidence.value) / 10.0;
1000}
1001
1009inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
1010 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
1011 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
1012 throw std::invalid_argument("Acceleration is not Cartesian");
1013 }
1014 gm::Vector3 acceleration;
1015 acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
1016 acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
1017 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1018 acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
1019 }
1020 return acceleration;
1021}
1022
1034inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
1035 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1036 return object.object_dimension_x.value.value;
1037}
1038
1050inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
1051 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1052 return object.object_dimension_y.value.value;
1053}
1054
1066inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
1067 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1068 return object.object_dimension_z.value.value;
1069}
1070
1080inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
1081 gm::Vector3 dimensions;
1082 dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
1083 dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
1084 dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
1085 return dimensions;
1086}
1087
1098inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
1099 const PerceivedObject &object) {
1100 gm::PointStamped utm_position;
1101 gm::PointStamped reference_position = getUTMPosition(cpm);
1102 gm::Point relative_position = getPositionOfPerceivedObject(object);
1103
1104 utm_position.header.frame_id = reference_position.header.frame_id;
1105 utm_position.point.x = reference_position.point.x + relative_position.x;
1106 utm_position.point.y = reference_position.point.y + relative_position.y;
1107 utm_position.point.z = reference_position.point.z + relative_position.z;
1108
1109 return utm_position;
1110}
1111
1112} // namespace etsi_its_cpm_ts_msgs::access
Generated by doxygen 1.12.0