etsi_its_messages v3.1.0
 
Loading...
Searching...
No Matches
Classes | Macros | Functions | Variables
cpm_ts_setters.h File Reference

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

#include <etsi_its_msgs_utils/impl/checks.h>
#include <etsi_its_msgs_utils/impl/constants.h>
#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>

Go to the source code of this file.

Classes

struct  etsi_its_cpm_ts_msgs::access::etsi_its_msgs::OneCentimeterHelper< SemiAxisLength, typename >
 
struct  etsi_its_cpm_ts_msgs::access::etsi_its_msgs::OneCentimeterHelper< SemiAxisLength, std::void_t< decltype(SemiAxisLength::ONE_CENTIMETER)> >
 

Functions

template<typename T1, typename T2>
void etsi_its_cpm_ts_msgs::access::throwIfOutOfRange (const T1 &val, const T2 &min, const T2 &max, const std::string val_desc)
 Throws an exception if a given value is out of a defined range.
 
void etsi_its_cpm_ts_msgs::access::throwIfNotPresent (const bool is_present, const std::string val_desc)
 Throws an exception if the given value is not present.
 
uint16_t etsi_its_cpm_ts_msgs::access::etsi_its_msgs::getLeapSecondInsertionsSince2004 (const uint64_t unix_seconds)
 Get the leap second insertions since 2004 for given unix seconds.
 
void etsi_its_cpm_ts_msgs::access::setTimestampITS (TimestampIts &timestamp_its, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin() ->second)
 Set the TimestampITS object.
 
void etsi_its_cpm_ts_msgs::access::setLatitude (Latitude &latitude, const double deg)
 Set the Latitude object.
 
void etsi_its_cpm_ts_msgs::access::setLongitude (Longitude &longitude, const double deg)
 Set the Longitude object.
 
void etsi_its_cpm_ts_msgs::access::setAltitudeValue (AltitudeValue &altitude, const double value)
 Set the AltitudeValue object.
 
void etsi_its_cpm_ts_msgs::access::setAltitude (Altitude &altitude, const double value)
 Set the Altitude object.
 
void etsi_its_cpm_ts_msgs::access::setSpeedValue (SpeedValue &speed, const double value)
 Set the SpeedValue object.
 
void etsi_its_cpm_ts_msgs::access::setSpeedConfidence (SpeedConfidence &speed_confidence, const double value)
 Set the Speed Confidence object.
 
void etsi_its_cpm_ts_msgs::access::setSpeed (Speed &speed, const double value, const double confidence=std::numeric_limits< double >::infinity())
 Set the Speed object.
 
template<typename AccelerationConfidence>
void etsi_its_cpm_ts_msgs::access::setAccelerationConfidence (AccelerationConfidence &accel_confidence, const double value)
 Set the Acceleration Confidence object.
 
template<typename T>
void etsi_its_cpm_ts_msgs::access::setReferencePosition (T &ref_position, const double latitude, const double longitude, const double altitude=AltitudeValue::UNAVAILABLE)
 Sets the reference position in the given ReferencePostion object.
 
template<typename T>
void etsi_its_cpm_ts_msgs::access::setFromUTMPosition (T &reference_position, const gm::PointStamped &utm_position, const int zone, const bool northp)
 Set the ReferencePosition from a given UTM-Position.
 
template<typename HeadingValue>
void etsi_its_cpm_ts_msgs::access::setHeadingValue (HeadingValue &heading, const double value)
 Set the HeadingValue object.
 
template<typename HeadingConfidence>
void etsi_its_cpm_ts_msgs::access::setHeadingConfidence (HeadingConfidence &heading_confidence, const double value)
 Set the Heading Confidence object.
 
template<typename Heading, typename HeadingConfidence = decltype(Heading::heading_confidence)>
void etsi_its_cpm_ts_msgs::access::setHeadingCDD (Heading &heading, const double value, double confidence=std::numeric_limits< double >::infinity())
 Set the Heading object.
 
template<typename SemiAxisLength>
void etsi_its_cpm_ts_msgs::access::setSemiAxis (SemiAxisLength &semi_axis_length, const double length)
 Set the Semi Axis length.
 
template<typename PosConfidenceEllipse>
void etsi_its_cpm_ts_msgs::access::setPosConfidenceEllipse (PosConfidenceEllipse &position_confidence_ellipse, const double semi_major_axis, const double semi_minor_axis, const double orientation)
 Set the Pos Confidence Ellipse object.
 
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::confidenceEllipseFromCovMatrix (const std::array< double, 4 > &covariance_matrix, const double object_heading)
 Gets the values needed to set a confidence ellipse from a covariance matrix.
 
std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::confidenceEllipseFromWGSCovMatrix (const std::array< double, 4 > &covariance_matrix)
 Gets the values needed to set a confidence ellipse from a covariance matrix.
 
template<typename PosConfidenceEllipse>
void etsi_its_cpm_ts_msgs::access::setPosConfidenceEllipse (PosConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix, const double object_heading)
 Set the Pos Confidence Ellipse object.
 
template<typename PosConfidenceEllipse>
void etsi_its_cpm_ts_msgs::access::setWGSPosConfidenceEllipse (PosConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix)
 Set the Pos Confidence Ellipse object.
 
void etsi_its_cpm_ts_msgs::access::setStationId (StationId &station_id, const uint32_t id_value)
 Set the Station Id object.
 
void etsi_its_cpm_ts_msgs::access::setItsPduHeader (ItsPduHeader &header, const uint8_t message_id, const uint32_t station_id, const uint8_t protocol_version=0)
 Set the Its Pdu Header object.
 
void etsi_its_cpm_ts_msgs::access::setStationType (TrafficParticipantType &station_type, const uint8_t value)
 Set the Station Type.
 
void etsi_its_cpm_ts_msgs::access::setLongitudinalAccelerationValue (AccelerationValue &accel, const double value)
 Set the LongitudinalAccelerationValue object.
 
void etsi_its_cpm_ts_msgs::access::setLongitudinalAcceleration (AccelerationComponent &accel, const double value, const double confidence)
 Set the LongitudinalAcceleration object.
 
void etsi_its_cpm_ts_msgs::access::setLateralAccelerationValue (AccelerationValue &accel, const double value)
 Set the LateralAccelerationValue object.
 
void etsi_its_cpm_ts_msgs::access::setLateralAcceleration (AccelerationComponent &accel, const double value, const double confidence)
 Set the LateralAcceleration object.
 
template<typename PositionConfidenceEllipse, typename Wgs84AngleValue = decltype(PositionConfidenceEllipse::semi_major_axis_orientation)>
void etsi_its_cpm_ts_msgs::access::setPositionConfidenceEllipse (PositionConfidenceEllipse &position_confidence_ellipse, const double semi_major_axis, const double semi_minor_axis, const double orientation)
 Set the Position Confidence Ellipse object.
 
template<typename PositionConfidenceEllipse>
void etsi_its_cpm_ts_msgs::access::setPositionConfidenceEllipse (PositionConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix, const double object_heading)
 Set the Position Confidence Ellipse object.
 
template<typename PositionConfidenceEllipse>
void etsi_its_cpm_ts_msgs::access::setWGSPositionConfidenceEllipse (PositionConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix)
 Set the Position Confidence Ellipse object.
 
void etsi_its_cpm_ts_msgs::access::setItsPduHeader (CollectivePerceptionMessage &cpm, const uint32_t station_id, const uint8_t protocol_version=0)
 Sets the ITS PDU header of a CPM.
 
void etsi_its_cpm_ts_msgs::access::setReferenceTime (CollectivePerceptionMessage &cpm, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin() ->second)
 Sets the reference time in a CPM.
 
void etsi_its_cpm_ts_msgs::access::setReferencePosition (CollectivePerceptionMessage &cpm, const double latitude, const double longitude, const double altitude=AltitudeValue::UNAVAILABLE)
 Set the ReferencePositionWithConfidence for a CPM TS.
 
void etsi_its_cpm_ts_msgs::access::setFromUTMPosition (CollectivePerceptionMessage &cpm, const gm::PointStamped &utm_position, const int &zone, const bool &northp)
 Set the ReferencePosition of a CPM from a given UTM-Position.
 
void etsi_its_cpm_ts_msgs::access::setIdOfPerceivedObject (PerceivedObject &object, const uint16_t id)
 Set the ID of a PerceivedObject.
 
void etsi_its_cpm_ts_msgs::access::setMeasurementDeltaTimeOfPerceivedObject (PerceivedObject &object, const int16_t delta_time=0)
 Sets the measurement delta time of a PerceivedObject.
 
void etsi_its_cpm_ts_msgs::access::setCartesianCoordinateWithConfidence (CartesianCoordinateWithConfidence &coordinate, const double value, const double confidence=std::numeric_limits< double >::infinity())
 Sets the value and confidence of a CartesianCoordinateWithConfidence object.
 
void etsi_its_cpm_ts_msgs::access::setPositionOfPerceivedObject (PerceivedObject &object, const gm::Point &point, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
 Sets the position of a perceived object (relative to the CPM's reference position).
 
void etsi_its_cpm_ts_msgs::access::setUTMPositionOfPerceivedObject (CollectivePerceptionMessage &cpm, PerceivedObject &object, const gm::PointStamped &utm_position, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
 Sets the position of a perceived object based on a UTM position.
 
void etsi_its_cpm_ts_msgs::access::setVelocityComponent (VelocityComponent &velocity, const double value, const double confidence=std::numeric_limits< double >::infinity())
 Sets the value and confidence of a VelocityComponent.
 
void etsi_its_cpm_ts_msgs::access::setVelocityOfPerceivedObject (PerceivedObject &object, const gm::Vector3 &cartesian_velocity, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
 
void etsi_its_cpm_ts_msgs::access::setAccelerationComponent (AccelerationComponent &acceleration, const double value, const double confidence=std::numeric_limits< double >::infinity())
 Sets the value and confidence of a AccelerationComponent.
 
void etsi_its_cpm_ts_msgs::access::setAccelerationOfPerceivedObject (PerceivedObject &object, const gm::Vector3 &cartesian_acceleration, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
 Sets the acceleration of a perceived object.
 
void etsi_its_cpm_ts_msgs::access::setYawOfPerceivedObject (PerceivedObject &object, const double yaw, double yaw_std=std::numeric_limits< double >::infinity())
 Sets the yaw angle of a perceived object.
 
void etsi_its_cpm_ts_msgs::access::setYawRateOfPerceivedObject (PerceivedObject &object, const double yaw_rate, double yaw_rate_std=std::numeric_limits< double >::infinity())
 Sets the yaw rate of a perceived object.
 
void etsi_its_cpm_ts_msgs::access::setObjectDimension (ObjectDimension &dimension, const double value, const double confidence=std::numeric_limits< double >::infinity())
 Sets the object dimension with the given value and confidence.
 
void etsi_its_cpm_ts_msgs::access::setXDimensionOfPerceivedObject (PerceivedObject &object, const double value, const double std=std::numeric_limits< double >::infinity())
 Sets the x-dimension of a perceived object.
 
void etsi_its_cpm_ts_msgs::access::setYDimensionOfPerceivedObject (PerceivedObject &object, const double value, const double std=std::numeric_limits< double >::infinity())
 Sets the y-dimension of a perceived object.
 
void etsi_its_cpm_ts_msgs::access::setZDimensionOfPerceivedObject (PerceivedObject &object, const double value, const double std=std::numeric_limits< double >::infinity())
 Sets the z-dimension of a perceived object.
 
void etsi_its_cpm_ts_msgs::access::setDimensionsOfPerceivedObject (PerceivedObject &object, const gm::Vector3 &dimensions, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
 Sets all dimensions of a perceived object.
 
void etsi_its_cpm_ts_msgs::access::initPerceivedObject (PerceivedObject &object, const gm::Point &point, const int16_t delta_time=0)
 Initializes a PerceivedObject with the given point and delta time.
 
void etsi_its_cpm_ts_msgs::access::initPerceivedObjectWithUTMPosition (CollectivePerceptionMessage &cpm, PerceivedObject &object, const gm::PointStamped &point, const int16_t delta_time=0)
 Initializes a PerceivedObject with the given point (utm-position) and delta time.
 
void etsi_its_cpm_ts_msgs::access::initPerceivedObjectContainer (WrappedCpmContainer &container, const uint8_t n_objects=0)
 Initializes a WrappedCpmContainer as a PerceivedObjectContainer with the given number of objects.
 
void etsi_its_cpm_ts_msgs::access::addPerceivedObjectToContainer (WrappedCpmContainer &container, const PerceivedObject &perceived_object)
 Adds a PerceivedObject to the PerceivedObjectContainer / WrappedCpmContainer.
 
void etsi_its_cpm_ts_msgs::access::addContainerToCPM (CollectivePerceptionMessage &cpm, const WrappedCpmContainer &container)
 Adds a container to the Collective Perception Message (CPM).
 
void etsi_its_cpm_ts_msgs::access::setWGSRefPosConfidence (CollectivePerceptionMessage &cpm, const std::array< double, 4 > &covariance_matrix)
 Set the confidence of the reference position.
 

Variables

const uint64_t etsi_its_cpm_ts_msgs::access::etsi_its_msgs::UNIX_SECONDS_2004 = 1072915200
 
const std::map< uint64_t, uint16_t > etsi_its_cpm_ts_msgs::access::etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004
 std::map that stores all leap second insertions since 2004 with the corresponding unix-date of the insertion
 
constexpr const double etsi_its_cpm_ts_msgs::access::etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR = 2.0
 
constexpr const double etsi_its_cpm_ts_msgs::access::etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR = 2.4477
 

Detailed Description

Setter functions for the ETSI ITS CPM (TS)

Definition in file cpm_ts_setters.h.

Function Documentation

◆ addContainerToCPM()

void etsi_its_cpm_ts_msgs::access::addContainerToCPM ( CollectivePerceptionMessage & cpm,
const WrappedCpmContainer & container )
inline

Adds a container to the Collective Perception Message (CPM).

This function adds a WrappedCpmContainer to the CPM's payload. It first checks if the current number of containers is less than the maximum allowed. If so, it appends the container to the array. Otherwise, it throws an exception.

Parameters
cpmThe Collective Perception Message to which the container will be added.
containerThe WrappedCpmContainer to be added to the CPM.
Exceptions
std::invalid_argumentif the maximum number of CPM-Containers is reached.

Definition at line 790 of file cpm_ts_setters.h.

◆ addPerceivedObjectToContainer()

void etsi_its_cpm_ts_msgs::access::addPerceivedObjectToContainer ( WrappedCpmContainer & container,
const PerceivedObject & perceived_object )
inline

Adds a PerceivedObject to the PerceivedObjectContainer / WrappedCpmContainer.

This function checks if the provided container is a PerceivedObjectContainer. If it is, the function adds the given PerceivedObject to the container's perceived_objects array and updates the number_of_perceived_objects value. If the container is not a PerceivedObjectContainer, the function throws an std::invalid_argument exception.

Parameters
containerThe WrappedCpmContainer to which the PerceivedObject will be added.
perceived_objectThe PerceivedObject to add to the container.
Exceptions
std::invalid_argumentif the container is not a PerceivedObjectContainer.

Definition at line 768 of file cpm_ts_setters.h.

804 {
805
806#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
807
817inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
818 const uint8_t protocol_version = 0) {
819 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
820}
821
833inline void setReferenceTime(
834 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
835 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
836 TimestampIts t_its;
837 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
838 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
839 cpm.payload.management_container.reference_time = t_its;
840}
841
853inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
854 const double altitude = AltitudeValue::UNAVAILABLE) {
855 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
856}
857
870inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
871 const bool& northp) {
872 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
873}
874
883inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
884 object.object_id.value = id;
885 object.object_id_is_present = true;
886}
887
899inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
900 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
901 throw std::invalid_argument("MeasurementDeltaTime out of range");
902 } else {
903 object.measurement_delta_time.value = delta_time;
904 }
905}
906
920inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
921 const double confidence = std::numeric_limits<double>::infinity()) {
922 // limit value range
923 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
924 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
925 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
926 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
927 } else {
928 coordinate.value.value = static_cast<int32_t>(value);
929 }
930
931 // limit confidence range
932 if (confidence == std::numeric_limits<double>::infinity()) {
933 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
934 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
935 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
936 } else {
937 coordinate.confidence.value = static_cast<uint16_t>(confidence);
938 }
939}
940
952inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
953 const double x_std = std::numeric_limits<double>::infinity(),
954 const double y_std = std::numeric_limits<double>::infinity(),
955 const double z_std = std::numeric_limits<double>::infinity()) {
956 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
957 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
958 if (point.z != 0.0) {
959 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
960 object.position.z_coordinate_is_present = true;
961 }
962}
963
979inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
980 const gm::PointStamped& utm_position,
981 const double x_std = std::numeric_limits<double>::infinity(),
982 const double y_std = std::numeric_limits<double>::infinity(),
983 const double z_std = std::numeric_limits<double>::infinity()) {
984 gm::PointStamped reference_position = getUTMPosition(cpm);
985 if (utm_position.header.frame_id != reference_position.header.frame_id) {
986 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
987 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
988 ")");
989 }
990 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
991 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
992 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
993 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
994 if (utm_position.point.z != 0.0) {
995 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
996 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
997 object.position.z_coordinate_is_present = true;
998 }
999}
1000
1012inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1013 const double confidence = std::numeric_limits<double>::infinity()) {
1014 // limit value range
1015 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1016 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1017 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1018 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1019 } else {
1020 velocity.value.value = static_cast<int16_t>(value);
1021 }
1022
1023 // limit confidence range
1024 if(confidence == std::numeric_limits<double>::infinity()) {
1025 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1026 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1027 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1028 } else {
1029 velocity.confidence.value = static_cast<uint8_t>(confidence);
1030 }
1031}
1032
1045inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1046 const double x_std = std::numeric_limits<double>::infinity(),
1047 const double y_std = std::numeric_limits<double>::infinity(),
1048 const double z_std = std::numeric_limits<double>::infinity()) {
1049 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1050 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1051 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1052 if (cartesian_velocity.z != 0.0) {
1053 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1054 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1055 }
1056 object.velocity_is_present = true;
1057}
1058
1070inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1071 const double confidence = std::numeric_limits<double>::infinity()) {
1072 // limit value range
1073 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1074 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1075 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1076 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1077 } else {
1078 acceleration.value.value = static_cast<int16_t>(value);
1079 }
1080
1081 // limit confidence range
1082 if(confidence == std::numeric_limits<double>::infinity()) {
1083 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1084 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1085 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1086 } else {
1087 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1088 }
1089}
1090
1103inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1104 const double x_std = std::numeric_limits<double>::infinity(),
1105 const double y_std = std::numeric_limits<double>::infinity(),
1106 const double z_std = std::numeric_limits<double>::infinity()) {
1107 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1108 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1109 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1110 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1111 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1112 if (cartesian_acceleration.z != 0.0) {
1113 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1114 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1115 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1116 }
1117 object.acceleration_is_present = true;
1118}
1119
1130inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1131 double yaw_std = std::numeric_limits<double>::infinity()) {
1132 // wrap angle to range [0, 360)
1133 double yaw_in_degrees = yaw * 180 / M_PI;
1134 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1135 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1136 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1137
1138 if(yaw_std == std::numeric_limits<double>::infinity()) {
1139 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1140 } else {
1141 yaw_std *= 180.0 / M_PI;
1142 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1143
1144 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1145 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1146 } else {
1147 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1148 }
1149 }
1150 object.angles_is_present = true;
1151}
1152
1164inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1165 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1166 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1167 // limit value range
1168 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1169 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1170 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1171 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1172 }
1173
1174 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1175
1176 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1177 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1178 } else {
1179 yaw_rate_std *= 180.0 / M_PI;
1180 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1181 // How stupid is this?!
1182 static const std::map<double, uint8_t> confidence_map = {
1183 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1184 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1185 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1186 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1187 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1188 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1189 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1190 };
1191 for(const auto& [thresh, conf_val] : confidence_map) {
1192 if (yaw_rate_std <= thresh) {
1193 object.z_angular_velocity.confidence.value = conf_val;
1194 break;
1195 }
1196 }
1197 }
1198
1199 object.z_angular_velocity_is_present = true;
1200}
1201
1216inline void setObjectDimension(ObjectDimension& dimension, const double value,
1217 const double confidence = std::numeric_limits<double>::infinity()) {
1218 // limit value range
1219 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1220 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1221 } else {
1222 dimension.value.value = static_cast<uint16_t>(value);
1223 }
1224
1225 // limit confidence range
1226 if (confidence == std::numeric_limits<double>::infinity()) {
1227 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1228 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1229 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1230 } else {
1231 dimension.confidence.value = static_cast<uint8_t>(confidence);
1232 }
1233
1234}
1235
1246inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1247 const double std = std::numeric_limits<double>::infinity()) {
1248 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1249 object.object_dimension_x_is_present = true;
1250}
1251
1262inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1263 const double std = std::numeric_limits<double>::infinity()) {
1264 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1265 object.object_dimension_y_is_present = true;
1266}
1267
1278inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1279 const double std = std::numeric_limits<double>::infinity()) {
1280 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1281 object.object_dimension_z_is_present = true;
1282}
1283
1295inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1296 const double x_std = std::numeric_limits<double>::infinity(),
1297 const double y_std = std::numeric_limits<double>::infinity(),
1298 const double z_std = std::numeric_limits<double>::infinity()) {
1299 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1300 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1301 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1302}
1303
1313inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1314 setPositionOfPerceivedObject(object, point);
1315 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1316}
1317
1330inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1331 const gm::PointStamped& point, const int16_t delta_time = 0) {
1332 setUTMPositionOfPerceivedObject(cpm, object, point);
1333 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1334}
1335
1345inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1346 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1347 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1348}
1349
1363inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1364 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1365 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1366 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1367 container.container_data_perceived_object_container.perceived_objects.array.size();
1368 } else {
1369 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1370 }
1371}
1372
1385inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1386 // check for maximum number of containers
1387 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1388 cpm.payload.cpm_containers.value.array.push_back(container);
1389 } else {
1390 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1391 }
1392}
1393
1402inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1403 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1404 covariance_matrix);
1405}
1406
1407} // namespace etsi_its_cpm_ts_msgs::access
setReferencePosition
void setReferencePosition(CAM &cam, const double latitude, const double longitude, const double altitude=AltitudeValue::UNAVAILABLE)
Set the ReferencePosition for a CAM.
Definition cam_setters_common.h:197
setTimestampITS
void setTimestampITS(TimestampIts &timestamp_its, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin() ->second)
Set the TimestampITS object.
Definition cdd_setters_common.h:52
cdd_v2-1-1_setters.h
Setter functions for the ETSI ITS Common Data Dictionary (CDD) v2.1.1.
throwIfOutOfRange
void throwIfOutOfRange(const T1 &val, const T2 &min, const T2 &max, const std::string val_desc)
Throws an exception if a given value is out of a defined range.
Definition checks.h:46
etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004
const std::map< uint64_t, uint16_t > LEAP_SECOND_INSERTIONS_SINCE_2004
std::map that stores all leap second insertions since 2004 with the corresponding unix-date of the in...
Definition constants.h:45
etsi_its_cpm_ts_msgs::access::getUTMPosition
gm::PointStamped getUTMPosition(const T &reference_position, int &zone, bool &northp)
Get the UTM Position defined by the given ReferencePosition.
Definition cpm_ts_getters.h:103
etsi_its_cpm_ts_msgs::access::setVelocityOfPerceivedObject
void setVelocityOfPerceivedObject(PerceivedObject &object, const gm::Vector3 &cartesian_velocity, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
Definition cpm_ts_setters.h:450
etsi_its_cpm_ts_msgs::access::setWGSPosConfidenceEllipse
void setWGSPosConfidenceEllipse(PosConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix)
Set the Pos Confidence Ellipse object.
Definition cpm_ts_setters.h:448
etsi_its_cpm_ts_msgs::access::setAccelerationComponent
void setAccelerationComponent(AccelerationComponent &acceleration, const double value, const double confidence=std::numeric_limits< double >::infinity())
Sets the value and confidence of a AccelerationComponent.
Definition cpm_ts_setters.h:475
etsi_its_cpm_ts_msgs::access::setAccelerationOfPerceivedObject
void setAccelerationOfPerceivedObject(PerceivedObject &object, const gm::Vector3 &cartesian_acceleration, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
Sets the acceleration of a perceived object.
Definition cpm_ts_setters.h:508
etsi_its_cpm_ts_msgs::access::initPerceivedObjectContainer
void initPerceivedObjectContainer(WrappedCpmContainer &container, const uint8_t n_objects=0)
Initializes a WrappedCpmContainer as a PerceivedObjectContainer with the given number of objects.
Definition cpm_ts_setters.h:750
etsi_its_cpm_ts_msgs::access::setDimensionsOfPerceivedObject
void setDimensionsOfPerceivedObject(PerceivedObject &object, const gm::Vector3 &dimensions, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
Sets all dimensions of a perceived object.
Definition cpm_ts_setters.h:700
etsi_its_cpm_ts_msgs::access::setYDimensionOfPerceivedObject
void setYDimensionOfPerceivedObject(PerceivedObject &object, const double value, const double std=std::numeric_limits< double >::infinity())
Sets the y-dimension of a perceived object.
Definition cpm_ts_setters.h:667
etsi_its_cpm_ts_msgs::access::setWGSRefPosConfidence
void setWGSRefPosConfidence(CollectivePerceptionMessage &cpm, const std::array< double, 4 > &covariance_matrix)
Set the confidence of the reference position.
Definition cpm_ts_setters.h:807
etsi_its_cpm_ts_msgs::access::setVelocityComponent
void setVelocityComponent(VelocityComponent &velocity, const double value, const double confidence=std::numeric_limits< double >::infinity())
Sets the value and confidence of a VelocityComponent.
Definition cpm_ts_setters.h:417
etsi_its_cpm_ts_msgs::access::setYawRateOfPerceivedObject
void setYawRateOfPerceivedObject(PerceivedObject &object, const double yaw_rate, double yaw_rate_std=std::numeric_limits< double >::infinity())
Sets the yaw rate of a perceived object.
Definition cpm_ts_setters.h:569
etsi_its_cpm_ts_msgs::access::setItsPduHeader
void setItsPduHeader(ItsPduHeader &header, const uint8_t message_id, const uint32_t station_id, const uint8_t protocol_version=0)
Set the Its Pdu Header object.
Definition cpm_ts_setters.h:85
etsi_its_cpm_ts_msgs::access::setFromUTMPosition
void setFromUTMPosition(T &reference_position, const gm::PointStamped &utm_position, const int zone, const bool northp)
Set the ReferencePosition from a given UTM-Position.
Definition cpm_ts_setters.h:268
etsi_its_cpm_ts_msgs::access::setYawOfPerceivedObject
void setYawOfPerceivedObject(PerceivedObject &object, const double yaw, double yaw_std=std::numeric_limits< double >::infinity())
Sets the yaw angle of a perceived object.
Definition cpm_ts_setters.h:535
etsi_its_cpm_ts_msgs::access::addPerceivedObjectToContainer
void addPerceivedObjectToContainer(WrappedCpmContainer &container, const PerceivedObject &perceived_object)
Adds a PerceivedObject to the PerceivedObjectContainer / WrappedCpmContainer.
Definition cpm_ts_setters.h:768
etsi_its_cpm_ts_msgs::access::setUTMPositionOfPerceivedObject
void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage &cpm, PerceivedObject &object, const gm::PointStamped &utm_position, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
Sets the position of a perceived object based on a UTM position.
Definition cpm_ts_setters.h:384
etsi_its_cpm_ts_msgs::access::addContainerToCPM
void addContainerToCPM(CollectivePerceptionMessage &cpm, const WrappedCpmContainer &container)
Adds a container to the Collective Perception Message (CPM).
Definition cpm_ts_setters.h:790
etsi_its_cpm_ts_msgs::access::setIdOfPerceivedObject
void setIdOfPerceivedObject(PerceivedObject &object, const uint16_t id)
Set the ID of a PerceivedObject.
Definition cpm_ts_setters.h:288
etsi_its_cpm_ts_msgs::access::setObjectDimension
void setObjectDimension(ObjectDimension &dimension, const double value, const double confidence=std::numeric_limits< double >::infinity())
Sets the object dimension with the given value and confidence.
Definition cpm_ts_setters.h:621
etsi_its_cpm_ts_msgs::access::setXDimensionOfPerceivedObject
void setXDimensionOfPerceivedObject(PerceivedObject &object, const double value, const double std=std::numeric_limits< double >::infinity())
Sets the x-dimension of a perceived object.
Definition cpm_ts_setters.h:651
etsi_its_cpm_ts_msgs::access::setZDimensionOfPerceivedObject
void setZDimensionOfPerceivedObject(PerceivedObject &object, const double value, const double std=std::numeric_limits< double >::infinity())
Sets the z-dimension of a perceived object.
Definition cpm_ts_setters.h:683
etsi_its_cpm_ts_msgs::access::setCartesianCoordinateWithConfidence
void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence &coordinate, const double value, const double confidence=std::numeric_limits< double >::infinity())
Sets the value and confidence of a CartesianCoordinateWithConfidence object.
Definition cpm_ts_setters.h:325
etsi_its_cpm_ts_msgs::access::setPositionOfPerceivedObject
void setPositionOfPerceivedObject(PerceivedObject &object, const gm::Point &point, const double x_std=std::numeric_limits< double >::infinity(), const double y_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
Sets the position of a perceived object (relative to the CPM's reference position).
Definition cpm_ts_setters.h:357
etsi_its_cpm_ts_msgs::access::setMeasurementDeltaTimeOfPerceivedObject
void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject &object, const int16_t delta_time=0)
Sets the measurement delta time of a PerceivedObject.
Definition cpm_ts_setters.h:304
etsi_its_cpm_ts_msgs::access::initPerceivedObjectWithUTMPosition
void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage &cpm, PerceivedObject &object, const gm::PointStamped &point, const int16_t delta_time=0)
Initializes a PerceivedObject with the given point (utm-position) and delta time.
Definition cpm_ts_setters.h:735
etsi_its_cpm_ts_msgs::access::initPerceivedObject
void initPerceivedObject(PerceivedObject &object, const gm::Point &point, const int16_t delta_time=0)
Initializes a PerceivedObject with the given point and delta time.
Definition cpm_ts_setters.h:718

◆ confidenceEllipseFromCovMatrix()

std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::confidenceEllipseFromCovMatrix ( const std::array< double, 4 > & covariance_matrix,
const double object_heading )
inline

Gets the values needed to set a confidence ellipse from a covariance matrix.

Parameters
covariance_matrixThe four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the object (x = longitudinal, y = lateral)
object_headingThe heading of the object in rad, with respect to WGS84
Returns
std::tuple<double, double, double> semi_major_axis [m], semi_minor_axis [m], orientation [deg], with respect to WGS84

Definition at line 376 of file cpm_ts_setters.h.

377 {
378 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
379 } else {
380 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
381 }
382 }
383 object.angles_is_present = true;
384}
385
397inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
398 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
399 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
400 // limit value range
401 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
402 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
403 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
404 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
405 }
406

◆ confidenceEllipseFromWGSCovMatrix()

std::tuple< double, double, double > etsi_its_cpm_ts_msgs::access::confidenceEllipseFromWGSCovMatrix ( const std::array< double, 4 > & covariance_matrix)
inline

Gets the values needed to set a confidence ellipse from a covariance matrix.

Parameters
covariance_matrixThe four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the WGS axes (x = North, y = East)
object_headingThe heading of the object in rad, with respect to WGS84
Returns
std::tuple<double, double, double> semi_major_axis [m], semi_minor_axis [m], orientation [deg], with respect to WGS84

Definition at line 417 of file cpm_ts_setters.h.

417 {2.0, AngularSpeedConfidence::DEG_SEC_02},
418 {5.0, AngularSpeedConfidence::DEG_SEC_05},
419 {10.0, AngularSpeedConfidence::DEG_SEC_10},
420 {20.0, AngularSpeedConfidence::DEG_SEC_20},
421 {50.0, AngularSpeedConfidence::DEG_SEC_50},

◆ getLeapSecondInsertionsSince2004()

uint16_t etsi_its_cpm_ts_msgs::access::etsi_its_msgs::getLeapSecondInsertionsSince2004 ( const uint64_t unix_seconds)
inline

Get the leap second insertions since 2004 for given unix seconds.

Parameters
unix_secondsthe current unix seconds for that the leap second insertions since 2004 shall be provided
Returns
uint16_t the number of leap second insertions since 2004 for unix_seconds

Definition at line 61 of file cpm_ts_setters.h.

68 {
69 TimestampIts t_its;

◆ initPerceivedObject()

void etsi_its_cpm_ts_msgs::access::initPerceivedObject ( PerceivedObject & object,
const gm::Point & point,
const int16_t delta_time = 0 )
inline

Initializes a PerceivedObject with the given point and delta time.

This function sets the position and measurement delta time of the PerceivedObject.

Parameters
objectThe PerceivedObject to be initialized.
pointThe position of the PerceivedObject relative to the CPM's reference position in meters.
delta_timeThe measurement delta time of the PerceivedObject in milliseconds (default = 0).

Definition at line 718 of file cpm_ts_setters.h.

754 {
755
756#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
757
767inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
768 const uint8_t protocol_version = 0) {
769 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
770}
771
783inline void setReferenceTime(
784 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
785 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
786 TimestampIts t_its;
787 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
788 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
789 cpm.payload.management_container.reference_time = t_its;
790}
791
803inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
804 const double altitude = AltitudeValue::UNAVAILABLE) {
805 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
806}
807
820inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
821 const bool& northp) {
822 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
823}
824
833inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
834 object.object_id.value = id;
835 object.object_id_is_present = true;
836}
837
849inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
850 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
851 throw std::invalid_argument("MeasurementDeltaTime out of range");
852 } else {
853 object.measurement_delta_time.value = delta_time;
854 }
855}
856
870inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
871 const double confidence = std::numeric_limits<double>::infinity()) {
872 // limit value range
873 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
874 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
875 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
876 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
877 } else {
878 coordinate.value.value = static_cast<int32_t>(value);
879 }
880
881 // limit confidence range
882 if (confidence == std::numeric_limits<double>::infinity()) {
883 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
884 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
885 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
886 } else {
887 coordinate.confidence.value = static_cast<uint16_t>(confidence);
888 }
889}
890
902inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
903 const double x_std = std::numeric_limits<double>::infinity(),
904 const double y_std = std::numeric_limits<double>::infinity(),
905 const double z_std = std::numeric_limits<double>::infinity()) {
906 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
907 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
908 if (point.z != 0.0) {
909 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
910 object.position.z_coordinate_is_present = true;
911 }
912}
913
929inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
930 const gm::PointStamped& utm_position,
931 const double x_std = std::numeric_limits<double>::infinity(),
932 const double y_std = std::numeric_limits<double>::infinity(),
933 const double z_std = std::numeric_limits<double>::infinity()) {
934 gm::PointStamped reference_position = getUTMPosition(cpm);
935 if (utm_position.header.frame_id != reference_position.header.frame_id) {
936 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
937 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
938 ")");
939 }
940 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
941 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
942 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
943 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
944 if (utm_position.point.z != 0.0) {
945 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
946 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
947 object.position.z_coordinate_is_present = true;
948 }
949}
950
962inline void setVelocityComponent(VelocityComponent& velocity, const double value,
963 const double confidence = std::numeric_limits<double>::infinity()) {
964 // limit value range
965 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
966 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
967 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
968 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
969 } else {
970 velocity.value.value = static_cast<int16_t>(value);
971 }
972
973 // limit confidence range
974 if(confidence == std::numeric_limits<double>::infinity()) {
975 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
976 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
977 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
978 } else {
979 velocity.confidence.value = static_cast<uint8_t>(confidence);
980 }
981}
982
995inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
996 const double x_std = std::numeric_limits<double>::infinity(),
997 const double y_std = std::numeric_limits<double>::infinity(),
998 const double z_std = std::numeric_limits<double>::infinity()) {
999 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1000 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1001 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1002 if (cartesian_velocity.z != 0.0) {
1003 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1004 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1005 }
1006 object.velocity_is_present = true;
1007}
1008
1020inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1021 const double confidence = std::numeric_limits<double>::infinity()) {
1022 // limit value range
1023 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1024 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1025 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1026 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1027 } else {
1028 acceleration.value.value = static_cast<int16_t>(value);
1029 }
1030
1031 // limit confidence range
1032 if(confidence == std::numeric_limits<double>::infinity()) {
1033 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1034 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1035 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1036 } else {
1037 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1038 }
1039}
1040
1053inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1054 const double x_std = std::numeric_limits<double>::infinity(),
1055 const double y_std = std::numeric_limits<double>::infinity(),
1056 const double z_std = std::numeric_limits<double>::infinity()) {
1057 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1058 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1059 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1060 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1061 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1062 if (cartesian_acceleration.z != 0.0) {
1063 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1064 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1065 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1066 }
1067 object.acceleration_is_present = true;
1068}
1069
1080inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1081 double yaw_std = std::numeric_limits<double>::infinity()) {
1082 // wrap angle to range [0, 360)
1083 double yaw_in_degrees = yaw * 180 / M_PI;
1084 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1085 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1086 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1087
1088 if(yaw_std == std::numeric_limits<double>::infinity()) {
1089 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1090 } else {
1091 yaw_std *= 180.0 / M_PI;
1092 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1093
1094 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1095 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1096 } else {
1097 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1098 }
1099 }
1100 object.angles_is_present = true;
1101}
1102
1114inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1115 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1116 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1117 // limit value range
1118 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1119 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1120 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1121 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1122 }
1123
1124 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1125
1126 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1127 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1128 } else {
1129 yaw_rate_std *= 180.0 / M_PI;
1130 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1131 // How stupid is this?!
1132 static const std::map<double, uint8_t> confidence_map = {
1133 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1134 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1135 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1136 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1137 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1138 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1139 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1140 };
1141 for(const auto& [thresh, conf_val] : confidence_map) {
1142 if (yaw_rate_std <= thresh) {
1143 object.z_angular_velocity.confidence.value = conf_val;
1144 break;
1145 }
1146 }
1147 }
1148
1149 object.z_angular_velocity_is_present = true;
1150}
1151
1166inline void setObjectDimension(ObjectDimension& dimension, const double value,
1167 const double confidence = std::numeric_limits<double>::infinity()) {
1168 // limit value range
1169 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1170 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1171 } else {
1172 dimension.value.value = static_cast<uint16_t>(value);
1173 }
1174
1175 // limit confidence range
1176 if (confidence == std::numeric_limits<double>::infinity()) {
1177 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1178 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1179 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1180 } else {
1181 dimension.confidence.value = static_cast<uint8_t>(confidence);
1182 }
1183
1184}
1185
1196inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1197 const double std = std::numeric_limits<double>::infinity()) {
1198 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1199 object.object_dimension_x_is_present = true;
1200}
1201
1212inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1213 const double std = std::numeric_limits<double>::infinity()) {
1214 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1215 object.object_dimension_y_is_present = true;
1216}
1217
1228inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1229 const double std = std::numeric_limits<double>::infinity()) {
1230 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1231 object.object_dimension_z_is_present = true;
1232}
1233
1245inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1246 const double x_std = std::numeric_limits<double>::infinity(),
1247 const double y_std = std::numeric_limits<double>::infinity(),
1248 const double z_std = std::numeric_limits<double>::infinity()) {
1249 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1250 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1251 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1252}
1253
1263inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1264 setPositionOfPerceivedObject(object, point);
1265 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1266}
1267
1280inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1281 const gm::PointStamped& point, const int16_t delta_time = 0) {
1282 setUTMPositionOfPerceivedObject(cpm, object, point);
1283 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1284}
1285
1295inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1296 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1297 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1298}
1299
1313inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1314 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1315 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1316 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1317 container.container_data_perceived_object_container.perceived_objects.array.size();
1318 } else {
1319 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1320 }
1321}
1322
1335inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1336 // check for maximum number of containers
1337 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1338 cpm.payload.cpm_containers.value.array.push_back(container);
1339 } else {
1340 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1341 }
1342}
1343
1352inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1353 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1354 covariance_matrix);
1355}
1356
1357} // namespace etsi_its_cpm_ts_msgs::access

◆ initPerceivedObjectContainer()

void etsi_its_cpm_ts_msgs::access::initPerceivedObjectContainer ( WrappedCpmContainer & container,
const uint8_t n_objects = 0 )
inline

Initializes a WrappedCpmContainer as a PerceivedObjectContainer with the given number of objects.

This function sets the container ID to CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER and initializes the number of perceived objects in the container to the specified value.

Parameters
containerA reference to the WrappedCpmContainer to be initialized as a PerceivedObjectContainer.
n_objectsThe number of perceived objects to initialize in the container. Default is 0.

Definition at line 750 of file cpm_ts_setters.h.

786 {
787
788#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
789
799inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
800 const uint8_t protocol_version = 0) {
801 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
802}
803
815inline void setReferenceTime(
816 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
817 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
818 TimestampIts t_its;
819 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
820 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
821 cpm.payload.management_container.reference_time = t_its;
822}
823
835inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
836 const double altitude = AltitudeValue::UNAVAILABLE) {
837 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
838}
839
852inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
853 const bool& northp) {
854 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
855}
856
865inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
866 object.object_id.value = id;
867 object.object_id_is_present = true;
868}
869
881inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
882 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
883 throw std::invalid_argument("MeasurementDeltaTime out of range");
884 } else {
885 object.measurement_delta_time.value = delta_time;
886 }
887}
888
902inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
903 const double confidence = std::numeric_limits<double>::infinity()) {
904 // limit value range
905 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
906 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
907 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
908 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
909 } else {
910 coordinate.value.value = static_cast<int32_t>(value);
911 }
912
913 // limit confidence range
914 if (confidence == std::numeric_limits<double>::infinity()) {
915 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
916 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
917 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
918 } else {
919 coordinate.confidence.value = static_cast<uint16_t>(confidence);
920 }
921}
922
934inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
935 const double x_std = std::numeric_limits<double>::infinity(),
936 const double y_std = std::numeric_limits<double>::infinity(),
937 const double z_std = std::numeric_limits<double>::infinity()) {
938 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
939 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
940 if (point.z != 0.0) {
941 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
942 object.position.z_coordinate_is_present = true;
943 }
944}
945
961inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
962 const gm::PointStamped& utm_position,
963 const double x_std = std::numeric_limits<double>::infinity(),
964 const double y_std = std::numeric_limits<double>::infinity(),
965 const double z_std = std::numeric_limits<double>::infinity()) {
966 gm::PointStamped reference_position = getUTMPosition(cpm);
967 if (utm_position.header.frame_id != reference_position.header.frame_id) {
968 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
969 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
970 ")");
971 }
972 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
973 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
974 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
975 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
976 if (utm_position.point.z != 0.0) {
977 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
978 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
979 object.position.z_coordinate_is_present = true;
980 }
981}
982
994inline void setVelocityComponent(VelocityComponent& velocity, const double value,
995 const double confidence = std::numeric_limits<double>::infinity()) {
996 // limit value range
997 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
998 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
999 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1000 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1001 } else {
1002 velocity.value.value = static_cast<int16_t>(value);
1003 }
1004
1005 // limit confidence range
1006 if(confidence == std::numeric_limits<double>::infinity()) {
1007 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1008 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1009 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1010 } else {
1011 velocity.confidence.value = static_cast<uint8_t>(confidence);
1012 }
1013}
1014
1027inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1028 const double x_std = std::numeric_limits<double>::infinity(),
1029 const double y_std = std::numeric_limits<double>::infinity(),
1030 const double z_std = std::numeric_limits<double>::infinity()) {
1031 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1032 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1033 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1034 if (cartesian_velocity.z != 0.0) {
1035 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1036 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1037 }
1038 object.velocity_is_present = true;
1039}
1040
1052inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1053 const double confidence = std::numeric_limits<double>::infinity()) {
1054 // limit value range
1055 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1056 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1057 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1058 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1059 } else {
1060 acceleration.value.value = static_cast<int16_t>(value);
1061 }
1062
1063 // limit confidence range
1064 if(confidence == std::numeric_limits<double>::infinity()) {
1065 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1066 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1067 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1068 } else {
1069 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1070 }
1071}
1072
1085inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1086 const double x_std = std::numeric_limits<double>::infinity(),
1087 const double y_std = std::numeric_limits<double>::infinity(),
1088 const double z_std = std::numeric_limits<double>::infinity()) {
1089 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1090 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1091 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1092 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1093 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1094 if (cartesian_acceleration.z != 0.0) {
1095 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1096 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1097 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1098 }
1099 object.acceleration_is_present = true;
1100}
1101
1112inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1113 double yaw_std = std::numeric_limits<double>::infinity()) {
1114 // wrap angle to range [0, 360)
1115 double yaw_in_degrees = yaw * 180 / M_PI;
1116 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1117 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1118 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1119
1120 if(yaw_std == std::numeric_limits<double>::infinity()) {
1121 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1122 } else {
1123 yaw_std *= 180.0 / M_PI;
1124 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1125
1126 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1127 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1128 } else {
1129 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1130 }
1131 }
1132 object.angles_is_present = true;
1133}
1134
1146inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1147 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1148 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1149 // limit value range
1150 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1151 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1152 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1153 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1154 }
1155
1156 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1157
1158 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1159 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1160 } else {
1161 yaw_rate_std *= 180.0 / M_PI;
1162 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1163 // How stupid is this?!
1164 static const std::map<double, uint8_t> confidence_map = {
1165 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1166 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1167 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1168 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1169 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1170 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1171 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1172 };
1173 for(const auto& [thresh, conf_val] : confidence_map) {
1174 if (yaw_rate_std <= thresh) {
1175 object.z_angular_velocity.confidence.value = conf_val;
1176 break;
1177 }
1178 }
1179 }
1180
1181 object.z_angular_velocity_is_present = true;
1182}
1183
1198inline void setObjectDimension(ObjectDimension& dimension, const double value,
1199 const double confidence = std::numeric_limits<double>::infinity()) {
1200 // limit value range
1201 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1202 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1203 } else {
1204 dimension.value.value = static_cast<uint16_t>(value);
1205 }
1206
1207 // limit confidence range
1208 if (confidence == std::numeric_limits<double>::infinity()) {
1209 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1210 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1211 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1212 } else {
1213 dimension.confidence.value = static_cast<uint8_t>(confidence);
1214 }
1215
1216}
1217
1228inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1229 const double std = std::numeric_limits<double>::infinity()) {
1230 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1231 object.object_dimension_x_is_present = true;
1232}
1233
1244inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1245 const double std = std::numeric_limits<double>::infinity()) {
1246 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1247 object.object_dimension_y_is_present = true;
1248}
1249
1260inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1261 const double std = std::numeric_limits<double>::infinity()) {
1262 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1263 object.object_dimension_z_is_present = true;
1264}
1265
1277inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1278 const double x_std = std::numeric_limits<double>::infinity(),
1279 const double y_std = std::numeric_limits<double>::infinity(),
1280 const double z_std = std::numeric_limits<double>::infinity()) {
1281 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1282 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1283 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1284}
1285
1295inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1296 setPositionOfPerceivedObject(object, point);
1297 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1298}
1299
1312inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1313 const gm::PointStamped& point, const int16_t delta_time = 0) {
1314 setUTMPositionOfPerceivedObject(cpm, object, point);
1315 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1316}
1317
1327inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1328 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1329 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1330}
1331
1345inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1346 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1347 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1348 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1349 container.container_data_perceived_object_container.perceived_objects.array.size();
1350 } else {
1351 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1352 }
1353}
1354
1367inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1368 // check for maximum number of containers
1369 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1370 cpm.payload.cpm_containers.value.array.push_back(container);
1371 } else {
1372 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1373 }
1374}
1375
1384inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1385 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1386 covariance_matrix);
1387}
1388
1389} // namespace etsi_its_cpm_ts_msgs::access

◆ initPerceivedObjectWithUTMPosition()

void etsi_its_cpm_ts_msgs::access::initPerceivedObjectWithUTMPosition ( CollectivePerceptionMessage & cpm,
PerceivedObject & object,
const gm::PointStamped & point,
const int16_t delta_time = 0 )
inline

Initializes a PerceivedObject with the given point (utm-position) and delta time.

This function initializes a PerceivedObject within a position and measurement delta time. It sets the position of a perceived object using the provided UTM position and the CPM's reference position. It sets the measurement delta time using the provided delta_time value.

Parameters
cpmThe CPM to get the reference position from.
objectThe PerceivedObject to be initialized.
pointThe gm::PointStamped representing the UTM position of the object including the frame_id (utm_<zone><N/S>).
delta_timeThe measurement delta time in milliseconds (default: 0).

Definition at line 735 of file cpm_ts_setters.h.

771 {
772
773#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
774
784inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
785 const uint8_t protocol_version = 0) {
786 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
787}
788
800inline void setReferenceTime(
801 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
802 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
803 TimestampIts t_its;
804 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
805 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
806 cpm.payload.management_container.reference_time = t_its;
807}
808
820inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
821 const double altitude = AltitudeValue::UNAVAILABLE) {
822 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
823}
824
837inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
838 const bool& northp) {
839 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
840}
841
850inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
851 object.object_id.value = id;
852 object.object_id_is_present = true;
853}
854
866inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
867 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
868 throw std::invalid_argument("MeasurementDeltaTime out of range");
869 } else {
870 object.measurement_delta_time.value = delta_time;
871 }
872}
873
887inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
888 const double confidence = std::numeric_limits<double>::infinity()) {
889 // limit value range
890 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
891 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
892 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
893 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
894 } else {
895 coordinate.value.value = static_cast<int32_t>(value);
896 }
897
898 // limit confidence range
899 if (confidence == std::numeric_limits<double>::infinity()) {
900 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
901 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
902 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
903 } else {
904 coordinate.confidence.value = static_cast<uint16_t>(confidence);
905 }
906}
907
919inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
920 const double x_std = std::numeric_limits<double>::infinity(),
921 const double y_std = std::numeric_limits<double>::infinity(),
922 const double z_std = std::numeric_limits<double>::infinity()) {
923 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
924 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
925 if (point.z != 0.0) {
926 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
927 object.position.z_coordinate_is_present = true;
928 }
929}
930
946inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
947 const gm::PointStamped& utm_position,
948 const double x_std = std::numeric_limits<double>::infinity(),
949 const double y_std = std::numeric_limits<double>::infinity(),
950 const double z_std = std::numeric_limits<double>::infinity()) {
951 gm::PointStamped reference_position = getUTMPosition(cpm);
952 if (utm_position.header.frame_id != reference_position.header.frame_id) {
953 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
954 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
955 ")");
956 }
957 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
958 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
959 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
960 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
961 if (utm_position.point.z != 0.0) {
962 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
963 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
964 object.position.z_coordinate_is_present = true;
965 }
966}
967
979inline void setVelocityComponent(VelocityComponent& velocity, const double value,
980 const double confidence = std::numeric_limits<double>::infinity()) {
981 // limit value range
982 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
983 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
984 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
985 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
986 } else {
987 velocity.value.value = static_cast<int16_t>(value);
988 }
989
990 // limit confidence range
991 if(confidence == std::numeric_limits<double>::infinity()) {
992 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
993 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
994 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
995 } else {
996 velocity.confidence.value = static_cast<uint8_t>(confidence);
997 }
998}
999
1012inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1013 const double x_std = std::numeric_limits<double>::infinity(),
1014 const double y_std = std::numeric_limits<double>::infinity(),
1015 const double z_std = std::numeric_limits<double>::infinity()) {
1016 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1017 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1018 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1019 if (cartesian_velocity.z != 0.0) {
1020 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1021 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1022 }
1023 object.velocity_is_present = true;
1024}
1025
1037inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1038 const double confidence = std::numeric_limits<double>::infinity()) {
1039 // limit value range
1040 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1041 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1042 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1043 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1044 } else {
1045 acceleration.value.value = static_cast<int16_t>(value);
1046 }
1047
1048 // limit confidence range
1049 if(confidence == std::numeric_limits<double>::infinity()) {
1050 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1051 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1052 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1053 } else {
1054 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1055 }
1056}
1057
1070inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1071 const double x_std = std::numeric_limits<double>::infinity(),
1072 const double y_std = std::numeric_limits<double>::infinity(),
1073 const double z_std = std::numeric_limits<double>::infinity()) {
1074 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1075 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1076 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1077 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1078 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1079 if (cartesian_acceleration.z != 0.0) {
1080 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1081 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1082 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1083 }
1084 object.acceleration_is_present = true;
1085}
1086
1097inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1098 double yaw_std = std::numeric_limits<double>::infinity()) {
1099 // wrap angle to range [0, 360)
1100 double yaw_in_degrees = yaw * 180 / M_PI;
1101 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1102 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1103 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1104
1105 if(yaw_std == std::numeric_limits<double>::infinity()) {
1106 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1107 } else {
1108 yaw_std *= 180.0 / M_PI;
1109 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1110
1111 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1112 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1113 } else {
1114 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1115 }
1116 }
1117 object.angles_is_present = true;
1118}
1119
1131inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1132 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1133 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1134 // limit value range
1135 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1136 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1137 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1138 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1139 }
1140
1141 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1142
1143 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1144 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1145 } else {
1146 yaw_rate_std *= 180.0 / M_PI;
1147 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1148 // How stupid is this?!
1149 static const std::map<double, uint8_t> confidence_map = {
1150 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1151 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1152 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1153 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1154 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1155 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1156 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1157 };
1158 for(const auto& [thresh, conf_val] : confidence_map) {
1159 if (yaw_rate_std <= thresh) {
1160 object.z_angular_velocity.confidence.value = conf_val;
1161 break;
1162 }
1163 }
1164 }
1165
1166 object.z_angular_velocity_is_present = true;
1167}
1168
1183inline void setObjectDimension(ObjectDimension& dimension, const double value,
1184 const double confidence = std::numeric_limits<double>::infinity()) {
1185 // limit value range
1186 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1187 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1188 } else {
1189 dimension.value.value = static_cast<uint16_t>(value);
1190 }
1191
1192 // limit confidence range
1193 if (confidence == std::numeric_limits<double>::infinity()) {
1194 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1195 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1196 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1197 } else {
1198 dimension.confidence.value = static_cast<uint8_t>(confidence);
1199 }
1200
1201}
1202
1213inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1214 const double std = std::numeric_limits<double>::infinity()) {
1215 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1216 object.object_dimension_x_is_present = true;
1217}
1218
1229inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1230 const double std = std::numeric_limits<double>::infinity()) {
1231 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1232 object.object_dimension_y_is_present = true;
1233}
1234
1245inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1246 const double std = std::numeric_limits<double>::infinity()) {
1247 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1248 object.object_dimension_z_is_present = true;
1249}
1250
1262inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1263 const double x_std = std::numeric_limits<double>::infinity(),
1264 const double y_std = std::numeric_limits<double>::infinity(),
1265 const double z_std = std::numeric_limits<double>::infinity()) {
1266 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1267 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1268 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1269}
1270
1280inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1281 setPositionOfPerceivedObject(object, point);
1282 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1283}
1284
1297inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1298 const gm::PointStamped& point, const int16_t delta_time = 0) {
1299 setUTMPositionOfPerceivedObject(cpm, object, point);
1300 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1301}
1302
1312inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1313 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1314 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1315}
1316
1330inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1331 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1332 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1333 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1334 container.container_data_perceived_object_container.perceived_objects.array.size();
1335 } else {
1336 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1337 }
1338}
1339
1352inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1353 // check for maximum number of containers
1354 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1355 cpm.payload.cpm_containers.value.array.push_back(container);
1356 } else {
1357 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1358 }
1359}
1360
1369inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1370 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1371 covariance_matrix);
1372}
1373
1374} // namespace etsi_its_cpm_ts_msgs::access

◆ setAccelerationComponent()

void etsi_its_cpm_ts_msgs::access::setAccelerationComponent ( AccelerationComponent & acceleration,
const double value,
const double confidence = std::numeric_limits<double>::infinity() )
inline

Sets the value and confidence of a AccelerationComponent.

This function sets the value and confidence of a AccelerationComponent object. The value is limited to a specific range, and the confidence is limited to a specific range as well. If the provided value or confidence is out of range, it will be set to the corresponding out-of-range value.

Parameters
accelerationThe AccelerationComponent object to set the value and confidence for.
valueThe value to set for the AccelerationComponent in dm/s^2.
confidenceThe confidence to set for the AccelerationComponent in dm/s^2. Default value is infinity, mapping to AccelerationConfidence::UNAVAILABLE.

Definition at line 475 of file cpm_ts_setters.h.

480 {
481 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
482 object.object_dimension_x_is_present = true;
483}
484

◆ setAccelerationConfidence()

template<typename AccelerationConfidence>
void etsi_its_cpm_ts_msgs::access::setAccelerationConfidence ( AccelerationConfidence & accel_confidence,
const double value )
inline

Set the Acceleration Confidence object.

Parameters
accel_confidenceobject to set
valuestandard deviation in m/s^2 as decimal number

Definition at line 220 of file cpm_ts_setters.h.

227 {
228 setCartesianCoordinateWithConfidence(object.position.z_coordinate,

◆ setAccelerationOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setAccelerationOfPerceivedObject ( PerceivedObject & object,
const gm::Vector3 & cartesian_acceleration,
const double x_std = std::numeric_limits<double>::infinity(),
const double y_std = std::numeric_limits<double>::infinity(),
const double z_std = std::numeric_limits<double>::infinity() )
inline

Sets the acceleration of a perceived object.

This function sets the acceleration of a perceived object using the provided Cartesian acceleration components. Optionally, confidence values can be specified for each acceleration component.

Parameters
objectThe perceived object for which the acceleration is being set.
cartesian_accelerationThe Cartesian acceleration components (x, y, z) of the object (in m/s^2).
x_stdThe standard deviation in m/s^2 for the x acceleration component (default: infinity).
y_stdThe standard deviation in m/s^2 for the y acceleration component (default: infinity).
z_stdThe standard deviation in m/s^2 for the z acceleration component (default: infinity).

Definition at line 508 of file cpm_ts_setters.h.

512 {
513 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
514 object.object_dimension_z_is_present = true;
515}
516

◆ setAltitude()

void etsi_its_cpm_ts_msgs::access::setAltitude ( Altitude & altitude,
const double value )
inline

Set the Altitude object.

AltitudeConfidence is set to UNAVAILABLE

Parameters
altitudeobject to set
valueAltitude value (above the reference ellipsoid surface) in meter as decimal number

Definition at line 166 of file cpm_ts_setters.h.

◆ setAltitudeValue()

void etsi_its_cpm_ts_msgs::access::setAltitudeValue ( AltitudeValue & altitude,
const double value )
inline

Set the AltitudeValue object.

Parameters
altitudeobject to set
valueAltitudeValue value (above the reference ellipsoid surface) in meter as decimal number

Definition at line 147 of file cpm_ts_setters.h.

◆ setCartesianCoordinateWithConfidence()

void etsi_its_cpm_ts_msgs::access::setCartesianCoordinateWithConfidence ( CartesianCoordinateWithConfidence & coordinate,
const double value,
const double confidence = std::numeric_limits<double>::infinity() )
inline

Sets the value and confidence of a CartesianCoordinateWithConfidence object.

This function sets the value and confidence of a CartesianCoordinateWithConfidence object. The value is limited to the range defined by CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE and CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE. The confidence is limited to the range defined by CoordinateConfidence::MIN and CoordinateConfidence::MAX. If the provided confidence is out of range, the confidence value is set to CoordinateConfidence::OUT_OF_RANGE.

Parameters
coordinateThe CartesianCoordinateWithConfidence object to be modified.
valueThe value to be set in centimeters.
confidenceThe confidence to be set in centimeters (default: infinity, which will map to CoordinateConfidence::UNAVAILABLE).

Definition at line 325 of file cpm_ts_setters.h.

◆ setDimensionsOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setDimensionsOfPerceivedObject ( PerceivedObject & object,
const gm::Vector3 & dimensions,
const double x_std = std::numeric_limits<double>::infinity(),
const double y_std = std::numeric_limits<double>::infinity(),
const double z_std = std::numeric_limits<double>::infinity() )
inline

Sets all dimensions of a perceived object.

This function sets the dimensions of a perceived object using the provided dimensions and confidence values.

Parameters
objectThe perceived object to set the dimensions for.
dimensionsThe dimensions of the object as a gm::Vector3 (x, y, z) in meters.
x_stdThe standard deviation in meters for the x dimension (optional, default: infinity).
y_stdThe standard deviation in meters for the y dimension (optional, default: infinity).
z_stdThe standard deviation in meters for the z dimension (optional, default: infinity).

Definition at line 700 of file cpm_ts_setters.h.

749inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
750 const uint8_t protocol_version = 0) {
751 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
752}
753
765inline void setReferenceTime(
766 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
767 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
768 TimestampIts t_its;
769 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
770 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
771 cpm.payload.management_container.reference_time = t_its;
772}
773
785inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
786 const double altitude = AltitudeValue::UNAVAILABLE) {
787 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
788}
789
802inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
803 const bool& northp) {
804 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
805}
806
815inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
816 object.object_id.value = id;
817 object.object_id_is_present = true;
818}
819
831inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
832 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
833 throw std::invalid_argument("MeasurementDeltaTime out of range");
834 } else {
835 object.measurement_delta_time.value = delta_time;
836 }
837}
838
852inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
853 const double confidence = std::numeric_limits<double>::infinity()) {
854 // limit value range
855 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
856 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
857 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
858 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
859 } else {
860 coordinate.value.value = static_cast<int32_t>(value);
861 }
862
863 // limit confidence range
864 if (confidence == std::numeric_limits<double>::infinity()) {
865 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
866 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
867 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
868 } else {
869 coordinate.confidence.value = static_cast<uint16_t>(confidence);
870 }
871}
872
884inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
885 const double x_std = std::numeric_limits<double>::infinity(),
886 const double y_std = std::numeric_limits<double>::infinity(),
887 const double z_std = std::numeric_limits<double>::infinity()) {
888 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
889 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
890 if (point.z != 0.0) {
891 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
892 object.position.z_coordinate_is_present = true;
893 }
894}
895
911inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
912 const gm::PointStamped& utm_position,
913 const double x_std = std::numeric_limits<double>::infinity(),
914 const double y_std = std::numeric_limits<double>::infinity(),
915 const double z_std = std::numeric_limits<double>::infinity()) {
916 gm::PointStamped reference_position = getUTMPosition(cpm);
917 if (utm_position.header.frame_id != reference_position.header.frame_id) {
918 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
919 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
920 ")");
921 }
922 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
923 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
924 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
925 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
926 if (utm_position.point.z != 0.0) {
927 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
928 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
929 object.position.z_coordinate_is_present = true;
930 }
931}
932
944inline void setVelocityComponent(VelocityComponent& velocity, const double value,
945 const double confidence = std::numeric_limits<double>::infinity()) {
946 // limit value range
947 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
948 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
949 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
950 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
951 } else {
952 velocity.value.value = static_cast<int16_t>(value);
953 }
954
955 // limit confidence range
956 if(confidence == std::numeric_limits<double>::infinity()) {
957 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
958 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
959 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
960 } else {
961 velocity.confidence.value = static_cast<uint8_t>(confidence);
962 }
963}
964
977inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
978 const double x_std = std::numeric_limits<double>::infinity(),
979 const double y_std = std::numeric_limits<double>::infinity(),
980 const double z_std = std::numeric_limits<double>::infinity()) {
981 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
982 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
983 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
984 if (cartesian_velocity.z != 0.0) {
985 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
986 object.velocity.cartesian_velocity.z_velocity_is_present = true;
987 }
988 object.velocity_is_present = true;
989}
990
1002inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1003 const double confidence = std::numeric_limits<double>::infinity()) {
1004 // limit value range
1005 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1006 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1007 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1008 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1009 } else {
1010 acceleration.value.value = static_cast<int16_t>(value);
1011 }
1012
1013 // limit confidence range
1014 if(confidence == std::numeric_limits<double>::infinity()) {
1015 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1016 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1017 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1018 } else {
1019 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1020 }
1021}
1022
1035inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1036 const double x_std = std::numeric_limits<double>::infinity(),
1037 const double y_std = std::numeric_limits<double>::infinity(),
1038 const double z_std = std::numeric_limits<double>::infinity()) {
1039 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1040 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1041 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1042 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1043 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1044 if (cartesian_acceleration.z != 0.0) {
1045 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1046 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1047 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1048 }
1049 object.acceleration_is_present = true;
1050}
1051
1062inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1063 double yaw_std = std::numeric_limits<double>::infinity()) {
1064 // wrap angle to range [0, 360)
1065 double yaw_in_degrees = yaw * 180 / M_PI;
1066 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1067 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1068 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1069
1070 if(yaw_std == std::numeric_limits<double>::infinity()) {
1071 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1072 } else {
1073 yaw_std *= 180.0 / M_PI;
1074 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1075
1076 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1077 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1078 } else {
1079 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1080 }
1081 }
1082 object.angles_is_present = true;
1083}
1084
1096inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1097 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1098 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1099 // limit value range
1100 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1101 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1102 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1103 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1104 }
1105
1106 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1107
1108 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1109 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1110 } else {
1111 yaw_rate_std *= 180.0 / M_PI;
1112 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1113 // How stupid is this?!
1114 static const std::map<double, uint8_t> confidence_map = {
1115 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1116 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1117 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1118 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1119 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1120 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1121 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1122 };
1123 for(const auto& [thresh, conf_val] : confidence_map) {
1124 if (yaw_rate_std <= thresh) {
1125 object.z_angular_velocity.confidence.value = conf_val;
1126 break;
1127 }
1128 }
1129 }
1130
1131 object.z_angular_velocity_is_present = true;
1132}
1133
1148inline void setObjectDimension(ObjectDimension& dimension, const double value,
1149 const double confidence = std::numeric_limits<double>::infinity()) {
1150 // limit value range
1151 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1152 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1153 } else {
1154 dimension.value.value = static_cast<uint16_t>(value);
1155 }
1156
1157 // limit confidence range
1158 if (confidence == std::numeric_limits<double>::infinity()) {
1159 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1160 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1161 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1162 } else {
1163 dimension.confidence.value = static_cast<uint8_t>(confidence);
1164 }
1165
1166}
1167
1178inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1179 const double std = std::numeric_limits<double>::infinity()) {
1180 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1181 object.object_dimension_x_is_present = true;
1182}
1183
1194inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1195 const double std = std::numeric_limits<double>::infinity()) {
1196 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1197 object.object_dimension_y_is_present = true;
1198}
1199
1210inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1211 const double std = std::numeric_limits<double>::infinity()) {
1212 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1213 object.object_dimension_z_is_present = true;
1214}
1215
1227inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1228 const double x_std = std::numeric_limits<double>::infinity(),
1229 const double y_std = std::numeric_limits<double>::infinity(),
1230 const double z_std = std::numeric_limits<double>::infinity()) {
1231 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1232 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1233 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1234}
1235
1245inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1246 setPositionOfPerceivedObject(object, point);
1247 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1248}
1249
1262inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1263 const gm::PointStamped& point, const int16_t delta_time = 0) {
1264 setUTMPositionOfPerceivedObject(cpm, object, point);
1265 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1266}
1267
1277inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1278 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1279 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1280}
1281
1295inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1296 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1297 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1298 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1299 container.container_data_perceived_object_container.perceived_objects.array.size();
1300 } else {
1301 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1302 }
1303}
1304
1317inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1318 // check for maximum number of containers
1319 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1320 cpm.payload.cpm_containers.value.array.push_back(container);
1321 } else {
1322 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1323 }
1324}
1325
1334inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1335 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1336 covariance_matrix);
1337}
1338
1339} // namespace etsi_its_cpm_ts_msgs::access

◆ setFromUTMPosition() [1/2]

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

Set the ReferencePosition of a CPM from a given UTM-Position.

The position is transformed to latitude and longitude by using GeographicLib::UTMUPS The z-Coordinate is directly used as altitude value The frame_id of the given utm_position must be set to 'utm_<zone><N/S>'

Parameters
[out]cpmCPM for which to set the ReferencePosition
[in]utm_positiongeometry_msgs::PointStamped describing the given utm position in meters
[in]zonethe UTM zone (zero means UPS) of the given position
[in]northphemisphere (true means north, false means south)

Definition at line 275 of file cpm_ts_setters.h.

275 : infinity).
276 * @param z_std The standard deviation in m/s for the z velocity component (default: infinity).
277 */
278inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,

◆ setFromUTMPosition() [2/2]

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

Set the ReferencePosition from a given UTM-Position.

The position is transformed to latitude and longitude by using GeographicLib::UTMUPS The z-Coordinate is directly used as altitude value The frame_id of the given utm_position must be set to 'utm_<zone><N/S>'

Parameters
[out]reference_positionReferencePostion or ReferencePositionWithConfidence to set
[in]utm_positiongeometry_msgs::PointStamped describing the given utm position
[in]zonethe UTM zone (zero means UPS) of the given position
[in]northphemisphere (true means north, false means south)

Definition at line 268 of file cpm_ts_setters.h.

274 : infinity).
275 * @param y_std The standard deviation in m/s for the y velocity component (default: infinity).
276 * @param z_std The standard deviation in m/s for the z velocity component (default: infinity).
277 */
278inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
279 const double x_std = std::numeric_limits<double>::infinity(),
280 const double y_std = std::numeric_limits<double>::infinity(),
281 const double z_std = std::numeric_limits<double>::infinity()) {
282 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;

◆ setHeadingCDD()

template<typename Heading, typename HeadingConfidence = decltype(Heading::heading_confidence)>
void etsi_its_cpm_ts_msgs::access::setHeadingCDD ( Heading & heading,
const double value,
double confidence = std::numeric_limits<double>::infinity() )

Set the Heading object.

0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West HeadingConfidence is set to UNAVAILABLE

Parameters
headingobject to set
valueHeading value in degree as decimal number
confidencestandard deviation of heading in degree as decimal number (default: infinity, mapping to HeadingConfidence::UNAVAILABLE)

Definition at line 327 of file cpm_ts_setters.h.

◆ setHeadingConfidence()

template<typename HeadingConfidence>
void etsi_its_cpm_ts_msgs::access::setHeadingConfidence ( HeadingConfidence & heading_confidence,
const double value )
inline

Set the Heading Confidence object.

Parameters
heading_confidenceobject to set
valuestandard deviation of heading in degree as decimal number

Definition at line 306 of file cpm_ts_setters.h.

306 {
307 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
308 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
309 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
310 } else {
311 acceleration.value.value = static_cast<int16_t>(value);
312 }
313
314 // limit confidence range

◆ setHeadingValue()

template<typename HeadingValue>
void etsi_its_cpm_ts_msgs::access::setHeadingValue ( HeadingValue & heading,
const double value )
inline

Set the HeadingValue object.

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

Parameters
headingobject to set
valueHeading value in degree as decimal number

Definition at line 293 of file cpm_ts_setters.h.

◆ setIdOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setIdOfPerceivedObject ( PerceivedObject & object,
const uint16_t id )
inline

Set the ID of a PerceivedObject.

Sets the object_id of the PerceivedObject to the given ID.

Parameters
objectPerceivedObject to set the ID for
idID to set

Definition at line 288 of file cpm_ts_setters.h.

◆ setItsPduHeader() [1/2]

void etsi_its_cpm_ts_msgs::access::setItsPduHeader ( CollectivePerceptionMessage & cpm,
const uint32_t station_id,
const uint8_t protocol_version = 0 )
inline

Sets the ITS PDU header of a CPM.

This function sets the ITS PDU header of a CPM with the provided station ID and protocol version.

Parameters
cpmThe CPM to set the ITS PDU header for.
station_idThe station ID to set in the ITS PDU header.
protocol_versionThe protocol version to set in the ITS PDU header. Default is 0.

Definition at line 222 of file cpm_ts_setters.h.

◆ setItsPduHeader() [2/2]

void etsi_its_cpm_ts_msgs::access::setItsPduHeader ( ItsPduHeader & header,
const uint8_t message_id,
const uint32_t station_id,
const uint8_t protocol_version = 0 )
inline

Set the Its Pdu Header object.

Parameters
headerItsPduHeader to be set
message_idID of the message
station_id
protocol_version

Definition at line 85 of file cpm_ts_setters.h.

87 {
88 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
89}
90

◆ setLateralAcceleration()

void etsi_its_cpm_ts_msgs::access::setLateralAcceleration ( AccelerationComponent & accel,
const double value,
const double confidence )
inline

Set the LateralAcceleration object.

AccelerationConfidence is set to UNAVAILABLE

Parameters
accelobject to set
valueLaterallAccelerationValue in m/s^2 as decimal number (left is positive)

Definition at line 160 of file cpm_ts_setters.h.

◆ setLateralAccelerationValue()

void etsi_its_cpm_ts_msgs::access::setLateralAccelerationValue ( AccelerationValue & accel,
const double value )
inline

Set the LateralAccelerationValue object.

Parameters
accelobject to set
valueLateralAccelerationValue in m/s^2 as decimal number (left is positive)

Definition at line 141 of file cpm_ts_setters.h.

◆ setLatitude()

void etsi_its_cpm_ts_msgs::access::setLatitude ( Latitude & latitude,
const double deg )
inline

Set the Latitude object.

Parameters
latitudeobject to set
degLatitude value in degree as decimal number

Definition at line 123 of file cpm_ts_setters.h.

◆ setLongitude()

void etsi_its_cpm_ts_msgs::access::setLongitude ( Longitude & longitude,
const double deg )
inline

Set the Longitude object.

Parameters
longitudeobject to set
degLongitude value in degree as decimal number

Definition at line 135 of file cpm_ts_setters.h.

◆ setLongitudinalAcceleration()

void etsi_its_cpm_ts_msgs::access::setLongitudinalAcceleration ( AccelerationComponent & accel,
const double value,
const double confidence )
inline

Set the LongitudinalAcceleration object.

AccelerationConfidence is set to UNAVAILABLE

Parameters
accelobject to set
valueLongitudinalAccelerationValue in m/s^2 as decimal number (braking is negative)

Definition at line 130 of file cpm_ts_setters.h.

◆ setLongitudinalAccelerationValue()

void etsi_its_cpm_ts_msgs::access::setLongitudinalAccelerationValue ( AccelerationValue & accel,
const double value )
inline

Set the LongitudinalAccelerationValue object.

Parameters
accelobject to set
valueLongitudinalAccelerationValue in m/s^2 as decimal number (braking is negative)

Definition at line 111 of file cpm_ts_setters.h.

◆ setMeasurementDeltaTimeOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setMeasurementDeltaTimeOfPerceivedObject ( PerceivedObject & object,
const int16_t delta_time = 0 )
inline

Sets the measurement delta time of a PerceivedObject.

This function sets the measurement delta time of a PerceivedObject. The measurement delta time represents the time difference between the reference time of the CPM and the measurement of the object.

Parameters
objectThe PerceivedObject to set the measurement delta time for.
delta_timeThe measurement delta time to set in milliseconds. Default value is 0.
Exceptions
std::invalid_argumentif the delta_time is out of range.

Definition at line 304 of file cpm_ts_setters.h.

◆ setObjectDimension()

void etsi_its_cpm_ts_msgs::access::setObjectDimension ( ObjectDimension & dimension,
const double value,
const double confidence = std::numeric_limits<double>::infinity() )
inline

Sets the object dimension with the given value and confidence.

This function sets the value and confidence of the object dimension based on the provided parameters. The value is limited to the range defined by ObjectDimensionValue::MIN and ObjectDimensionValue::MAX. If the provided value is outside this range, the dimension value is set to ObjectDimensionValue::OUT_OF_RANGE.

The confidence is limited to the range defined by ObjectDimensionConfidence::MIN and ObjectDimensionConfidence::MAX. If the provided confidence is outside this range, the confidence value is set to ObjectDimensionConfidence::OUT_OF_RANGE.

Parameters
dimensionThe object dimension to be set.
valueThe value of the object dimension in decimeters.
confidenceThe confidence of the object dimension in decimeters (optional, default is infinty, mapping to ObjectDimensionConfidence::UNAVAILABLE).

Definition at line 621 of file cpm_ts_setters.h.

635inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
636 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
637 covariance_matrix);
638}
639
setWGSPosConfidenceEllipse
void setWGSPosConfidenceEllipse(PosConfidenceEllipse &position_confidence_ellipse, const std::array< double, 4 > &covariance_matrix)
Set the Pos Confidence Ellipse object.
Definition cdd_setters_common.h:391

◆ setPosConfidenceEllipse() [1/2]

template<typename PosConfidenceEllipse>
void etsi_its_cpm_ts_msgs::access::setPosConfidenceEllipse ( PosConfidenceEllipse & position_confidence_ellipse,
const double semi_major_axis,
const double semi_minor_axis,
const double orientation )
inline

Set the Pos Confidence Ellipse object.

Parameters
[out]position_confidence_ellipseThe PosConfidenceEllipse to set
[in]semi_major_axislength of the semi-major axis in meters
[in]semi_minor_axislength of the semi-minor axis in meters
[in]orientationof the semi-major axis in degrees, with respect to WGS84

Definition at line 360 of file cpm_ts_setters.h.

364 {
365 // wrap angle to range [0, 360)

◆ setPosConfidenceEllipse() [2/2]

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

Set the Pos Confidence Ellipse object.

Parameters
position_confidence_ellipse
covariance_matrixThe four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the object (x = longitudinal, y = lateral)
object_headingThe heading of the object in rad, with respect to WGS84

Definition at line 433 of file cpm_ts_setters.h.

◆ setPositionConfidenceEllipse() [1/2]

template<typename PositionConfidenceEllipse, typename Wgs84AngleValue = decltype(PositionConfidenceEllipse::semi_major_axis_orientation)>
void etsi_its_cpm_ts_msgs::access::setPositionConfidenceEllipse ( PositionConfidenceEllipse & position_confidence_ellipse,
const double semi_major_axis,
const double semi_minor_axis,
const double orientation )
inline

Set the Position Confidence Ellipse object.

Parameters
position_confidence_ellipseThe position confidence ellipse to set
semi_major_axisThe length of the semi-major axis in meters
semi_minor_axisThe length of the semi-minor axis in meters
orientationThe orientation of the semi-major axis in degrees, relative to WGS84

Definition at line 174 of file cpm_ts_setters.h.

◆ setPositionConfidenceEllipse() [2/2]

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

Set the Position Confidence Ellipse object.

Parameters
position_confidence_ellipse
covariance_matrixThe four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the object (x = longitudinal, y = lateral)
object_headingThe heading of the object in rad, with respect to WGS84

Definition at line 191 of file cpm_ts_setters.h.

191 {
192 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
193 object.position.z_coordinate_is_present = true;
194 }

◆ setPositionOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setPositionOfPerceivedObject ( PerceivedObject & object,
const gm::Point & point,
const double x_std = std::numeric_limits<double>::infinity(),
const double y_std = std::numeric_limits<double>::infinity(),
const double z_std = std::numeric_limits<double>::infinity() )
inline

Sets the position of a perceived object (relative to the CPM's reference position).

This function sets the position of a perceived object using the provided coordinates and confidence values.

Parameters
objectThe PerceivedObject to set the position for.
pointThe gm::Point representing the coordinates of the object in meters.
x_stdThe standard deviation in meters for the x-coordinate (default: infinity).
y_stdThe standard deviation in meters for the y-coordinate (default: infinity).
z_stdThe standard deviation in meters for the z-coordinate (default: infinity).

Definition at line 357 of file cpm_ts_setters.h.

364 {
365 // wrap angle to range [0, 360)
366 double yaw_in_degrees = yaw * 180 / M_PI;
367 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;

◆ setReferencePosition() [1/2]

void etsi_its_cpm_ts_msgs::access::setReferencePosition ( CollectivePerceptionMessage & cpm,
const double latitude,
const double longitude,
const double altitude = AltitudeValue::UNAVAILABLE )
inline

Set the ReferencePositionWithConfidence for a CPM TS.

This function sets the latitude, longitude, and altitude of the CPMs reference position. If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE.

Parameters
cpmCPM to set the ReferencePosition
latitudeThe latitude value position in degree as decimal number.
longitudeThe longitude value in degree as decimal number.
altitudeThe altitude value (above the reference ellipsoid surface) in meter as decimal number (optional).

Definition at line 258 of file cpm_ts_setters.h.

259 {
260 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
261 } else {

◆ setReferencePosition() [2/2]

template<typename T>
void etsi_its_cpm_ts_msgs::access::setReferencePosition ( T & ref_position,
const double latitude,
const double longitude,
const double altitude = AltitudeValue::UNAVAILABLE )
inline

Sets the reference position in the given ReferencePostion object.

This function sets the latitude, longitude, and altitude of the reference position. If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE.

Parameters
ref_positionReferencePostion or ReferencePositionWithConfidence object to set the reference position in.
latitudeThe latitude value position in degree as decimal number.
longitudeThe longitude value in degree as decimal number.
altitudeThe altitude value (above the reference ellipsoid surface) in meter as decimal number (optional).

Definition at line 242 of file cpm_ts_setters.h.

246 {
247 // limit value range
248 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
249 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
250 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
251 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
252 } else {
253 velocity.value.value = static_cast<int16_t>(value);

◆ setReferenceTime()

void etsi_its_cpm_ts_msgs::access::setReferenceTime ( CollectivePerceptionMessage & cpm,
const uint64_t unix_nanosecs,
const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second )
inline

Sets the reference time in a CPM.

This function sets the reference time in a CPM object. The reference time is represented by a Unix timestamp in nanoseconds including the number of leap seconds. The reference time is stored in the payload management container of the CPM.

Parameters
cpmThe CPM object to set the reference time in.
unix_nanosecsThe Unix timestamp in nanoseconds representing the reference time.
n_leap_secondsThe number of leap seconds to be considered. Defaults to the todays number of leap seconds since 2004.

Definition at line 238 of file cpm_ts_setters.h.

◆ setSemiAxis()

template<typename SemiAxisLength>
void etsi_its_cpm_ts_msgs::access::setSemiAxis ( SemiAxisLength & semi_axis_length,
const double length )
inline

Set the Semi Axis length.

// See https://godbolt.org/z/Eceavfo99 on how the OneCentimeterHelper works with this template

Parameters
semi_axis_lengthThe SemiAxisLength to set
lengththe desired length in meters

Definition at line 341 of file cpm_ts_setters.h.

345 {
346 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
347 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
348 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
349 }

◆ setSpeed()

void etsi_its_cpm_ts_msgs::access::setSpeed ( Speed & speed,
const double value,
const double confidence = std::numeric_limits<double>::infinity() )
inline

Set the Speed object.

SpeedConfidence is set to UNAVAILABLE

Parameters
speedobject to set
valueSpeed in in m/s as decimal number
confidencestandard deviation in m/s as decimal number (Optional. Default is std::numeric_limits<double>::infinity(), mapping to SpeedConfidence::UNAVAILABLE)

Definition at line 208 of file cpm_ts_setters.h.

208 : CoordinateConfidence::UNAVAILABLE).
209 *
210 * @throws std::invalid_argument if the UTM-Position frame_id does not match the reference position frame_id.
211 */

◆ setSpeedConfidence()

void etsi_its_cpm_ts_msgs::access::setSpeedConfidence ( SpeedConfidence & speed_confidence,
const double value )
inline

Set the Speed Confidence object.

Parameters
speed_confidenceobject to set
valuestandard deviation in m/s as decimal number

Definition at line 189 of file cpm_ts_setters.h.

191 {
192 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
193 object.position.z_coordinate_is_present = true;
194 }
195}
196

◆ setSpeedValue()

void etsi_its_cpm_ts_msgs::access::setSpeedValue ( SpeedValue & speed,
const double value )
inline

Set the SpeedValue object.

Parameters
speedobject to set
valueSpeedValue in m/s as decimal number

Definition at line 177 of file cpm_ts_setters.h.

◆ setStationId()

void etsi_its_cpm_ts_msgs::access::setStationId ( StationId & station_id,
const uint32_t id_value )
inline

Set the Station Id object.

Parameters
station_id
id_value

Definition at line 72 of file cpm_ts_setters.h.

◆ setStationType()

void etsi_its_cpm_ts_msgs::access::setStationType ( TrafficParticipantType & station_type,
const uint8_t value )
inline

Set the Station Type.

Parameters
station_type
value

Definition at line 100 of file cpm_ts_setters.h.

◆ setTimestampITS()

void etsi_its_cpm_ts_msgs::access::setTimestampITS ( TimestampIts & timestamp_its,
const uint64_t unix_nanosecs,
const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second )
inline

Set the TimestampITS object.

Parameters
[in]timestamp_itsTimestampITS object to set the timestamp
[in]unix_nanosecsUnix-Nanoseconds to set the timestamp for
[in]n_leap_secondsNumber of leap-seconds since 2004. (Defaults to the todays number of leap seconds since 2004.)
[in]epoch_offsetUnix-Timestamp in seconds for the 01.01.2004 at 00:00:00

Definition at line 109 of file cpm_ts_setters.h.

◆ setUTMPositionOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setUTMPositionOfPerceivedObject ( CollectivePerceptionMessage & cpm,
PerceivedObject & object,
const gm::PointStamped & utm_position,
const double x_std = std::numeric_limits<double>::infinity(),
const double y_std = std::numeric_limits<double>::infinity(),
const double z_std = std::numeric_limits<double>::infinity() )
inline

Sets the position of a perceived object based on a UTM position.

This function sets the position of a perceived object using the provided UTM position and the CPM's reference position. It also allows specifying confidence values for the x, y, and z coordinates.

Parameters
cpmThe CPM to get the reference position from.
objectThe PerceivedObject to set the position for.
utm_positiongm::PointStamped representing the UTM position of the object including the frame_id (utm_<zone><N/S>).
x_confidenceThe standard deviation in meters for the x coordinate (default: CoordinateConfidence::UNAVAILABLE).
y_confidenceThe standard deviation in meters for the y coordinate (default: CoordinateConfidence::UNAVAILABLE).
z_confidenceThe standard deviation in meters for the z coordinate (default: CoordinateConfidence::UNAVAILABLE).
Exceptions
std::invalid_argumentif the UTM-Position frame_id does not match the reference position frame_id.

Definition at line 384 of file cpm_ts_setters.h.

◆ setVelocityComponent()

void etsi_its_cpm_ts_msgs::access::setVelocityComponent ( VelocityComponent & velocity,
const double value,
const double confidence = std::numeric_limits<double>::infinity() )
inline

Sets the value and confidence of a VelocityComponent.

This function sets the value and confidence of a VelocityComponent object. The value is limited to a specific range, and the confidence is limited to a specific range as well. If the provided value or confidence is out of range, it will be set to the corresponding out-of-range value.

Parameters
velocityThe VelocityComponent object to set the value and confidence for.
valueThe value to set for the VelocityComponent in cm/s.
confidenceThe confidence to set for the VelocityComponent in cm/s. Default value is infinity, mapping to SpeedConfidence::UNAVAILABLE.

Definition at line 417 of file cpm_ts_setters.h.

◆ setVelocityOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setVelocityOfPerceivedObject ( PerceivedObject & object,
const gm::Vector3 & cartesian_velocity,
const double x_std = std::numeric_limits<double>::infinity(),
const double y_std = std::numeric_limits<double>::infinity(),
const double z_std = std::numeric_limits<double>::infinity() )
inline

Sets the velocity of a perceived object.

This function sets the velocity of a perceived object using the provided Cartesian velocity components. Optionally, confidence values can be specified for each velocity component.

Parameters
objectThe perceived object for which the velocity is being set.
cartesian_velocityThe Cartesian velocity components (x, y, z) of the object (in m/s).
x_stdThe standard deviation in m/s for the x velocity component (default: infinity).
y_stdThe standard deviation in m/s for the y velocity component (default: infinity).
z_stdThe standard deviation in m/s for the z velocity component (default: infinity).

Definition at line 450 of file cpm_ts_setters.h.

◆ setWGSPosConfidenceEllipse()

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

Set the Pos Confidence Ellipse object.

Parameters
position_confidence_ellipse
covariance_matrixThe four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the WGS axes (x = North, y = East)
object_headingThe heading of the object in rad, with respect to WGS84

Definition at line 448 of file cpm_ts_setters.h.

◆ setWGSPositionConfidenceEllipse()

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

Set the Position Confidence Ellipse object.

Parameters
position_confidence_ellipse
covariance_matrixThe four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is aligned with the WGS axes (x = North, y = East)
object_headingThe heading of the object in rad, with respect to WGS84

Definition at line 206 of file cpm_ts_setters.h.

◆ setWGSRefPosConfidence()

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

Set the confidence of the reference position.

Parameters
cpmCPM-Message to set the confidence
covariance_matrixThe four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown. Its coordinate system is WGS84 (x = North, y = East)

Definition at line 807 of file cpm_ts_setters.h.

856inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
857 const uint8_t protocol_version = 0) {
858 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
859}
860
872inline void setReferenceTime(
873 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
874 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
875 TimestampIts t_its;
876 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
877 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
878 cpm.payload.management_container.reference_time = t_its;
879}
880
892inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
893 const double altitude = AltitudeValue::UNAVAILABLE) {
894 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
895}
896
909inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
910 const bool& northp) {
911 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
912}
913
922inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
923 object.object_id.value = id;
924 object.object_id_is_present = true;
925}
926
938inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
939 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
940 throw std::invalid_argument("MeasurementDeltaTime out of range");
941 } else {
942 object.measurement_delta_time.value = delta_time;
943 }
944}
945
959inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
960 const double confidence = std::numeric_limits<double>::infinity()) {
961 // limit value range
962 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
963 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
964 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
965 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
966 } else {
967 coordinate.value.value = static_cast<int32_t>(value);
968 }
969
970 // limit confidence range
971 if (confidence == std::numeric_limits<double>::infinity()) {
972 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
973 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
974 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
975 } else {
976 coordinate.confidence.value = static_cast<uint16_t>(confidence);
977 }
978}
979
991inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
992 const double x_std = std::numeric_limits<double>::infinity(),
993 const double y_std = std::numeric_limits<double>::infinity(),
994 const double z_std = std::numeric_limits<double>::infinity()) {
995 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
996 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
997 if (point.z != 0.0) {
998 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
999 object.position.z_coordinate_is_present = true;
1000 }
1001}
1002
1018inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1019 const gm::PointStamped& utm_position,
1020 const double x_std = std::numeric_limits<double>::infinity(),
1021 const double y_std = std::numeric_limits<double>::infinity(),
1022 const double z_std = std::numeric_limits<double>::infinity()) {
1023 gm::PointStamped reference_position = getUTMPosition(cpm);
1024 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1025 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1026 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1027 ")");
1028 }
1029 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1030 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1031 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1032 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1033 if (utm_position.point.z != 0.0) {
1034 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1035 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1036 object.position.z_coordinate_is_present = true;
1037 }
1038}
1039
1051inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1052 const double confidence = std::numeric_limits<double>::infinity()) {
1053 // limit value range
1054 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1055 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1056 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1057 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1058 } else {
1059 velocity.value.value = static_cast<int16_t>(value);
1060 }
1061
1062 // limit confidence range
1063 if(confidence == std::numeric_limits<double>::infinity()) {
1064 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1065 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1066 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1067 } else {
1068 velocity.confidence.value = static_cast<uint8_t>(confidence);
1069 }
1070}
1071
1084inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1085 const double x_std = std::numeric_limits<double>::infinity(),
1086 const double y_std = std::numeric_limits<double>::infinity(),
1087 const double z_std = std::numeric_limits<double>::infinity()) {
1088 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1089 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1090 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1091 if (cartesian_velocity.z != 0.0) {
1092 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1093 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1094 }
1095 object.velocity_is_present = true;
1096}
1097
1109inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1110 const double confidence = std::numeric_limits<double>::infinity()) {
1111 // limit value range
1112 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1113 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1114 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1115 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1116 } else {
1117 acceleration.value.value = static_cast<int16_t>(value);
1118 }
1119
1120 // limit confidence range
1121 if(confidence == std::numeric_limits<double>::infinity()) {
1122 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1123 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1124 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1125 } else {
1126 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1127 }
1128}
1129
1142inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1143 const double x_std = std::numeric_limits<double>::infinity(),
1144 const double y_std = std::numeric_limits<double>::infinity(),
1145 const double z_std = std::numeric_limits<double>::infinity()) {
1146 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1147 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1148 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1149 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1150 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1151 if (cartesian_acceleration.z != 0.0) {
1152 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1153 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1154 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1155 }
1156 object.acceleration_is_present = true;
1157}
1158
1169inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1170 double yaw_std = std::numeric_limits<double>::infinity()) {
1171 // wrap angle to range [0, 360)
1172 double yaw_in_degrees = yaw * 180 / M_PI;
1173 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1174 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1175 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1176
1177 if(yaw_std == std::numeric_limits<double>::infinity()) {
1178 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1179 } else {
1180 yaw_std *= 180.0 / M_PI;
1181 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1182
1183 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1184 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1185 } else {
1186 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1187 }
1188 }
1189 object.angles_is_present = true;
1190}
1191
1203inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1204 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1205 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1206 // limit value range
1207 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1208 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1209 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1210 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1211 }
1212
1213 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1214
1215 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1216 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1217 } else {
1218 yaw_rate_std *= 180.0 / M_PI;
1219 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1220 // How stupid is this?!
1221 static const std::map<double, uint8_t> confidence_map = {
1222 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1223 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1224 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1225 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1226 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1227 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1228 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1229 };
1230 for(const auto& [thresh, conf_val] : confidence_map) {
1231 if (yaw_rate_std <= thresh) {
1232 object.z_angular_velocity.confidence.value = conf_val;
1233 break;
1234 }
1235 }
1236 }
1237
1238 object.z_angular_velocity_is_present = true;
1239}
1240
1255inline void setObjectDimension(ObjectDimension& dimension, const double value,
1256 const double confidence = std::numeric_limits<double>::infinity()) {
1257 // limit value range
1258 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1259 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1260 } else {
1261 dimension.value.value = static_cast<uint16_t>(value);
1262 }
1263
1264 // limit confidence range
1265 if (confidence == std::numeric_limits<double>::infinity()) {
1266 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1267 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1268 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1269 } else {
1270 dimension.confidence.value = static_cast<uint8_t>(confidence);
1271 }
1272
1273}
1274
1285inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1286 const double std = std::numeric_limits<double>::infinity()) {
1287 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1288 object.object_dimension_x_is_present = true;
1289}
1290
1301inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1302 const double std = std::numeric_limits<double>::infinity()) {
1303 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1304 object.object_dimension_y_is_present = true;
1305}
1306
1317inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1318 const double std = std::numeric_limits<double>::infinity()) {
1319 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1320 object.object_dimension_z_is_present = true;
1321}
1322
1334inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1335 const double x_std = std::numeric_limits<double>::infinity(),
1336 const double y_std = std::numeric_limits<double>::infinity(),
1337 const double z_std = std::numeric_limits<double>::infinity()) {
1338 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1339 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1340 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1341}
1342
1352inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1353 setPositionOfPerceivedObject(object, point);
1354 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1355}
1356
1369inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1370 const gm::PointStamped& point, const int16_t delta_time = 0) {
1371 setUTMPositionOfPerceivedObject(cpm, object, point);
1372 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1373}
1374
1384inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1385 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1386 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1387}
1388
1402inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1403 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1404 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1405 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1406 container.container_data_perceived_object_container.perceived_objects.array.size();
1407 } else {
1408 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1409 }
1410}
1411
1424inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1425 // check for maximum number of containers
1426 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1427 cpm.payload.cpm_containers.value.array.push_back(container);
1428 } else {
1429 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1430 }
1431}
1432
1441inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1442 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1443 covariance_matrix);
1444}
1445
1446} // namespace etsi_its_cpm_ts_msgs::access

◆ setXDimensionOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setXDimensionOfPerceivedObject ( PerceivedObject & object,
const double value,
const double std = std::numeric_limits<double>::infinity() )
inline

Sets the x-dimension of a perceived object.

This function sets the x-dimension of the given PerceivedObject to the specified value. The x-dimension usually represents the length of the object.

Parameters
objectThe PerceivedObject to modify.
valueThe value to set as the x-dimension in meters.
stdThe standard deviation of the x-dimension value in meters (optional, default is infinity).

Definition at line 651 of file cpm_ts_setters.h.

687 {
688
689#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
690
700inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
701 const uint8_t protocol_version = 0) {
702 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
703}
704
716inline void setReferenceTime(
717 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
718 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
719 TimestampIts t_its;
720 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
721 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
722 cpm.payload.management_container.reference_time = t_its;
723}
724
736inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
737 const double altitude = AltitudeValue::UNAVAILABLE) {
738 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
739}
740
753inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
754 const bool& northp) {
755 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
756}
757
766inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
767 object.object_id.value = id;
768 object.object_id_is_present = true;
769}
770
782inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
783 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
784 throw std::invalid_argument("MeasurementDeltaTime out of range");
785 } else {
786 object.measurement_delta_time.value = delta_time;
787 }
788}
789
803inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
804 const double confidence = std::numeric_limits<double>::infinity()) {
805 // limit value range
806 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
807 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
808 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
809 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
810 } else {
811 coordinate.value.value = static_cast<int32_t>(value);
812 }
813
814 // limit confidence range
815 if (confidence == std::numeric_limits<double>::infinity()) {
816 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
817 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
818 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
819 } else {
820 coordinate.confidence.value = static_cast<uint16_t>(confidence);
821 }
822}
823
835inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
836 const double x_std = std::numeric_limits<double>::infinity(),
837 const double y_std = std::numeric_limits<double>::infinity(),
838 const double z_std = std::numeric_limits<double>::infinity()) {
839 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
840 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
841 if (point.z != 0.0) {
842 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
843 object.position.z_coordinate_is_present = true;
844 }
845}
846
862inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
863 const gm::PointStamped& utm_position,
864 const double x_std = std::numeric_limits<double>::infinity(),
865 const double y_std = std::numeric_limits<double>::infinity(),
866 const double z_std = std::numeric_limits<double>::infinity()) {
867 gm::PointStamped reference_position = getUTMPosition(cpm);
868 if (utm_position.header.frame_id != reference_position.header.frame_id) {
869 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
870 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
871 ")");
872 }
873 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
874 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
875 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
876 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
877 if (utm_position.point.z != 0.0) {
878 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
879 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
880 object.position.z_coordinate_is_present = true;
881 }
882}
883
895inline void setVelocityComponent(VelocityComponent& velocity, const double value,
896 const double confidence = std::numeric_limits<double>::infinity()) {
897 // limit value range
898 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
899 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
900 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
901 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
902 } else {
903 velocity.value.value = static_cast<int16_t>(value);
904 }
905
906 // limit confidence range
907 if(confidence == std::numeric_limits<double>::infinity()) {
908 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
909 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
910 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
911 } else {
912 velocity.confidence.value = static_cast<uint8_t>(confidence);
913 }
914}
915
928inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
929 const double x_std = std::numeric_limits<double>::infinity(),
930 const double y_std = std::numeric_limits<double>::infinity(),
931 const double z_std = std::numeric_limits<double>::infinity()) {
932 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
933 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
934 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
935 if (cartesian_velocity.z != 0.0) {
936 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
937 object.velocity.cartesian_velocity.z_velocity_is_present = true;
938 }
939 object.velocity_is_present = true;
940}
941
953inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
954 const double confidence = std::numeric_limits<double>::infinity()) {
955 // limit value range
956 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
957 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
958 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
959 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
960 } else {
961 acceleration.value.value = static_cast<int16_t>(value);
962 }
963
964 // limit confidence range
965 if(confidence == std::numeric_limits<double>::infinity()) {
966 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
967 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
968 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
969 } else {
970 acceleration.confidence.value = static_cast<uint8_t>(confidence);
971 }
972}
973
986inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
987 const double x_std = std::numeric_limits<double>::infinity(),
988 const double y_std = std::numeric_limits<double>::infinity(),
989 const double z_std = std::numeric_limits<double>::infinity()) {
990 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
991 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
992 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
993 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
994 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
995 if (cartesian_acceleration.z != 0.0) {
996 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
997 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
998 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
999 }
1000 object.acceleration_is_present = true;
1001}
1002
1013inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1014 double yaw_std = std::numeric_limits<double>::infinity()) {
1015 // wrap angle to range [0, 360)
1016 double yaw_in_degrees = yaw * 180 / M_PI;
1017 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1018 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1019 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1020
1021 if(yaw_std == std::numeric_limits<double>::infinity()) {
1022 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1023 } else {
1024 yaw_std *= 180.0 / M_PI;
1025 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1026
1027 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1028 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1029 } else {
1030 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1031 }
1032 }
1033 object.angles_is_present = true;
1034}
1035
1047inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1048 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1049 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1050 // limit value range
1051 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1052 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1053 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1054 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1055 }
1056
1057 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1058
1059 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1060 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1061 } else {
1062 yaw_rate_std *= 180.0 / M_PI;
1063 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1064 // How stupid is this?!
1065 static const std::map<double, uint8_t> confidence_map = {
1066 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1067 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1068 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1069 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1070 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1071 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1072 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1073 };
1074 for(const auto& [thresh, conf_val] : confidence_map) {
1075 if (yaw_rate_std <= thresh) {
1076 object.z_angular_velocity.confidence.value = conf_val;
1077 break;
1078 }
1079 }
1080 }
1081
1082 object.z_angular_velocity_is_present = true;
1083}
1084
1099inline void setObjectDimension(ObjectDimension& dimension, const double value,
1100 const double confidence = std::numeric_limits<double>::infinity()) {
1101 // limit value range
1102 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1103 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1104 } else {
1105 dimension.value.value = static_cast<uint16_t>(value);
1106 }
1107
1108 // limit confidence range
1109 if (confidence == std::numeric_limits<double>::infinity()) {
1110 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1111 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1112 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1113 } else {
1114 dimension.confidence.value = static_cast<uint8_t>(confidence);
1115 }
1116
1117}
1118
1129inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1130 const double std = std::numeric_limits<double>::infinity()) {
1131 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1132 object.object_dimension_x_is_present = true;
1133}
1134
1145inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1146 const double std = std::numeric_limits<double>::infinity()) {
1147 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1148 object.object_dimension_y_is_present = true;
1149}
1150
1161inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1162 const double std = std::numeric_limits<double>::infinity()) {
1163 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1164 object.object_dimension_z_is_present = true;
1165}
1166
1178inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1179 const double x_std = std::numeric_limits<double>::infinity(),
1180 const double y_std = std::numeric_limits<double>::infinity(),
1181 const double z_std = std::numeric_limits<double>::infinity()) {
1182 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1183 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1184 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1185}
1186
1196inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1197 setPositionOfPerceivedObject(object, point);
1198 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1199}
1200
1213inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1214 const gm::PointStamped& point, const int16_t delta_time = 0) {
1215 setUTMPositionOfPerceivedObject(cpm, object, point);
1216 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1217}
1218
1228inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1229 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1230 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1231}
1232
1246inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1247 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1248 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1249 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1250 container.container_data_perceived_object_container.perceived_objects.array.size();
1251 } else {
1252 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1253 }
1254}
1255
1268inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1269 // check for maximum number of containers
1270 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1271 cpm.payload.cpm_containers.value.array.push_back(container);
1272 } else {
1273 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1274 }
1275}
1276
1285inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1286 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1287 covariance_matrix);
1288}
1289
1290} // namespace etsi_its_cpm_ts_msgs::access

◆ setYawOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setYawOfPerceivedObject ( PerceivedObject & object,
const double yaw,
double yaw_std = std::numeric_limits<double>::infinity() )
inline

Sets the yaw angle of a perceived object.

This function sets the yaw angle of a PerceivedObject. The yaw angle is wrapped to the range [0, 360] degrees. The function also allows specifying the confidence level of the yaw angle.

Parameters
objectThe PerceivedObject to set the yaw angle for.
yawThe yaw angle in radians.
yaw_stdThe standard deviation of the yaw angle in radians (optional, default is Infinity).

Definition at line 535 of file cpm_ts_setters.h.

546 {
547 setPositionOfPerceivedObject(object, point);
548 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
549}
550

◆ setYawRateOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setYawRateOfPerceivedObject ( PerceivedObject & object,
const double yaw_rate,
double yaw_rate_std = std::numeric_limits<double>::infinity() )
inline

Sets the yaw rate of a perceived object.

This function sets the yaw rate of a PerceivedObject. The yaw rate is limited to the range defined by CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE and CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE. The function also allows specifying the confidence level of the yaw rate.

Parameters
objectThe PerceivedObject to set the yaw rate for.
yaw_rateThe yaw rate in rad/s.
yaw_rate_stdStandard deviation of the yaw rate in rad/s (optional, default is infinity, mapping to AngularSpeedConfidence::UNAVAILABLE).

Definition at line 569 of file cpm_ts_setters.h.

596inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
597 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
598 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
599 container.container_data_perceived_object_container.number_of_perceived_objects.value =
600 container.container_data_perceived_object_container.perceived_objects.array.size();
601 } else {
602 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
603 }
604}
605

◆ setYDimensionOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setYDimensionOfPerceivedObject ( PerceivedObject & object,
const double value,
const double std = std::numeric_limits<double>::infinity() )
inline

Sets the y-dimension of a perceived object.

This function sets the y-dimension of the given PerceivedObject to the specified value. The y-dimension usually represents the width of the object.

Parameters
objectThe PerceivedObject to modify.
valueThe value to set as the y-dimension in meters.
stdThe standard deviation of the y-dimension value in meters (optional, default is infinity).

Definition at line 667 of file cpm_ts_setters.h.

703 {
704
705#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
706
716inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
717 const uint8_t protocol_version = 0) {
718 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
719}
720
732inline void setReferenceTime(
733 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
734 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
735 TimestampIts t_its;
736 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
737 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
738 cpm.payload.management_container.reference_time = t_its;
739}
740
752inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
753 const double altitude = AltitudeValue::UNAVAILABLE) {
754 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
755}
756
769inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
770 const bool& northp) {
771 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
772}
773
782inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
783 object.object_id.value = id;
784 object.object_id_is_present = true;
785}
786
798inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
799 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
800 throw std::invalid_argument("MeasurementDeltaTime out of range");
801 } else {
802 object.measurement_delta_time.value = delta_time;
803 }
804}
805
819inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
820 const double confidence = std::numeric_limits<double>::infinity()) {
821 // limit value range
822 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
823 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
824 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
825 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
826 } else {
827 coordinate.value.value = static_cast<int32_t>(value);
828 }
829
830 // limit confidence range
831 if (confidence == std::numeric_limits<double>::infinity()) {
832 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
833 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
834 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
835 } else {
836 coordinate.confidence.value = static_cast<uint16_t>(confidence);
837 }
838}
839
851inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
852 const double x_std = std::numeric_limits<double>::infinity(),
853 const double y_std = std::numeric_limits<double>::infinity(),
854 const double z_std = std::numeric_limits<double>::infinity()) {
855 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
856 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
857 if (point.z != 0.0) {
858 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
859 object.position.z_coordinate_is_present = true;
860 }
861}
862
878inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
879 const gm::PointStamped& utm_position,
880 const double x_std = std::numeric_limits<double>::infinity(),
881 const double y_std = std::numeric_limits<double>::infinity(),
882 const double z_std = std::numeric_limits<double>::infinity()) {
883 gm::PointStamped reference_position = getUTMPosition(cpm);
884 if (utm_position.header.frame_id != reference_position.header.frame_id) {
885 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
886 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
887 ")");
888 }
889 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
890 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
891 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
892 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
893 if (utm_position.point.z != 0.0) {
894 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
895 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
896 object.position.z_coordinate_is_present = true;
897 }
898}
899
911inline void setVelocityComponent(VelocityComponent& velocity, const double value,
912 const double confidence = std::numeric_limits<double>::infinity()) {
913 // limit value range
914 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
915 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
916 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
917 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
918 } else {
919 velocity.value.value = static_cast<int16_t>(value);
920 }
921
922 // limit confidence range
923 if(confidence == std::numeric_limits<double>::infinity()) {
924 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
925 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
926 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
927 } else {
928 velocity.confidence.value = static_cast<uint8_t>(confidence);
929 }
930}
931
944inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
945 const double x_std = std::numeric_limits<double>::infinity(),
946 const double y_std = std::numeric_limits<double>::infinity(),
947 const double z_std = std::numeric_limits<double>::infinity()) {
948 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
949 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
950 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
951 if (cartesian_velocity.z != 0.0) {
952 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
953 object.velocity.cartesian_velocity.z_velocity_is_present = true;
954 }
955 object.velocity_is_present = true;
956}
957
969inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
970 const double confidence = std::numeric_limits<double>::infinity()) {
971 // limit value range
972 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
973 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
974 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
975 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
976 } else {
977 acceleration.value.value = static_cast<int16_t>(value);
978 }
979
980 // limit confidence range
981 if(confidence == std::numeric_limits<double>::infinity()) {
982 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
983 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
984 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
985 } else {
986 acceleration.confidence.value = static_cast<uint8_t>(confidence);
987 }
988}
989
1002inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1003 const double x_std = std::numeric_limits<double>::infinity(),
1004 const double y_std = std::numeric_limits<double>::infinity(),
1005 const double z_std = std::numeric_limits<double>::infinity()) {
1006 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1007 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1008 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1009 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1010 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1011 if (cartesian_acceleration.z != 0.0) {
1012 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1013 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1014 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1015 }
1016 object.acceleration_is_present = true;
1017}
1018
1029inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1030 double yaw_std = std::numeric_limits<double>::infinity()) {
1031 // wrap angle to range [0, 360)
1032 double yaw_in_degrees = yaw * 180 / M_PI;
1033 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1034 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1035 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1036
1037 if(yaw_std == std::numeric_limits<double>::infinity()) {
1038 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1039 } else {
1040 yaw_std *= 180.0 / M_PI;
1041 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1042
1043 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1044 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1045 } else {
1046 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1047 }
1048 }
1049 object.angles_is_present = true;
1050}
1051
1063inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1064 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1065 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1066 // limit value range
1067 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1068 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1069 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1070 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1071 }
1072
1073 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1074
1075 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1076 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1077 } else {
1078 yaw_rate_std *= 180.0 / M_PI;
1079 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1080 // How stupid is this?!
1081 static const std::map<double, uint8_t> confidence_map = {
1082 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1083 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1084 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1085 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1086 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1087 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1088 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1089 };
1090 for(const auto& [thresh, conf_val] : confidence_map) {
1091 if (yaw_rate_std <= thresh) {
1092 object.z_angular_velocity.confidence.value = conf_val;
1093 break;
1094 }
1095 }
1096 }
1097
1098 object.z_angular_velocity_is_present = true;
1099}
1100
1115inline void setObjectDimension(ObjectDimension& dimension, const double value,
1116 const double confidence = std::numeric_limits<double>::infinity()) {
1117 // limit value range
1118 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1119 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1120 } else {
1121 dimension.value.value = static_cast<uint16_t>(value);
1122 }
1123
1124 // limit confidence range
1125 if (confidence == std::numeric_limits<double>::infinity()) {
1126 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1127 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1128 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1129 } else {
1130 dimension.confidence.value = static_cast<uint8_t>(confidence);
1131 }
1132
1133}
1134
1145inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1146 const double std = std::numeric_limits<double>::infinity()) {
1147 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1148 object.object_dimension_x_is_present = true;
1149}
1150
1161inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1162 const double std = std::numeric_limits<double>::infinity()) {
1163 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1164 object.object_dimension_y_is_present = true;
1165}
1166
1177inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1178 const double std = std::numeric_limits<double>::infinity()) {
1179 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1180 object.object_dimension_z_is_present = true;
1181}
1182
1194inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1195 const double x_std = std::numeric_limits<double>::infinity(),
1196 const double y_std = std::numeric_limits<double>::infinity(),
1197 const double z_std = std::numeric_limits<double>::infinity()) {
1198 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1199 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1200 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1201}
1202
1212inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1213 setPositionOfPerceivedObject(object, point);
1214 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1215}
1216
1229inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1230 const gm::PointStamped& point, const int16_t delta_time = 0) {
1231 setUTMPositionOfPerceivedObject(cpm, object, point);
1232 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1233}
1234
1244inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1245 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1246 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1247}
1248
1262inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1263 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1264 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1265 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1266 container.container_data_perceived_object_container.perceived_objects.array.size();
1267 } else {
1268 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1269 }
1270}
1271
1284inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1285 // check for maximum number of containers
1286 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1287 cpm.payload.cpm_containers.value.array.push_back(container);
1288 } else {
1289 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1290 }
1291}
1292
1301inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1302 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1303 covariance_matrix);
1304}
1305
1306} // namespace etsi_its_cpm_ts_msgs::access

◆ setZDimensionOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setZDimensionOfPerceivedObject ( PerceivedObject & object,
const double value,
const double std = std::numeric_limits<double>::infinity() )
inline

Sets the z-dimension of a perceived object.

This function sets the z-dimension of the given PerceivedObject to the specified value. The z-dimension usually represents the height of the object.

Parameters
objectThe PerceivedObject to modify.
valueThe value to set as the z-dimension in meters.
stdThe standard deviation of the z-dimension value in meters (optional, default is infinity).

Definition at line 683 of file cpm_ts_setters.h.

719 {
720
721#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
722
732inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
733 const uint8_t protocol_version = 0) {
734 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
735}
736
748inline void setReferenceTime(
749 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
750 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
751 TimestampIts t_its;
752 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
753 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
754 cpm.payload.management_container.reference_time = t_its;
755}
756
768inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
769 const double altitude = AltitudeValue::UNAVAILABLE) {
770 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
771}
772
785inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
786 const bool& northp) {
787 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
788}
789
798inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
799 object.object_id.value = id;
800 object.object_id_is_present = true;
801}
802
814inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
815 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
816 throw std::invalid_argument("MeasurementDeltaTime out of range");
817 } else {
818 object.measurement_delta_time.value = delta_time;
819 }
820}
821
835inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
836 const double confidence = std::numeric_limits<double>::infinity()) {
837 // limit value range
838 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
839 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
840 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
841 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
842 } else {
843 coordinate.value.value = static_cast<int32_t>(value);
844 }
845
846 // limit confidence range
847 if (confidence == std::numeric_limits<double>::infinity()) {
848 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
849 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
850 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
851 } else {
852 coordinate.confidence.value = static_cast<uint16_t>(confidence);
853 }
854}
855
867inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
868 const double x_std = std::numeric_limits<double>::infinity(),
869 const double y_std = std::numeric_limits<double>::infinity(),
870 const double z_std = std::numeric_limits<double>::infinity()) {
871 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
872 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
873 if (point.z != 0.0) {
874 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
875 object.position.z_coordinate_is_present = true;
876 }
877}
878
894inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
895 const gm::PointStamped& utm_position,
896 const double x_std = std::numeric_limits<double>::infinity(),
897 const double y_std = std::numeric_limits<double>::infinity(),
898 const double z_std = std::numeric_limits<double>::infinity()) {
899 gm::PointStamped reference_position = getUTMPosition(cpm);
900 if (utm_position.header.frame_id != reference_position.header.frame_id) {
901 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
902 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
903 ")");
904 }
905 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
906 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
907 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
908 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
909 if (utm_position.point.z != 0.0) {
910 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
911 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
912 object.position.z_coordinate_is_present = true;
913 }
914}
915
927inline void setVelocityComponent(VelocityComponent& velocity, const double value,
928 const double confidence = std::numeric_limits<double>::infinity()) {
929 // limit value range
930 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
931 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
932 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
933 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
934 } else {
935 velocity.value.value = static_cast<int16_t>(value);
936 }
937
938 // limit confidence range
939 if(confidence == std::numeric_limits<double>::infinity()) {
940 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
941 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
942 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
943 } else {
944 velocity.confidence.value = static_cast<uint8_t>(confidence);
945 }
946}
947
960inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
961 const double x_std = std::numeric_limits<double>::infinity(),
962 const double y_std = std::numeric_limits<double>::infinity(),
963 const double z_std = std::numeric_limits<double>::infinity()) {
964 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
965 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
966 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
967 if (cartesian_velocity.z != 0.0) {
968 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
969 object.velocity.cartesian_velocity.z_velocity_is_present = true;
970 }
971 object.velocity_is_present = true;
972}
973
985inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
986 const double confidence = std::numeric_limits<double>::infinity()) {
987 // limit value range
988 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
989 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
990 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
991 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
992 } else {
993 acceleration.value.value = static_cast<int16_t>(value);
994 }
995
996 // limit confidence range
997 if(confidence == std::numeric_limits<double>::infinity()) {
998 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
999 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1000 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1001 } else {
1002 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1003 }
1004}
1005
1018inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1019 const double x_std = std::numeric_limits<double>::infinity(),
1020 const double y_std = std::numeric_limits<double>::infinity(),
1021 const double z_std = std::numeric_limits<double>::infinity()) {
1022 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1023 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1024 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1025 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1026 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1027 if (cartesian_acceleration.z != 0.0) {
1028 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1029 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1030 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1031 }
1032 object.acceleration_is_present = true;
1033}
1034
1045inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1046 double yaw_std = std::numeric_limits<double>::infinity()) {
1047 // wrap angle to range [0, 360)
1048 double yaw_in_degrees = yaw * 180 / M_PI;
1049 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1050 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1051 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1052
1053 if(yaw_std == std::numeric_limits<double>::infinity()) {
1054 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1055 } else {
1056 yaw_std *= 180.0 / M_PI;
1057 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1058
1059 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1060 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1061 } else {
1062 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1063 }
1064 }
1065 object.angles_is_present = true;
1066}
1067
1079inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1080 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1081 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1082 // limit value range
1083 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1084 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1085 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1086 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1087 }
1088
1089 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1090
1091 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1092 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1093 } else {
1094 yaw_rate_std *= 180.0 / M_PI;
1095 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1096 // How stupid is this?!
1097 static const std::map<double, uint8_t> confidence_map = {
1098 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1099 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1100 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1101 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1102 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1103 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1104 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1105 };
1106 for(const auto& [thresh, conf_val] : confidence_map) {
1107 if (yaw_rate_std <= thresh) {
1108 object.z_angular_velocity.confidence.value = conf_val;
1109 break;
1110 }
1111 }
1112 }
1113
1114 object.z_angular_velocity_is_present = true;
1115}
1116
1131inline void setObjectDimension(ObjectDimension& dimension, const double value,
1132 const double confidence = std::numeric_limits<double>::infinity()) {
1133 // limit value range
1134 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1135 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1136 } else {
1137 dimension.value.value = static_cast<uint16_t>(value);
1138 }
1139
1140 // limit confidence range
1141 if (confidence == std::numeric_limits<double>::infinity()) {
1142 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1143 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1144 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1145 } else {
1146 dimension.confidence.value = static_cast<uint8_t>(confidence);
1147 }
1148
1149}
1150
1161inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1162 const double std = std::numeric_limits<double>::infinity()) {
1163 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1164 object.object_dimension_x_is_present = true;
1165}
1166
1177inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1178 const double std = std::numeric_limits<double>::infinity()) {
1179 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1180 object.object_dimension_y_is_present = true;
1181}
1182
1193inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1194 const double std = std::numeric_limits<double>::infinity()) {
1195 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1196 object.object_dimension_z_is_present = true;
1197}
1198
1210inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1211 const double x_std = std::numeric_limits<double>::infinity(),
1212 const double y_std = std::numeric_limits<double>::infinity(),
1213 const double z_std = std::numeric_limits<double>::infinity()) {
1214 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1215 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1216 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1217}
1218
1228inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1229 setPositionOfPerceivedObject(object, point);
1230 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1231}
1232
1245inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1246 const gm::PointStamped& point, const int16_t delta_time = 0) {
1247 setUTMPositionOfPerceivedObject(cpm, object, point);
1248 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1249}
1250
1260inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1261 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1262 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1263}
1264
1278inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1279 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1280 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1281 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1282 container.container_data_perceived_object_container.perceived_objects.array.size();
1283 } else {
1284 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1285 }
1286}
1287
1300inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1301 // check for maximum number of containers
1302 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1303 cpm.payload.cpm_containers.value.array.push_back(container);
1304 } else {
1305 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1306 }
1307}
1308
1317inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1318 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1319 covariance_matrix);
1320}
1321
1322} // namespace etsi_its_cpm_ts_msgs::access

◆ throwIfNotPresent()

void etsi_its_cpm_ts_msgs::access::throwIfNotPresent ( const bool is_present,
const std::string val_desc )
inline

Throws an exception if the given value is not present.

Parameters
is_presentWhether the value is present.
val_descDescription of the value for the exception message.

Definition at line 58 of file cpm_ts_setters.h.

◆ throwIfOutOfRange()

template<typename T1, typename T2>
void etsi_its_cpm_ts_msgs::access::throwIfOutOfRange ( const T1 & val,
const T2 & min,
const T2 & max,
const std::string val_desc )

Throws an exception if a given value is out of a defined range.

Template Parameters
T1
T2
Parameters
valThe value to check if it is in the range.
minThe minimum value of the range.
maxThe maximum value of the range.
val_descDescription of the value for the exception message.

Definition at line 47 of file cpm_ts_setters.h.

51 {

Variable Documentation

◆ LEAP_SECOND_INSERTIONS_SINCE_2004

const std::map<uint64_t, uint16_t> etsi_its_cpm_ts_msgs::access::etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004
Initial value:
{
{UNIX_SECONDS_2004, 0},
{1136073599, 1},
{1230767999, 2},
{1341100799, 3},
{1435708799, 4},
{1483228799, 5}
}

std::map that stores all leap second insertions since 2004 with the corresponding unix-date of the insertion

Definition at line 46 of file cpm_ts_setters.h.

51 {
52 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
53}

◆ ONE_D_GAUSSIAN_FACTOR

const double etsi_its_cpm_ts_msgs::access::etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR = 2.0
constexpr

Definition at line 72 of file cpm_ts_setters.h.

◆ TWO_D_GAUSSIAN_FACTOR

const double etsi_its_cpm_ts_msgs::access::etsi_its_msgs::TWO_D_GAUSSIAN_FACTOR = 2.4477
constexpr

Definition at line 76 of file cpm_ts_setters.h.

◆ UNIX_SECONDS_2004

const uint64_t etsi_its_cpm_ts_msgs::access::etsi_its_msgs::UNIX_SECONDS_2004 = 1072915200

Definition at line 40 of file cpm_ts_setters.h.