etsi_its_messages v3.3.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 AccelerationMagnitudeValue>
void etsi_its_cpm_ts_msgs::access::setAccelerationMagnitudeValue (AccelerationMagnitudeValue &accel_mag_value, const double value)
 Set the Acceleration Magnitude Value object.
 
template<typename AccelerationConfidence>
void etsi_its_cpm_ts_msgs::access::setAccelerationMagnitudeConfidence (AccelerationConfidence &accel_mag_confidence, const double value)
 Set the AccelerationMagnitude Confidence object.
 
template<typename AccelerationMagnitude>
void etsi_its_cpm_ts_msgs::access::setAccelerationMagnitude (AccelerationMagnitude &accel_mag, const double value, const double confidence=std::numeric_limits< double >::infinity())
 Set the AccelerationMagnitude 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 YawRate, typename YawRateValue = decltype(YawRate::yaw_rate_value), typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
void etsi_its_cpm_ts_msgs::access::setYawRateCDD (YawRate &yaw_rate, const double value, double confidence=std::numeric_limits< double >::infinity())
 Set the Yaw Rate 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::setCartesianAngle (CartesianAngle &angle, const double value, double confidence=std::numeric_limits< double >::infinity())
 Set a Cartesian Angle.
 
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::setPolarVelocityOfPerceivedObject (PerceivedObject &object, const double magnitude, const double angle, const double z=0.0, const double magnitude_std=std::numeric_limits< double >::infinity(), const double angle_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::setPolarAccelerationOfPerceivedObject (PerceivedObject &object, const double magnitude, const double angle, const double z=0.0, const double magnitude_std=std::numeric_limits< double >::infinity(), const double angle_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
 Set the Polar Acceleration Of Perceived Object 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.
 
void etsi_its_cpm_ts_msgs::access::initSensorInformationContainer (WrappedCpmContainer &container)
 Initializes a WrappedCpmContainer as a SensorInformationContainer.
 
void etsi_its_cpm_ts_msgs::access::setSensorID (SensorInformation &sensor_information, const uint8_t sensor_id=0)
 Sets the sensorId of a SensorInformation object.
 
void etsi_its_cpm_ts_msgs::access::setSensorType (SensorInformation &sensor_information, const uint8_t sensor_type=SensorType::UNDEFINED)
 Sets the sensorType of a SensorInformation object.
 
void etsi_its_cpm_ts_msgs::access::addSensorInformationToContainer (WrappedCpmContainer &container, const SensorInformation &sensor_information)
 Adds a SensorInformation to the SensorInformationContainer / WrappedCpmContainer.
 
void etsi_its_cpm_ts_msgs::access::addSensorInformationContainerToCPM (CollectivePerceptionMessage &cpm, const WrappedCpmContainer &container)
 Adds a container to the Collective Perception Message (CPM).
 

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 882 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 860 of file cpm_ts_setters.h.

896 {
897
898#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
899
909inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
910 const uint8_t protocol_version = 0) {
911 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
912}
913
925inline void setReferenceTime(
926 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
927 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
928 TimestampIts t_its;
929 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
930 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
931 cpm.payload.management_container.reference_time = t_its;
932}
933
945inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
946 const double altitude = AltitudeValue::UNAVAILABLE) {
947 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
948}
949
962inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
963 const bool& northp) {
964 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
965}
966
975inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
976 object.object_id.value = id;
977 object.object_id_is_present = true;
978}
979
991inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
992 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
993 throw std::invalid_argument("MeasurementDeltaTime out of range");
994 } else {
995 object.measurement_delta_time.value = delta_time;
996 }
997}
998
1012inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
1013 const double confidence = std::numeric_limits<double>::infinity()) {
1014 // limit value range
1015 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
1016 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
1017 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1018 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1019 } else {
1020 coordinate.value.value = static_cast<int32_t>(value);
1021 }
1022
1023 // limit confidence range
1024 if (confidence == std::numeric_limits<double>::infinity()) {
1025 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1026 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1027 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1028 } else {
1029 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1030 }
1031}
1032
1044inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
1045 const double x_std = std::numeric_limits<double>::infinity(),
1046 const double y_std = std::numeric_limits<double>::infinity(),
1047 const double z_std = std::numeric_limits<double>::infinity()) {
1048 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1049 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1050 if (point.z != 0.0) {
1051 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1052 object.position.z_coordinate_is_present = true;
1053 }
1054}
1055
1071inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1072 const gm::PointStamped& utm_position,
1073 const double x_std = std::numeric_limits<double>::infinity(),
1074 const double y_std = std::numeric_limits<double>::infinity(),
1075 const double z_std = std::numeric_limits<double>::infinity()) {
1076 gm::PointStamped reference_position = getUTMPosition(cpm);
1077 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1078 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1079 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1080 ")");
1081 }
1082 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1083 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1084 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1085 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1086 if (utm_position.point.z != 0.0) {
1087 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1088 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1089 object.position.z_coordinate_is_present = true;
1090 }
1091}
1092
1104inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1105 const double confidence = std::numeric_limits<double>::infinity()) {
1106 // limit value range
1107 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1108 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1109 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1110 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1111 } else {
1112 velocity.value.value = static_cast<int16_t>(value);
1113 }
1114
1115 // limit confidence range
1116 if(confidence == std::numeric_limits<double>::infinity()) {
1117 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1118 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1119 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1120 } else {
1121 velocity.confidence.value = static_cast<uint8_t>(confidence);
1122 }
1123}
1124
1132inline void setCartesianAngle(CartesianAngle& angle, const double value,
1133 double confidence = std::numeric_limits<double>::infinity()) {
1134 // wrap angle to 0..2pi
1135 double wrapped_value = fmod(value, 2*M_PI);
1136 if (wrapped_value < 0.0) {
1137 wrapped_value += 2 * M_PI;
1138 }
1139 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1140
1141 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1142 // limit confidence range
1143 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1144 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1145 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1146 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1147 } else {
1148 angle.confidence.value = static_cast<uint8_t>(confidence);
1149 }
1150}
1151
1164inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1165 const double x_std = std::numeric_limits<double>::infinity(),
1166 const double y_std = std::numeric_limits<double>::infinity(),
1167 const double z_std = std::numeric_limits<double>::infinity()) {
1168 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1169 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1170 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1171 if (cartesian_velocity.z != 0.0) {
1172 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1173 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1174 } else {
1175 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1176 }
1177 object.velocity_is_present = true;
1178}
1179
1194inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1195 const double magnitude,
1196 const double angle,
1197 const double z = 0.0,
1198 const double magnitude_std = std::numeric_limits<double>::infinity(),
1199 const double angle_std = std::numeric_limits<double>::infinity(),
1200 const double z_std = std::numeric_limits<double>::infinity()) {
1201 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1202 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1203 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1204 if (z != 0.0) {
1205 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1206 object.velocity.polar_velocity.z_velocity_is_present = true;
1207 } else {
1208 object.velocity.polar_velocity.z_velocity_is_present = false;
1209 }
1210}
1211
1223inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1224 const double confidence = std::numeric_limits<double>::infinity()) {
1225 // limit value range
1226 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1227 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1228 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1229 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1230 } else {
1231 acceleration.value.value = static_cast<int16_t>(value);
1232 }
1233
1234 // limit confidence range
1235 if(confidence == std::numeric_limits<double>::infinity()) {
1236 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1237 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1238 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1239 } else {
1240 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1241 }
1242}
1243
1256inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1257 const double x_std = std::numeric_limits<double>::infinity(),
1258 const double y_std = std::numeric_limits<double>::infinity(),
1259 const double z_std = std::numeric_limits<double>::infinity()) {
1260 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1261 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1262 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1263 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1264 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1265 if (cartesian_acceleration.z != 0.0) {
1266 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1267 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1268 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1269 }
1270 object.acceleration_is_present = true;
1271}
1272
1284inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1285 const double magnitude,
1286 const double angle,
1287 const double z = 0.0,
1288 const double magnitude_std = std::numeric_limits<double>::infinity(),
1289 const double angle_std = std::numeric_limits<double>::infinity(),
1290 const double z_std = std::numeric_limits<double>::infinity()) {
1291 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1292 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1293 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1294 if (z != 0.0) {
1295 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1296 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1297 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1298 } else {
1299 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1300 }
1301 object.acceleration_is_present = true;
1302}
1303
1314inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1315 double yaw_std = std::numeric_limits<double>::infinity()) {
1316 // wrap angle to range [0, 360)
1317 double yaw_in_degrees = yaw * 180 / M_PI;
1318 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1319 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1320 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1321
1322 if(yaw_std == std::numeric_limits<double>::infinity()) {
1323 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1324 } else {
1325 yaw_std *= 180.0 / M_PI;
1326 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1327
1328 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1329 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1330 } else {
1331 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1332 }
1333 }
1334 object.angles_is_present = true;
1335}
1336
1348inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1349 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1350 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1351 // limit value range
1352 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1353 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1354 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1355 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1356 }
1357
1358 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1359
1360 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1361 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1362 } else {
1363 yaw_rate_std *= 180.0 / M_PI;
1364 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1365 // How stupid is this?!
1366 static const std::map<double, uint8_t> confidence_map = {
1367 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1368 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1369 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1370 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1371 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1372 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1373 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1374 };
1375 for(const auto& [thresh, conf_val] : confidence_map) {
1376 if (yaw_rate_std <= thresh) {
1377 object.z_angular_velocity.confidence.value = conf_val;
1378 break;
1379 }
1380 }
1381 }
1382
1383 object.z_angular_velocity_is_present = true;
1384}
1385
1400inline void setObjectDimension(ObjectDimension& dimension, const double value,
1401 const double confidence = std::numeric_limits<double>::infinity()) {
1402 // limit value range
1403 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1404 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1405 } else {
1406 dimension.value.value = static_cast<uint16_t>(value);
1407 }
1408
1409 // limit confidence range
1410 if (confidence == std::numeric_limits<double>::infinity()) {
1411 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1412 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1413 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1414 } else {
1415 dimension.confidence.value = static_cast<uint8_t>(confidence);
1416 }
1417
1418}
1419
1430inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1431 const double std = std::numeric_limits<double>::infinity()) {
1432 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1433 object.object_dimension_x_is_present = true;
1434}
1435
1446inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1447 const double std = std::numeric_limits<double>::infinity()) {
1448 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1449 object.object_dimension_y_is_present = true;
1450}
1451
1462inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1463 const double std = std::numeric_limits<double>::infinity()) {
1464 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1465 object.object_dimension_z_is_present = true;
1466}
1467
1479inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1480 const double x_std = std::numeric_limits<double>::infinity(),
1481 const double y_std = std::numeric_limits<double>::infinity(),
1482 const double z_std = std::numeric_limits<double>::infinity()) {
1483 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1484 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1485 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1486}
1487
1497inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1498 setPositionOfPerceivedObject(object, point);
1499 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1500}
1501
1514inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1515 const gm::PointStamped& point, const int16_t delta_time = 0) {
1516 setUTMPositionOfPerceivedObject(cpm, object, point);
1517 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1518}
1519
1529inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1530 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1531 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1532}
1533
1547inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1548 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1549 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1550 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1551 container.container_data_perceived_object_container.perceived_objects.array.size();
1552 } else {
1553 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1554 }
1555}
1556
1569inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1570 // check for maximum number of containers
1571 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1572 cpm.payload.cpm_containers.value.array.push_back(container);
1573 } else {
1574 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1575 }
1576}
1577
1586inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1587 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1588 covariance_matrix);
1589}
1590
1599inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1600 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1601 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1602}
1603
1614inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1615 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1616 sensor_information.sensor_id.value = sensor_id;
1617}
1618
1629inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1630 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1631 sensor_information.sensor_type.value = sensor_type;
1632}
1633
1652inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1653 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1654 // check for maximum number of SensorInformation entries
1655 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1656 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1657 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1658 container.container_data_sensor_information_container.array.push_back(sensor_information);
1659 } else {
1660 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1661 }
1662 } else {
1663 throw std::invalid_argument("Container is not a SensorInformationContainer");
1664 }
1665}
1666
1681inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1682 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1683 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1684 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1685 );
1686 addContainerToCPM(cpm, container);
1687 } else {
1688 throw std::invalid_argument("Container is not a SensorInformationContainer");
1689 }
1690}
1691
1692} // 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:211
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:130
etsi_its_cpm_ts_msgs::access::setCartesianAngle
void setCartesianAngle(CartesianAngle &angle, const double value, double confidence=std::numeric_limits< double >::infinity())
Set a Cartesian Angle.
Definition cpm_ts_setters.h:445
etsi_its_cpm_ts_msgs::access::addSensorInformationToContainer
void addSensorInformationToContainer(WrappedCpmContainer &container, const SensorInformation &sensor_information)
Adds a SensorInformation to the SensorInformationContainer / WrappedCpmContainer.
Definition cpm_ts_setters.h:965
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:477
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:539
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:536
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:569
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:842
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:792
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:759
etsi_its_cpm_ts_msgs::access::addSensorInformationContainerToCPM
void addSensorInformationContainerToCPM(CollectivePerceptionMessage &cpm, const WrappedCpmContainer &container)
Adds a container to the Collective Perception Message (CPM).
Definition cpm_ts_setters.h:994
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:899
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:661
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:314
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:627
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:860
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::setPolarVelocityOfPerceivedObject
void setPolarVelocityOfPerceivedObject(PerceivedObject &object, const double magnitude, const double angle, const double z=0.0, const double magnitude_std=std::numeric_limits< double >::infinity(), const double angle_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
Definition cpm_ts_setters.h:507
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:882
etsi_its_cpm_ts_msgs::access::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 cpm_ts_setters.h:47
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::setAccelerationMagnitude
void setAccelerationMagnitude(AccelerationMagnitude &accel_mag, const double value, const double confidence=std::numeric_limits< double >::infinity())
Set the AccelerationMagnitude object.
Definition cpm_ts_setters.h:253
etsi_its_cpm_ts_msgs::access::setPolarAccelerationOfPerceivedObject
void setPolarAccelerationOfPerceivedObject(PerceivedObject &object, const double magnitude, const double angle, const double z=0.0, const double magnitude_std=std::numeric_limits< double >::infinity(), const double angle_std=std::numeric_limits< double >::infinity(), const double z_std=std::numeric_limits< double >::infinity())
Set the Polar Acceleration Of Perceived Object object.
Definition cpm_ts_setters.h:597
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:713
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:743
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:775
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::setSensorType
void setSensorType(SensorInformation &sensor_information, const uint8_t sensor_type=SensorType::UNDEFINED)
Sets the sensorType of a SensorInformation object.
Definition cpm_ts_setters.h:942
etsi_its_cpm_ts_msgs::access::setSpeed
void setSpeed(Speed &speed, const double value, const double confidence=std::numeric_limits< double >::infinity())
Set the Speed object.
Definition cpm_ts_setters.h:208
etsi_its_cpm_ts_msgs::access::initSensorInformationContainer
void initSensorInformationContainer(WrappedCpmContainer &container)
Initializes a WrappedCpmContainer as a SensorInformationContainer.
Definition cpm_ts_setters.h:912
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:827
etsi_its_cpm_ts_msgs::access::setSensorID
void setSensorID(SensorInformation &sensor_information, const uint8_t sensor_id=0)
Sets the sensorId of a SensorInformation object.
Definition cpm_ts_setters.h:927
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:810

◆ addSensorInformationContainerToCPM()

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

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

This function adds a SensorInformationContainer / WrappedCpmContainer to the CPM's payload. It first checks if the container is a SensorInformationContainer. If so, it checks if the number of entries is in the allowed range. If the number of entries is valid, 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 container is not a SensorInformationContainer.
std::invalid_argumentif the number of SensorInformation entries is out of range.

Definition at line 994 of file cpm_ts_setters.h.

1030 {
1031
1032#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
1033
1043inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
1044 const uint8_t protocol_version = 0) {
1045 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
1046}
1047
1059inline void setReferenceTime(
1060 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
1061 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
1062 TimestampIts t_its;
1063 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
1064 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
1065 cpm.payload.management_container.reference_time = t_its;
1066}
1067
1079inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
1080 const double altitude = AltitudeValue::UNAVAILABLE) {
1081 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
1082}
1083
1096inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
1097 const bool& northp) {
1098 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
1099}
1100
1109inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
1110 object.object_id.value = id;
1111 object.object_id_is_present = true;
1112}
1113
1125inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
1126 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
1127 throw std::invalid_argument("MeasurementDeltaTime out of range");
1128 } else {
1129 object.measurement_delta_time.value = delta_time;
1130 }
1131}
1132
1146inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
1147 const double confidence = std::numeric_limits<double>::infinity()) {
1148 // limit value range
1149 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
1150 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
1151 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1152 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1153 } else {
1154 coordinate.value.value = static_cast<int32_t>(value);
1155 }
1156
1157 // limit confidence range
1158 if (confidence == std::numeric_limits<double>::infinity()) {
1159 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1160 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1161 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1162 } else {
1163 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1164 }
1165}
1166
1178inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
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 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1183 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1184 if (point.z != 0.0) {
1185 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1186 object.position.z_coordinate_is_present = true;
1187 }
1188}
1189
1205inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1206 const gm::PointStamped& utm_position,
1207 const double x_std = std::numeric_limits<double>::infinity(),
1208 const double y_std = std::numeric_limits<double>::infinity(),
1209 const double z_std = std::numeric_limits<double>::infinity()) {
1210 gm::PointStamped reference_position = getUTMPosition(cpm);
1211 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1212 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1213 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1214 ")");
1215 }
1216 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1217 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1218 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1219 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1220 if (utm_position.point.z != 0.0) {
1221 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1222 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1223 object.position.z_coordinate_is_present = true;
1224 }
1225}
1226
1238inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1239 const double confidence = std::numeric_limits<double>::infinity()) {
1240 // limit value range
1241 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1242 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1243 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1244 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1245 } else {
1246 velocity.value.value = static_cast<int16_t>(value);
1247 }
1248
1249 // limit confidence range
1250 if(confidence == std::numeric_limits<double>::infinity()) {
1251 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1252 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1253 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1254 } else {
1255 velocity.confidence.value = static_cast<uint8_t>(confidence);
1256 }
1257}
1258
1266inline void setCartesianAngle(CartesianAngle& angle, const double value,
1267 double confidence = std::numeric_limits<double>::infinity()) {
1268 // wrap angle to 0..2pi
1269 double wrapped_value = fmod(value, 2*M_PI);
1270 if (wrapped_value < 0.0) {
1271 wrapped_value += 2 * M_PI;
1272 }
1273 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1274
1275 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1276 // limit confidence range
1277 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1278 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1279 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1280 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1281 } else {
1282 angle.confidence.value = static_cast<uint8_t>(confidence);
1283 }
1284}
1285
1298inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1299 const double x_std = std::numeric_limits<double>::infinity(),
1300 const double y_std = std::numeric_limits<double>::infinity(),
1301 const double z_std = std::numeric_limits<double>::infinity()) {
1302 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1303 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1304 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1305 if (cartesian_velocity.z != 0.0) {
1306 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1307 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1308 } else {
1309 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1310 }
1311 object.velocity_is_present = true;
1312}
1313
1328inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1329 const double magnitude,
1330 const double angle,
1331 const double z = 0.0,
1332 const double magnitude_std = std::numeric_limits<double>::infinity(),
1333 const double angle_std = std::numeric_limits<double>::infinity(),
1334 const double z_std = std::numeric_limits<double>::infinity()) {
1335 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1336 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1337 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1338 if (z != 0.0) {
1339 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1340 object.velocity.polar_velocity.z_velocity_is_present = true;
1341 } else {
1342 object.velocity.polar_velocity.z_velocity_is_present = false;
1343 }
1344}
1345
1357inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1358 const double confidence = std::numeric_limits<double>::infinity()) {
1359 // limit value range
1360 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1361 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1362 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1363 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1364 } else {
1365 acceleration.value.value = static_cast<int16_t>(value);
1366 }
1367
1368 // limit confidence range
1369 if(confidence == std::numeric_limits<double>::infinity()) {
1370 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1371 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1372 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1373 } else {
1374 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1375 }
1376}
1377
1390inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1391 const double x_std = std::numeric_limits<double>::infinity(),
1392 const double y_std = std::numeric_limits<double>::infinity(),
1393 const double z_std = std::numeric_limits<double>::infinity()) {
1394 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1395 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1396 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1397 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1398 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1399 if (cartesian_acceleration.z != 0.0) {
1400 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1401 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1402 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1403 }
1404 object.acceleration_is_present = true;
1405}
1406
1418inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1419 const double magnitude,
1420 const double angle,
1421 const double z = 0.0,
1422 const double magnitude_std = std::numeric_limits<double>::infinity(),
1423 const double angle_std = std::numeric_limits<double>::infinity(),
1424 const double z_std = std::numeric_limits<double>::infinity()) {
1425 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1426 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1427 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1428 if (z != 0.0) {
1429 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1430 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1431 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1432 } else {
1433 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1434 }
1435 object.acceleration_is_present = true;
1436}
1437
1448inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1449 double yaw_std = std::numeric_limits<double>::infinity()) {
1450 // wrap angle to range [0, 360)
1451 double yaw_in_degrees = yaw * 180 / M_PI;
1452 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1453 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1454 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1455
1456 if(yaw_std == std::numeric_limits<double>::infinity()) {
1457 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1458 } else {
1459 yaw_std *= 180.0 / M_PI;
1460 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1461
1462 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1463 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1464 } else {
1465 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1466 }
1467 }
1468 object.angles_is_present = true;
1469}
1470
1482inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1483 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1484 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1485 // limit value range
1486 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1487 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1488 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1489 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1490 }
1491
1492 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1493
1494 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1495 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1496 } else {
1497 yaw_rate_std *= 180.0 / M_PI;
1498 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1499 // How stupid is this?!
1500 static const std::map<double, uint8_t> confidence_map = {
1501 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1502 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1503 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1504 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1505 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1506 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1507 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1508 };
1509 for(const auto& [thresh, conf_val] : confidence_map) {
1510 if (yaw_rate_std <= thresh) {
1511 object.z_angular_velocity.confidence.value = conf_val;
1512 break;
1513 }
1514 }
1515 }
1516
1517 object.z_angular_velocity_is_present = true;
1518}
1519
1534inline void setObjectDimension(ObjectDimension& dimension, const double value,
1535 const double confidence = std::numeric_limits<double>::infinity()) {
1536 // limit value range
1537 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1538 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1539 } else {
1540 dimension.value.value = static_cast<uint16_t>(value);
1541 }
1542
1543 // limit confidence range
1544 if (confidence == std::numeric_limits<double>::infinity()) {
1545 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1546 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1547 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1548 } else {
1549 dimension.confidence.value = static_cast<uint8_t>(confidence);
1550 }
1551
1552}
1553
1564inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1565 const double std = std::numeric_limits<double>::infinity()) {
1566 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1567 object.object_dimension_x_is_present = true;
1568}
1569
1580inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1581 const double std = std::numeric_limits<double>::infinity()) {
1582 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1583 object.object_dimension_y_is_present = true;
1584}
1585
1596inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1597 const double std = std::numeric_limits<double>::infinity()) {
1598 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1599 object.object_dimension_z_is_present = true;
1600}
1601
1613inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1614 const double x_std = std::numeric_limits<double>::infinity(),
1615 const double y_std = std::numeric_limits<double>::infinity(),
1616 const double z_std = std::numeric_limits<double>::infinity()) {
1617 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1618 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1619 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1620}
1621
1631inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1632 setPositionOfPerceivedObject(object, point);
1633 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1634}
1635
1648inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1649 const gm::PointStamped& point, const int16_t delta_time = 0) {
1650 setUTMPositionOfPerceivedObject(cpm, object, point);
1651 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1652}
1653
1663inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1664 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1665 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1666}
1667
1681inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1682 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1683 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1684 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1685 container.container_data_perceived_object_container.perceived_objects.array.size();
1686 } else {
1687 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1688 }
1689}
1690
1703inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1704 // check for maximum number of containers
1705 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1706 cpm.payload.cpm_containers.value.array.push_back(container);
1707 } else {
1708 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1709 }
1710}
1711
1720inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1721 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1722 covariance_matrix);
1723}
1724
1733inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1734 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1735 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1736}
1737
1748inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1749 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1750 sensor_information.sensor_id.value = sensor_id;
1751}
1752
1763inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1764 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1765 sensor_information.sensor_type.value = sensor_type;
1766}
1767
1786inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1787 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1788 // check for maximum number of SensorInformation entries
1789 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1790 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1791 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1792 container.container_data_sensor_information_container.array.push_back(sensor_information);
1793 } else {
1794 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1795 }
1796 } else {
1797 throw std::invalid_argument("Container is not a SensorInformationContainer");
1798 }
1799}
1800
1815inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1816 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1817 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1818 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1819 );
1820 addContainerToCPM(cpm, container);
1821 } else {
1822 throw std::invalid_argument("Container is not a SensorInformationContainer");
1823 }
1824}
1825
1826} // namespace etsi_its_cpm_ts_msgs::access

◆ addSensorInformationToContainer()

void etsi_its_cpm_ts_msgs::access::addSensorInformationToContainer ( WrappedCpmContainer & container,
const SensorInformation & sensor_information )
inline

Adds a SensorInformation to the SensorInformationContainer / WrappedCpmContainer.

This function checks if the provided container is a SensorInformationContainer. If it is, the function adds the given SensorInformation to the container's sensor_information array. If the container is not a SensorInformationContainer, the function throws an std::invalid_argument exception. If the maximum number of SensorInformation entries is reached, it throws an exception. If the sensor_id or sensor_type is out of range, it throws an exception.

Parameters
containerThe WrappedCpmContainer to which the SensorInformation will be added.
sensor_informationThe SensorInformation to add to the container.
Exceptions
std::invalid_argumentif the container is not a SensorInformationContainer.
std::invalid_argumentif the maximum number of SensorInformation entries is reached.
std::invalid_argumentif the sensor_id or sensor_type is out of range.

Definition at line 965 of file cpm_ts_setters.h.

1001 {
1002
1003#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
1004
1014inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
1015 const uint8_t protocol_version = 0) {
1016 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
1017}
1018
1030inline void setReferenceTime(
1031 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
1032 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
1033 TimestampIts t_its;
1034 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
1035 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
1036 cpm.payload.management_container.reference_time = t_its;
1037}
1038
1050inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
1051 const double altitude = AltitudeValue::UNAVAILABLE) {
1052 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
1053}
1054
1067inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
1068 const bool& northp) {
1069 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
1070}
1071
1080inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
1081 object.object_id.value = id;
1082 object.object_id_is_present = true;
1083}
1084
1096inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
1097 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
1098 throw std::invalid_argument("MeasurementDeltaTime out of range");
1099 } else {
1100 object.measurement_delta_time.value = delta_time;
1101 }
1102}
1103
1117inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
1118 const double confidence = std::numeric_limits<double>::infinity()) {
1119 // limit value range
1120 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
1121 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
1122 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1123 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1124 } else {
1125 coordinate.value.value = static_cast<int32_t>(value);
1126 }
1127
1128 // limit confidence range
1129 if (confidence == std::numeric_limits<double>::infinity()) {
1130 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1131 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1132 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1133 } else {
1134 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1135 }
1136}
1137
1149inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
1150 const double x_std = std::numeric_limits<double>::infinity(),
1151 const double y_std = std::numeric_limits<double>::infinity(),
1152 const double z_std = std::numeric_limits<double>::infinity()) {
1153 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1154 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1155 if (point.z != 0.0) {
1156 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1157 object.position.z_coordinate_is_present = true;
1158 }
1159}
1160
1176inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1177 const gm::PointStamped& utm_position,
1178 const double x_std = std::numeric_limits<double>::infinity(),
1179 const double y_std = std::numeric_limits<double>::infinity(),
1180 const double z_std = std::numeric_limits<double>::infinity()) {
1181 gm::PointStamped reference_position = getUTMPosition(cpm);
1182 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1183 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1184 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1185 ")");
1186 }
1187 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1188 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1189 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1190 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1191 if (utm_position.point.z != 0.0) {
1192 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1193 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1194 object.position.z_coordinate_is_present = true;
1195 }
1196}
1197
1209inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1210 const double confidence = std::numeric_limits<double>::infinity()) {
1211 // limit value range
1212 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1213 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1214 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1215 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1216 } else {
1217 velocity.value.value = static_cast<int16_t>(value);
1218 }
1219
1220 // limit confidence range
1221 if(confidence == std::numeric_limits<double>::infinity()) {
1222 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1223 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1224 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1225 } else {
1226 velocity.confidence.value = static_cast<uint8_t>(confidence);
1227 }
1228}
1229
1237inline void setCartesianAngle(CartesianAngle& angle, const double value,
1238 double confidence = std::numeric_limits<double>::infinity()) {
1239 // wrap angle to 0..2pi
1240 double wrapped_value = fmod(value, 2*M_PI);
1241 if (wrapped_value < 0.0) {
1242 wrapped_value += 2 * M_PI;
1243 }
1244 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1245
1246 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1247 // limit confidence range
1248 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1249 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1250 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1251 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1252 } else {
1253 angle.confidence.value = static_cast<uint8_t>(confidence);
1254 }
1255}
1256
1269inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1270 const double x_std = std::numeric_limits<double>::infinity(),
1271 const double y_std = std::numeric_limits<double>::infinity(),
1272 const double z_std = std::numeric_limits<double>::infinity()) {
1273 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1274 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1275 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1276 if (cartesian_velocity.z != 0.0) {
1277 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1278 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1279 } else {
1280 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1281 }
1282 object.velocity_is_present = true;
1283}
1284
1299inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1300 const double magnitude,
1301 const double angle,
1302 const double z = 0.0,
1303 const double magnitude_std = std::numeric_limits<double>::infinity(),
1304 const double angle_std = std::numeric_limits<double>::infinity(),
1305 const double z_std = std::numeric_limits<double>::infinity()) {
1306 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1307 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1308 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1309 if (z != 0.0) {
1310 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1311 object.velocity.polar_velocity.z_velocity_is_present = true;
1312 } else {
1313 object.velocity.polar_velocity.z_velocity_is_present = false;
1314 }
1315}
1316
1328inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1329 const double confidence = std::numeric_limits<double>::infinity()) {
1330 // limit value range
1331 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1332 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1333 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1334 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1335 } else {
1336 acceleration.value.value = static_cast<int16_t>(value);
1337 }
1338
1339 // limit confidence range
1340 if(confidence == std::numeric_limits<double>::infinity()) {
1341 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1342 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1343 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1344 } else {
1345 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1346 }
1347}
1348
1361inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1362 const double x_std = std::numeric_limits<double>::infinity(),
1363 const double y_std = std::numeric_limits<double>::infinity(),
1364 const double z_std = std::numeric_limits<double>::infinity()) {
1365 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1366 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1367 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1368 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1369 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1370 if (cartesian_acceleration.z != 0.0) {
1371 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1372 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1373 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1374 }
1375 object.acceleration_is_present = true;
1376}
1377
1389inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1390 const double magnitude,
1391 const double angle,
1392 const double z = 0.0,
1393 const double magnitude_std = std::numeric_limits<double>::infinity(),
1394 const double angle_std = std::numeric_limits<double>::infinity(),
1395 const double z_std = std::numeric_limits<double>::infinity()) {
1396 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1397 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1398 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1399 if (z != 0.0) {
1400 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1401 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1402 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1403 } else {
1404 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1405 }
1406 object.acceleration_is_present = true;
1407}
1408
1419inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1420 double yaw_std = std::numeric_limits<double>::infinity()) {
1421 // wrap angle to range [0, 360)
1422 double yaw_in_degrees = yaw * 180 / M_PI;
1423 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1424 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1425 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1426
1427 if(yaw_std == std::numeric_limits<double>::infinity()) {
1428 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1429 } else {
1430 yaw_std *= 180.0 / M_PI;
1431 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1432
1433 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1434 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1435 } else {
1436 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1437 }
1438 }
1439 object.angles_is_present = true;
1440}
1441
1453inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1454 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1455 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1456 // limit value range
1457 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1458 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1459 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1460 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1461 }
1462
1463 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1464
1465 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1466 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1467 } else {
1468 yaw_rate_std *= 180.0 / M_PI;
1469 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1470 // How stupid is this?!
1471 static const std::map<double, uint8_t> confidence_map = {
1472 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1473 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1474 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1475 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1476 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1477 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1478 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1479 };
1480 for(const auto& [thresh, conf_val] : confidence_map) {
1481 if (yaw_rate_std <= thresh) {
1482 object.z_angular_velocity.confidence.value = conf_val;
1483 break;
1484 }
1485 }
1486 }
1487
1488 object.z_angular_velocity_is_present = true;
1489}
1490
1505inline void setObjectDimension(ObjectDimension& dimension, const double value,
1506 const double confidence = std::numeric_limits<double>::infinity()) {
1507 // limit value range
1508 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1509 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1510 } else {
1511 dimension.value.value = static_cast<uint16_t>(value);
1512 }
1513
1514 // limit confidence range
1515 if (confidence == std::numeric_limits<double>::infinity()) {
1516 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1517 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1518 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1519 } else {
1520 dimension.confidence.value = static_cast<uint8_t>(confidence);
1521 }
1522
1523}
1524
1535inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1536 const double std = std::numeric_limits<double>::infinity()) {
1537 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1538 object.object_dimension_x_is_present = true;
1539}
1540
1551inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1552 const double std = std::numeric_limits<double>::infinity()) {
1553 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1554 object.object_dimension_y_is_present = true;
1555}
1556
1567inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1568 const double std = std::numeric_limits<double>::infinity()) {
1569 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1570 object.object_dimension_z_is_present = true;
1571}
1572
1584inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1585 const double x_std = std::numeric_limits<double>::infinity(),
1586 const double y_std = std::numeric_limits<double>::infinity(),
1587 const double z_std = std::numeric_limits<double>::infinity()) {
1588 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1589 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1590 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1591}
1592
1602inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1603 setPositionOfPerceivedObject(object, point);
1604 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1605}
1606
1619inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1620 const gm::PointStamped& point, const int16_t delta_time = 0) {
1621 setUTMPositionOfPerceivedObject(cpm, object, point);
1622 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1623}
1624
1634inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1635 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1636 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1637}
1638
1652inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1653 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1654 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1655 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1656 container.container_data_perceived_object_container.perceived_objects.array.size();
1657 } else {
1658 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1659 }
1660}
1661
1674inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1675 // check for maximum number of containers
1676 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1677 cpm.payload.cpm_containers.value.array.push_back(container);
1678 } else {
1679 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1680 }
1681}
1682
1691inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1692 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1693 covariance_matrix);
1694}
1695
1704inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1705 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1706 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1707}
1708
1719inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1720 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1721 sensor_information.sensor_id.value = sensor_id;
1722}
1723
1734inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1735 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1736 sensor_information.sensor_type.value = sensor_type;
1737}
1738
1757inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1758 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1759 // check for maximum number of SensorInformation entries
1760 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1761 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1762 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1763 container.container_data_sensor_information_container.array.push_back(sensor_information);
1764 } else {
1765 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1766 }
1767 } else {
1768 throw std::invalid_argument("Container is not a SensorInformationContainer");
1769 }
1770}
1771
1786inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1787 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1788 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1789 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1790 );
1791 addContainerToCPM(cpm, container);
1792 } else {
1793 throw std::invalid_argument("Container is not a SensorInformationContainer");
1794 }
1795}
1796
1797} // namespace etsi_its_cpm_ts_msgs::access

◆ 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 467 of file cpm_ts_setters.h.

469 {
470 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
471 } else {
472 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
473 }
474 }
475 object.angles_is_present = true;
476}
477
489inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
490 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
491 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
492 // limit value range
493 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
494 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
495 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
496 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
497 }

◆ 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 508 of file cpm_ts_setters.h.

508 {1.0, AngularSpeedConfidence::DEG_SEC_01},
509 {2.0, AngularSpeedConfidence::DEG_SEC_02},
510 {5.0, AngularSpeedConfidence::DEG_SEC_05},
511 {10.0, AngularSpeedConfidence::DEG_SEC_10},
512 {20.0, AngularSpeedConfidence::DEG_SEC_20},

◆ 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 810 of file cpm_ts_setters.h.

◆ 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 842 of file cpm_ts_setters.h.

878 {
879
880#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
881
891inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
892 const uint8_t protocol_version = 0) {
893 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
894}
895
907inline void setReferenceTime(
908 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
909 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
910 TimestampIts t_its;
911 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
912 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
913 cpm.payload.management_container.reference_time = t_its;
914}
915
927inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
928 const double altitude = AltitudeValue::UNAVAILABLE) {
929 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
930}
931
944inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
945 const bool& northp) {
946 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
947}
948
957inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
958 object.object_id.value = id;
959 object.object_id_is_present = true;
960}
961
973inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
974 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
975 throw std::invalid_argument("MeasurementDeltaTime out of range");
976 } else {
977 object.measurement_delta_time.value = delta_time;
978 }
979}
980
994inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
995 const double confidence = std::numeric_limits<double>::infinity()) {
996 // limit value range
997 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
998 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
999 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1000 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1001 } else {
1002 coordinate.value.value = static_cast<int32_t>(value);
1003 }
1004
1005 // limit confidence range
1006 if (confidence == std::numeric_limits<double>::infinity()) {
1007 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1008 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1009 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1010 } else {
1011 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1012 }
1013}
1014
1026inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
1027 const double x_std = std::numeric_limits<double>::infinity(),
1028 const double y_std = std::numeric_limits<double>::infinity(),
1029 const double z_std = std::numeric_limits<double>::infinity()) {
1030 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1031 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1032 if (point.z != 0.0) {
1033 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1034 object.position.z_coordinate_is_present = true;
1035 }
1036}
1037
1053inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1054 const gm::PointStamped& utm_position,
1055 const double x_std = std::numeric_limits<double>::infinity(),
1056 const double y_std = std::numeric_limits<double>::infinity(),
1057 const double z_std = std::numeric_limits<double>::infinity()) {
1058 gm::PointStamped reference_position = getUTMPosition(cpm);
1059 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1060 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1061 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1062 ")");
1063 }
1064 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1065 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1066 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1067 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1068 if (utm_position.point.z != 0.0) {
1069 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1070 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1071 object.position.z_coordinate_is_present = true;
1072 }
1073}
1074
1086inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1087 const double confidence = std::numeric_limits<double>::infinity()) {
1088 // limit value range
1089 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1090 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1091 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1092 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1093 } else {
1094 velocity.value.value = static_cast<int16_t>(value);
1095 }
1096
1097 // limit confidence range
1098 if(confidence == std::numeric_limits<double>::infinity()) {
1099 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1100 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1101 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1102 } else {
1103 velocity.confidence.value = static_cast<uint8_t>(confidence);
1104 }
1105}
1106
1114inline void setCartesianAngle(CartesianAngle& angle, const double value,
1115 double confidence = std::numeric_limits<double>::infinity()) {
1116 // wrap angle to 0..2pi
1117 double wrapped_value = fmod(value, 2*M_PI);
1118 if (wrapped_value < 0.0) {
1119 wrapped_value += 2 * M_PI;
1120 }
1121 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1122
1123 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1124 // limit confidence range
1125 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1126 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1127 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1128 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1129 } else {
1130 angle.confidence.value = static_cast<uint8_t>(confidence);
1131 }
1132}
1133
1146inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1147 const double x_std = std::numeric_limits<double>::infinity(),
1148 const double y_std = std::numeric_limits<double>::infinity(),
1149 const double z_std = std::numeric_limits<double>::infinity()) {
1150 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1151 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1152 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1153 if (cartesian_velocity.z != 0.0) {
1154 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1155 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1156 } else {
1157 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1158 }
1159 object.velocity_is_present = true;
1160}
1161
1176inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1177 const double magnitude,
1178 const double angle,
1179 const double z = 0.0,
1180 const double magnitude_std = std::numeric_limits<double>::infinity(),
1181 const double angle_std = std::numeric_limits<double>::infinity(),
1182 const double z_std = std::numeric_limits<double>::infinity()) {
1183 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1184 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1185 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1186 if (z != 0.0) {
1187 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1188 object.velocity.polar_velocity.z_velocity_is_present = true;
1189 } else {
1190 object.velocity.polar_velocity.z_velocity_is_present = false;
1191 }
1192}
1193
1205inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1206 const double confidence = std::numeric_limits<double>::infinity()) {
1207 // limit value range
1208 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1209 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1210 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1211 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1212 } else {
1213 acceleration.value.value = static_cast<int16_t>(value);
1214 }
1215
1216 // limit confidence range
1217 if(confidence == std::numeric_limits<double>::infinity()) {
1218 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1219 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1220 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1221 } else {
1222 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1223 }
1224}
1225
1238inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1239 const double x_std = std::numeric_limits<double>::infinity(),
1240 const double y_std = std::numeric_limits<double>::infinity(),
1241 const double z_std = std::numeric_limits<double>::infinity()) {
1242 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1243 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1244 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1245 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1246 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1247 if (cartesian_acceleration.z != 0.0) {
1248 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1249 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1250 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1251 }
1252 object.acceleration_is_present = true;
1253}
1254
1266inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1267 const double magnitude,
1268 const double angle,
1269 const double z = 0.0,
1270 const double magnitude_std = std::numeric_limits<double>::infinity(),
1271 const double angle_std = std::numeric_limits<double>::infinity(),
1272 const double z_std = std::numeric_limits<double>::infinity()) {
1273 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1274 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1275 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1276 if (z != 0.0) {
1277 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1278 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1279 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1280 } else {
1281 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1282 }
1283 object.acceleration_is_present = true;
1284}
1285
1296inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1297 double yaw_std = std::numeric_limits<double>::infinity()) {
1298 // wrap angle to range [0, 360)
1299 double yaw_in_degrees = yaw * 180 / M_PI;
1300 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1301 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1302 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1303
1304 if(yaw_std == std::numeric_limits<double>::infinity()) {
1305 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1306 } else {
1307 yaw_std *= 180.0 / M_PI;
1308 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1309
1310 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1311 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1312 } else {
1313 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1314 }
1315 }
1316 object.angles_is_present = true;
1317}
1318
1330inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1331 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1332 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1333 // limit value range
1334 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1335 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1336 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1337 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1338 }
1339
1340 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1341
1342 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1343 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1344 } else {
1345 yaw_rate_std *= 180.0 / M_PI;
1346 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1347 // How stupid is this?!
1348 static const std::map<double, uint8_t> confidence_map = {
1349 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1350 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1351 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1352 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1353 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1354 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1355 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1356 };
1357 for(const auto& [thresh, conf_val] : confidence_map) {
1358 if (yaw_rate_std <= thresh) {
1359 object.z_angular_velocity.confidence.value = conf_val;
1360 break;
1361 }
1362 }
1363 }
1364
1365 object.z_angular_velocity_is_present = true;
1366}
1367
1382inline void setObjectDimension(ObjectDimension& dimension, const double value,
1383 const double confidence = std::numeric_limits<double>::infinity()) {
1384 // limit value range
1385 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1386 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1387 } else {
1388 dimension.value.value = static_cast<uint16_t>(value);
1389 }
1390
1391 // limit confidence range
1392 if (confidence == std::numeric_limits<double>::infinity()) {
1393 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1394 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1395 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1396 } else {
1397 dimension.confidence.value = static_cast<uint8_t>(confidence);
1398 }
1399
1400}
1401
1412inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1413 const double std = std::numeric_limits<double>::infinity()) {
1414 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1415 object.object_dimension_x_is_present = true;
1416}
1417
1428inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1429 const double std = std::numeric_limits<double>::infinity()) {
1430 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1431 object.object_dimension_y_is_present = true;
1432}
1433
1444inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1445 const double std = std::numeric_limits<double>::infinity()) {
1446 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1447 object.object_dimension_z_is_present = true;
1448}
1449
1461inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1462 const double x_std = std::numeric_limits<double>::infinity(),
1463 const double y_std = std::numeric_limits<double>::infinity(),
1464 const double z_std = std::numeric_limits<double>::infinity()) {
1465 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1466 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1467 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1468}
1469
1479inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1480 setPositionOfPerceivedObject(object, point);
1481 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1482}
1483
1496inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1497 const gm::PointStamped& point, const int16_t delta_time = 0) {
1498 setUTMPositionOfPerceivedObject(cpm, object, point);
1499 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1500}
1501
1511inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1512 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1513 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1514}
1515
1529inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1530 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1531 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1532 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1533 container.container_data_perceived_object_container.perceived_objects.array.size();
1534 } else {
1535 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1536 }
1537}
1538
1551inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1552 // check for maximum number of containers
1553 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1554 cpm.payload.cpm_containers.value.array.push_back(container);
1555 } else {
1556 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1557 }
1558}
1559
1568inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1569 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1570 covariance_matrix);
1571}
1572
1581inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1582 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1583 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1584}
1585
1596inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1597 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1598 sensor_information.sensor_id.value = sensor_id;
1599}
1600
1611inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1612 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1613 sensor_information.sensor_type.value = sensor_type;
1614}
1615
1634inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1635 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1636 // check for maximum number of SensorInformation entries
1637 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1638 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1639 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1640 container.container_data_sensor_information_container.array.push_back(sensor_information);
1641 } else {
1642 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1643 }
1644 } else {
1645 throw std::invalid_argument("Container is not a SensorInformationContainer");
1646 }
1647}
1648
1663inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1664 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1665 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1666 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1667 );
1668 addContainerToCPM(cpm, container);
1669 } else {
1670 throw std::invalid_argument("Container is not a SensorInformationContainer");
1671 }
1672}
1673
1674} // 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 827 of file cpm_ts_setters.h.

828 {
829 throw std::invalid_argument("Container is not a SensorInformationContainer");
830 }
831}

◆ initSensorInformationContainer()

void etsi_its_cpm_ts_msgs::access::initSensorInformationContainer ( WrappedCpmContainer & container)
inline

Initializes a WrappedCpmContainer as a SensorInformationContainer.

This function sets the container ID to CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER and initializes an empty vector with SensorInformation.

Parameters
containerA reference to the WrappedCpmContainer to be initialized as a SensorInformationContainer.

Definition at line 912 of file cpm_ts_setters.h.

948 {
949
950#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
951
961inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
962 const uint8_t protocol_version = 0) {
963 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
964}
965
977inline void setReferenceTime(
978 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
979 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
980 TimestampIts t_its;
981 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
982 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
983 cpm.payload.management_container.reference_time = t_its;
984}
985
997inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
998 const double altitude = AltitudeValue::UNAVAILABLE) {
999 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
1000}
1001
1014inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
1015 const bool& northp) {
1016 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
1017}
1018
1027inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
1028 object.object_id.value = id;
1029 object.object_id_is_present = true;
1030}
1031
1043inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
1044 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
1045 throw std::invalid_argument("MeasurementDeltaTime out of range");
1046 } else {
1047 object.measurement_delta_time.value = delta_time;
1048 }
1049}
1050
1064inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
1065 const double confidence = std::numeric_limits<double>::infinity()) {
1066 // limit value range
1067 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
1068 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
1069 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1070 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1071 } else {
1072 coordinate.value.value = static_cast<int32_t>(value);
1073 }
1074
1075 // limit confidence range
1076 if (confidence == std::numeric_limits<double>::infinity()) {
1077 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1078 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1079 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1080 } else {
1081 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1082 }
1083}
1084
1096inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
1097 const double x_std = std::numeric_limits<double>::infinity(),
1098 const double y_std = std::numeric_limits<double>::infinity(),
1099 const double z_std = std::numeric_limits<double>::infinity()) {
1100 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1101 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1102 if (point.z != 0.0) {
1103 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1104 object.position.z_coordinate_is_present = true;
1105 }
1106}
1107
1123inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1124 const gm::PointStamped& utm_position,
1125 const double x_std = std::numeric_limits<double>::infinity(),
1126 const double y_std = std::numeric_limits<double>::infinity(),
1127 const double z_std = std::numeric_limits<double>::infinity()) {
1128 gm::PointStamped reference_position = getUTMPosition(cpm);
1129 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1130 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1131 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1132 ")");
1133 }
1134 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1135 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1136 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1137 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1138 if (utm_position.point.z != 0.0) {
1139 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1140 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1141 object.position.z_coordinate_is_present = true;
1142 }
1143}
1144
1156inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1157 const double confidence = std::numeric_limits<double>::infinity()) {
1158 // limit value range
1159 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1160 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1161 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1162 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1163 } else {
1164 velocity.value.value = static_cast<int16_t>(value);
1165 }
1166
1167 // limit confidence range
1168 if(confidence == std::numeric_limits<double>::infinity()) {
1169 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1170 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1171 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1172 } else {
1173 velocity.confidence.value = static_cast<uint8_t>(confidence);
1174 }
1175}
1176
1184inline void setCartesianAngle(CartesianAngle& angle, const double value,
1185 double confidence = std::numeric_limits<double>::infinity()) {
1186 // wrap angle to 0..2pi
1187 double wrapped_value = fmod(value, 2*M_PI);
1188 if (wrapped_value < 0.0) {
1189 wrapped_value += 2 * M_PI;
1190 }
1191 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1192
1193 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1194 // limit confidence range
1195 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1196 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1197 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1198 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1199 } else {
1200 angle.confidence.value = static_cast<uint8_t>(confidence);
1201 }
1202}
1203
1216inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1217 const double x_std = std::numeric_limits<double>::infinity(),
1218 const double y_std = std::numeric_limits<double>::infinity(),
1219 const double z_std = std::numeric_limits<double>::infinity()) {
1220 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1221 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1222 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1223 if (cartesian_velocity.z != 0.0) {
1224 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1225 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1226 } else {
1227 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1228 }
1229 object.velocity_is_present = true;
1230}
1231
1246inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1247 const double magnitude,
1248 const double angle,
1249 const double z = 0.0,
1250 const double magnitude_std = std::numeric_limits<double>::infinity(),
1251 const double angle_std = std::numeric_limits<double>::infinity(),
1252 const double z_std = std::numeric_limits<double>::infinity()) {
1253 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1254 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1255 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1256 if (z != 0.0) {
1257 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1258 object.velocity.polar_velocity.z_velocity_is_present = true;
1259 } else {
1260 object.velocity.polar_velocity.z_velocity_is_present = false;
1261 }
1262}
1263
1275inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1276 const double confidence = std::numeric_limits<double>::infinity()) {
1277 // limit value range
1278 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1279 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1280 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1281 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1282 } else {
1283 acceleration.value.value = static_cast<int16_t>(value);
1284 }
1285
1286 // limit confidence range
1287 if(confidence == std::numeric_limits<double>::infinity()) {
1288 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1289 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1290 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1291 } else {
1292 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1293 }
1294}
1295
1308inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1309 const double x_std = std::numeric_limits<double>::infinity(),
1310 const double y_std = std::numeric_limits<double>::infinity(),
1311 const double z_std = std::numeric_limits<double>::infinity()) {
1312 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1313 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1314 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1315 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1316 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1317 if (cartesian_acceleration.z != 0.0) {
1318 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1319 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1320 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1321 }
1322 object.acceleration_is_present = true;
1323}
1324
1336inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1337 const double magnitude,
1338 const double angle,
1339 const double z = 0.0,
1340 const double magnitude_std = std::numeric_limits<double>::infinity(),
1341 const double angle_std = std::numeric_limits<double>::infinity(),
1342 const double z_std = std::numeric_limits<double>::infinity()) {
1343 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1344 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1345 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1346 if (z != 0.0) {
1347 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1348 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1349 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1350 } else {
1351 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1352 }
1353 object.acceleration_is_present = true;
1354}
1355
1366inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1367 double yaw_std = std::numeric_limits<double>::infinity()) {
1368 // wrap angle to range [0, 360)
1369 double yaw_in_degrees = yaw * 180 / M_PI;
1370 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1371 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1372 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1373
1374 if(yaw_std == std::numeric_limits<double>::infinity()) {
1375 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1376 } else {
1377 yaw_std *= 180.0 / M_PI;
1378 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1379
1380 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1381 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1382 } else {
1383 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1384 }
1385 }
1386 object.angles_is_present = true;
1387}
1388
1400inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1401 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1402 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1403 // limit value range
1404 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1405 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1406 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1407 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1408 }
1409
1410 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1411
1412 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1413 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1414 } else {
1415 yaw_rate_std *= 180.0 / M_PI;
1416 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1417 // How stupid is this?!
1418 static const std::map<double, uint8_t> confidence_map = {
1419 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1420 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1421 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1422 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1423 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1424 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1425 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1426 };
1427 for(const auto& [thresh, conf_val] : confidence_map) {
1428 if (yaw_rate_std <= thresh) {
1429 object.z_angular_velocity.confidence.value = conf_val;
1430 break;
1431 }
1432 }
1433 }
1434
1435 object.z_angular_velocity_is_present = true;
1436}
1437
1452inline void setObjectDimension(ObjectDimension& dimension, const double value,
1453 const double confidence = std::numeric_limits<double>::infinity()) {
1454 // limit value range
1455 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1456 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1457 } else {
1458 dimension.value.value = static_cast<uint16_t>(value);
1459 }
1460
1461 // limit confidence range
1462 if (confidence == std::numeric_limits<double>::infinity()) {
1463 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1464 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1465 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1466 } else {
1467 dimension.confidence.value = static_cast<uint8_t>(confidence);
1468 }
1469
1470}
1471
1482inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1483 const double std = std::numeric_limits<double>::infinity()) {
1484 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1485 object.object_dimension_x_is_present = true;
1486}
1487
1498inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1499 const double std = std::numeric_limits<double>::infinity()) {
1500 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1501 object.object_dimension_y_is_present = true;
1502}
1503
1514inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1515 const double std = std::numeric_limits<double>::infinity()) {
1516 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1517 object.object_dimension_z_is_present = true;
1518}
1519
1531inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1532 const double x_std = std::numeric_limits<double>::infinity(),
1533 const double y_std = std::numeric_limits<double>::infinity(),
1534 const double z_std = std::numeric_limits<double>::infinity()) {
1535 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1536 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1537 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1538}
1539
1549inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1550 setPositionOfPerceivedObject(object, point);
1551 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1552}
1553
1566inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1567 const gm::PointStamped& point, const int16_t delta_time = 0) {
1568 setUTMPositionOfPerceivedObject(cpm, object, point);
1569 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1570}
1571
1581inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1582 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1583 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1584}
1585
1599inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1600 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1601 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1602 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1603 container.container_data_perceived_object_container.perceived_objects.array.size();
1604 } else {
1605 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1606 }
1607}
1608
1621inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1622 // check for maximum number of containers
1623 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1624 cpm.payload.cpm_containers.value.array.push_back(container);
1625 } else {
1626 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1627 }
1628}
1629
1638inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1639 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1640 covariance_matrix);
1641}
1642
1651inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1652 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1653 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1654}
1655
1666inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1667 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1668 sensor_information.sensor_id.value = sensor_id;
1669}
1670
1681inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1682 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1683 sensor_information.sensor_type.value = sensor_type;
1684}
1685
1704inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1705 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1706 // check for maximum number of SensorInformation entries
1707 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1708 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1709 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1710 container.container_data_sensor_information_container.array.push_back(sensor_information);
1711 } else {
1712 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1713 }
1714 } else {
1715 throw std::invalid_argument("Container is not a SensorInformationContainer");
1716 }
1717}
1718
1733inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1734 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1735 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1736 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1737 );
1738 addContainerToCPM(cpm, container);
1739 } else {
1740 throw std::invalid_argument("Container is not a SensorInformationContainer");
1741 }
1742}
1743
1744} // 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 536 of file cpm_ts_setters.h.

542 {
543 // limit value range
544 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
545 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
546 } else {
547 dimension.value.value = static_cast<uint16_t>(value);
548 }
549
550 // limit confidence range
551 if (confidence == std::numeric_limits<double>::infinity()) {
552 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
553 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
554 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
555 } else {

◆ 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 266 of file cpm_ts_setters.h.

274 {

◆ setAccelerationMagnitude()

template<typename AccelerationMagnitude>
void etsi_its_cpm_ts_msgs::access::setAccelerationMagnitude ( AccelerationMagnitude & accel_mag,
const double value,
const double confidence = std::numeric_limits<double>::infinity() )
inline

Set the AccelerationMagnitude object.

AccelerationConfidence is set to UNAVAILABLE

Parameters
accel_magobject to set
valueAccelerationMagnitudeValue in m/s^2 as decimal number
confidencestandard deviation in m/s^2 as decimal number (default: infinity, mapping to AccelerationConfidence::UNAVAILABLE)

Definition at line 253 of file cpm_ts_setters.h.

257 {

◆ setAccelerationMagnitudeConfidence()

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

Set the AccelerationMagnitude Confidence object.

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

Definition at line 233 of file cpm_ts_setters.h.

◆ setAccelerationMagnitudeValue()

template<typename AccelerationMagnitudeValue>
void etsi_its_cpm_ts_msgs::access::setAccelerationMagnitudeValue ( AccelerationMagnitudeValue & accel_mag_value,
const double value )
inline

Set the Acceleration Magnitude Value object.

Parameters
accel_mag_valueobject to set
valueAccelerationMagnitudeValue in m/s^2 as decimal number

Definition at line 220 of file cpm_ts_setters.h.

◆ 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 569 of file cpm_ts_setters.h.

572 {
573 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
574 object.object_dimension_x_is_present = true;
575}
576

◆ 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.

◆ setCartesianAngle()

void etsi_its_cpm_ts_msgs::access::setCartesianAngle ( CartesianAngle & angle,
const double value,
double confidence = std::numeric_limits<double>::infinity() )
inline

Set a Cartesian Angle.

Parameters
angleThe CartesianAngle object to be set.
valueAngle in radians to be set in the CartesianAngle object.
confidencestandard deviation of the angle in radians (default: infinity, which will map to AngleConfidence::UNAVAILABLE).

Definition at line 445 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 792 of file cpm_ts_setters.h.

◆ 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.

◆ 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 314 of file cpm_ts_setters.h.

◆ 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 373 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 352 of file cpm_ts_setters.h.

◆ 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 339 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.

◆ 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 713 of file cpm_ts_setters.h.

727inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
728 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
729 covariance_matrix);
730}
731
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:482

◆ setPolarAccelerationOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setPolarAccelerationOfPerceivedObject ( PerceivedObject & object,
const double magnitude,
const double angle,
const double z = 0.0,
const double magnitude_std = std::numeric_limits<double>::infinity(),
const double angle_std = std::numeric_limits<double>::infinity(),
const double z_std = std::numeric_limits<double>::infinity() )
inline

Set the Polar Acceleration Of Perceived Object object.

Parameters
objectPerceivedObject to set the Polar Acceleration for
magnitudeThe magnitude of the acceleration in m/s^2 in the xy plane.
angleThe angle of the acceleration in radians in the xy plane.
zThe z component of the acceleration in m/s^2 (default: 0.0).
magnitude_stdThe standard deviation in m/s^2 for the acceleration magnitude (default: infinity).
angle_stdThe standard deviation in radians for the angle (default: infinity).
z_stdThe standard deviation in m/s^2 for the z acceleration component (default: infinity).

Definition at line 597 of file cpm_ts_setters.h.

604 {
605 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
606 object.object_dimension_z_is_present = true;
607}
608

◆ setPolarVelocityOfPerceivedObject()

void etsi_its_cpm_ts_msgs::access::setPolarVelocityOfPerceivedObject ( PerceivedObject & object,
const double magnitude,
const double angle,
const double z = 0.0,
const double magnitude_std = std::numeric_limits<double>::infinity(),
const double angle_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 Polar velocity components. Optionally, confidence values can be specified for each velocity component.

Parameters
objectThe perceived object for which the velocity is being set.
magnitudeThe magnitude of the velocity in m/s in the xy plane.
angleThe angle of the velocity in radians in the xy plane.
zThe z component of the velocity in m/s (default: 0.0).
magnitude_stdThe standard deviation in m/s for the velocity magnitude (default: infinity).
angle_stdThe standard deviation in radians for the angle (default: infinity).
z_stdThe standard deviation in m/s for the z velocity component (default: infinity).

Definition at line 507 of file cpm_ts_setters.h.

◆ 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 451 of file cpm_ts_setters.h.

◆ 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 524 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.

◆ 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.

◆ 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.

◆ 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 288 of file cpm_ts_setters.h.

◆ 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 432 of file cpm_ts_setters.h.

◆ setSensorID()

void etsi_its_cpm_ts_msgs::access::setSensorID ( SensorInformation & sensor_information,
const uint8_t sensor_id = 0 )
inline

Sets the sensorId of a SensorInformation object.

This function sets the sensorId of a SensorInformation object. The sensorId is limited to the range defined by Identifier1B::MIN and Identifier1B::MAX.

Parameters
sensor_informationThe SensorInformation object to set the sensorId for.
sensor_idThe sensorId to set (default: 0).
Exceptions
std::invalid_argumentif the sensor_id is out of range.

Definition at line 927 of file cpm_ts_setters.h.

976inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
977 const uint8_t protocol_version = 0) {
978 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
979}
980
992inline void setReferenceTime(
993 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
994 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
995 TimestampIts t_its;
996 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
997 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
998 cpm.payload.management_container.reference_time = t_its;
999}
1000
1012inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
1013 const double altitude = AltitudeValue::UNAVAILABLE) {
1014 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
1015}
1016
1029inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
1030 const bool& northp) {
1031 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
1032}
1033
1042inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
1043 object.object_id.value = id;
1044 object.object_id_is_present = true;
1045}
1046
1058inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
1059 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
1060 throw std::invalid_argument("MeasurementDeltaTime out of range");
1061 } else {
1062 object.measurement_delta_time.value = delta_time;
1063 }
1064}
1065
1079inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
1080 const double confidence = std::numeric_limits<double>::infinity()) {
1081 // limit value range
1082 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
1083 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
1084 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1085 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1086 } else {
1087 coordinate.value.value = static_cast<int32_t>(value);
1088 }
1089
1090 // limit confidence range
1091 if (confidence == std::numeric_limits<double>::infinity()) {
1092 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1093 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1094 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1095 } else {
1096 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1097 }
1098}
1099
1111inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
1112 const double x_std = std::numeric_limits<double>::infinity(),
1113 const double y_std = std::numeric_limits<double>::infinity(),
1114 const double z_std = std::numeric_limits<double>::infinity()) {
1115 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1116 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1117 if (point.z != 0.0) {
1118 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1119 object.position.z_coordinate_is_present = true;
1120 }
1121}
1122
1138inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1139 const gm::PointStamped& utm_position,
1140 const double x_std = std::numeric_limits<double>::infinity(),
1141 const double y_std = std::numeric_limits<double>::infinity(),
1142 const double z_std = std::numeric_limits<double>::infinity()) {
1143 gm::PointStamped reference_position = getUTMPosition(cpm);
1144 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1145 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1146 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1147 ")");
1148 }
1149 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1150 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1151 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1152 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1153 if (utm_position.point.z != 0.0) {
1154 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1155 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1156 object.position.z_coordinate_is_present = true;
1157 }
1158}
1159
1171inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1172 const double confidence = std::numeric_limits<double>::infinity()) {
1173 // limit value range
1174 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1175 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1176 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1177 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1178 } else {
1179 velocity.value.value = static_cast<int16_t>(value);
1180 }
1181
1182 // limit confidence range
1183 if(confidence == std::numeric_limits<double>::infinity()) {
1184 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1185 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1186 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1187 } else {
1188 velocity.confidence.value = static_cast<uint8_t>(confidence);
1189 }
1190}
1191
1199inline void setCartesianAngle(CartesianAngle& angle, const double value,
1200 double confidence = std::numeric_limits<double>::infinity()) {
1201 // wrap angle to 0..2pi
1202 double wrapped_value = fmod(value, 2*M_PI);
1203 if (wrapped_value < 0.0) {
1204 wrapped_value += 2 * M_PI;
1205 }
1206 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1207
1208 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1209 // limit confidence range
1210 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1211 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1212 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1213 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1214 } else {
1215 angle.confidence.value = static_cast<uint8_t>(confidence);
1216 }
1217}
1218
1231inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1232 const double x_std = std::numeric_limits<double>::infinity(),
1233 const double y_std = std::numeric_limits<double>::infinity(),
1234 const double z_std = std::numeric_limits<double>::infinity()) {
1235 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1236 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1237 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1238 if (cartesian_velocity.z != 0.0) {
1239 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1240 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1241 } else {
1242 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1243 }
1244 object.velocity_is_present = true;
1245}
1246
1261inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1262 const double magnitude,
1263 const double angle,
1264 const double z = 0.0,
1265 const double magnitude_std = std::numeric_limits<double>::infinity(),
1266 const double angle_std = std::numeric_limits<double>::infinity(),
1267 const double z_std = std::numeric_limits<double>::infinity()) {
1268 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1269 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1270 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1271 if (z != 0.0) {
1272 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1273 object.velocity.polar_velocity.z_velocity_is_present = true;
1274 } else {
1275 object.velocity.polar_velocity.z_velocity_is_present = false;
1276 }
1277}
1278
1290inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1291 const double confidence = std::numeric_limits<double>::infinity()) {
1292 // limit value range
1293 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1294 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1295 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1296 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1297 } else {
1298 acceleration.value.value = static_cast<int16_t>(value);
1299 }
1300
1301 // limit confidence range
1302 if(confidence == std::numeric_limits<double>::infinity()) {
1303 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1304 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1305 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1306 } else {
1307 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1308 }
1309}
1310
1323inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1324 const double x_std = std::numeric_limits<double>::infinity(),
1325 const double y_std = std::numeric_limits<double>::infinity(),
1326 const double z_std = std::numeric_limits<double>::infinity()) {
1327 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1328 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1329 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1330 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1331 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1332 if (cartesian_acceleration.z != 0.0) {
1333 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1334 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1335 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1336 }
1337 object.acceleration_is_present = true;
1338}
1339
1351inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1352 const double magnitude,
1353 const double angle,
1354 const double z = 0.0,
1355 const double magnitude_std = std::numeric_limits<double>::infinity(),
1356 const double angle_std = std::numeric_limits<double>::infinity(),
1357 const double z_std = std::numeric_limits<double>::infinity()) {
1358 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1359 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1360 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1361 if (z != 0.0) {
1362 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1363 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1364 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1365 } else {
1366 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1367 }
1368 object.acceleration_is_present = true;
1369}
1370
1381inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1382 double yaw_std = std::numeric_limits<double>::infinity()) {
1383 // wrap angle to range [0, 360)
1384 double yaw_in_degrees = yaw * 180 / M_PI;
1385 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1386 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1387 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1388
1389 if(yaw_std == std::numeric_limits<double>::infinity()) {
1390 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1391 } else {
1392 yaw_std *= 180.0 / M_PI;
1393 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1394
1395 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1396 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1397 } else {
1398 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1399 }
1400 }
1401 object.angles_is_present = true;
1402}
1403
1415inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1416 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1417 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1418 // limit value range
1419 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1420 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1421 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1422 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1423 }
1424
1425 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1426
1427 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1428 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1429 } else {
1430 yaw_rate_std *= 180.0 / M_PI;
1431 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1432 // How stupid is this?!
1433 static const std::map<double, uint8_t> confidence_map = {
1434 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1435 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1436 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1437 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1438 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1439 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1440 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1441 };
1442 for(const auto& [thresh, conf_val] : confidence_map) {
1443 if (yaw_rate_std <= thresh) {
1444 object.z_angular_velocity.confidence.value = conf_val;
1445 break;
1446 }
1447 }
1448 }
1449
1450 object.z_angular_velocity_is_present = true;
1451}
1452
1467inline void setObjectDimension(ObjectDimension& dimension, const double value,
1468 const double confidence = std::numeric_limits<double>::infinity()) {
1469 // limit value range
1470 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1471 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1472 } else {
1473 dimension.value.value = static_cast<uint16_t>(value);
1474 }
1475
1476 // limit confidence range
1477 if (confidence == std::numeric_limits<double>::infinity()) {
1478 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1479 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1480 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1481 } else {
1482 dimension.confidence.value = static_cast<uint8_t>(confidence);
1483 }
1484
1485}
1486
1497inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1498 const double std = std::numeric_limits<double>::infinity()) {
1499 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1500 object.object_dimension_x_is_present = true;
1501}
1502
1513inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1514 const double std = std::numeric_limits<double>::infinity()) {
1515 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1516 object.object_dimension_y_is_present = true;
1517}
1518
1529inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1530 const double std = std::numeric_limits<double>::infinity()) {
1531 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1532 object.object_dimension_z_is_present = true;
1533}
1534
1546inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1547 const double x_std = std::numeric_limits<double>::infinity(),
1548 const double y_std = std::numeric_limits<double>::infinity(),
1549 const double z_std = std::numeric_limits<double>::infinity()) {
1550 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1551 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1552 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1553}
1554
1564inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1565 setPositionOfPerceivedObject(object, point);
1566 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1567}
1568
1581inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1582 const gm::PointStamped& point, const int16_t delta_time = 0) {
1583 setUTMPositionOfPerceivedObject(cpm, object, point);
1584 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1585}
1586
1596inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1597 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1598 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1599}
1600
1614inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1615 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1616 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1617 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1618 container.container_data_perceived_object_container.perceived_objects.array.size();
1619 } else {
1620 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1621 }
1622}
1623
1636inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1637 // check for maximum number of containers
1638 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1639 cpm.payload.cpm_containers.value.array.push_back(container);
1640 } else {
1641 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1642 }
1643}
1644
1653inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1654 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1655 covariance_matrix);
1656}
1657
1666inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1667 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1668 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1669}
1670
1681inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1682 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1683 sensor_information.sensor_id.value = sensor_id;
1684}
1685
1696inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1697 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1698 sensor_information.sensor_type.value = sensor_type;
1699}
1700
1719inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1720 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1721 // check for maximum number of SensorInformation entries
1722 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1723 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1724 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1725 container.container_data_sensor_information_container.array.push_back(sensor_information);
1726 } else {
1727 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1728 }
1729 } else {
1730 throw std::invalid_argument("Container is not a SensorInformationContainer");
1731 }
1732}
1733
1748inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1749 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1750 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1751 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1752 );
1753 addContainerToCPM(cpm, container);
1754 } else {
1755 throw std::invalid_argument("Container is not a SensorInformationContainer");
1756 }
1757}
1758
1759} // namespace etsi_its_cpm_ts_msgs::access

◆ setSensorType()

void etsi_its_cpm_ts_msgs::access::setSensorType ( SensorInformation & sensor_information,
const uint8_t sensor_type = SensorType::UNDEFINED )
inline

Sets the sensorType of a SensorInformation object.

This function sets the sensorType of a SensorInformation object. The sensorType is limited to the range defined by SensorType::MIN and SensorType::MAX.

Parameters
sensor_informationThe SensorInformation object to set the sensorType for.
sensor_typeThe sensorType to set (default: SensorType::UNDEFINED).
Exceptions
std::invalid_argumentif the sensor_type is out of range.

Definition at line 942 of file cpm_ts_setters.h.

978 {
979
980#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>
981
991inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
992 const uint8_t protocol_version = 0) {
993 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
994}
995
1007inline void setReferenceTime(
1008 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
1009 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
1010 TimestampIts t_its;
1011 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
1012 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
1013 cpm.payload.management_container.reference_time = t_its;
1014}
1015
1027inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
1028 const double altitude = AltitudeValue::UNAVAILABLE) {
1029 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
1030}
1031
1044inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
1045 const bool& northp) {
1046 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
1047}
1048
1057inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
1058 object.object_id.value = id;
1059 object.object_id_is_present = true;
1060}
1061
1073inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
1074 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
1075 throw std::invalid_argument("MeasurementDeltaTime out of range");
1076 } else {
1077 object.measurement_delta_time.value = delta_time;
1078 }
1079}
1080
1094inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
1095 const double confidence = std::numeric_limits<double>::infinity()) {
1096 // limit value range
1097 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
1098 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
1099 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1100 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1101 } else {
1102 coordinate.value.value = static_cast<int32_t>(value);
1103 }
1104
1105 // limit confidence range
1106 if (confidence == std::numeric_limits<double>::infinity()) {
1107 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1108 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1109 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1110 } else {
1111 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1112 }
1113}
1114
1126inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
1127 const double x_std = std::numeric_limits<double>::infinity(),
1128 const double y_std = std::numeric_limits<double>::infinity(),
1129 const double z_std = std::numeric_limits<double>::infinity()) {
1130 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1131 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1132 if (point.z != 0.0) {
1133 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1134 object.position.z_coordinate_is_present = true;
1135 }
1136}
1137
1153inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1154 const gm::PointStamped& utm_position,
1155 const double x_std = std::numeric_limits<double>::infinity(),
1156 const double y_std = std::numeric_limits<double>::infinity(),
1157 const double z_std = std::numeric_limits<double>::infinity()) {
1158 gm::PointStamped reference_position = getUTMPosition(cpm);
1159 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1160 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1161 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1162 ")");
1163 }
1164 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1165 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1166 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1167 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1168 if (utm_position.point.z != 0.0) {
1169 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1170 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1171 object.position.z_coordinate_is_present = true;
1172 }
1173}
1174
1186inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1187 const double confidence = std::numeric_limits<double>::infinity()) {
1188 // limit value range
1189 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1190 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1191 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1192 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1193 } else {
1194 velocity.value.value = static_cast<int16_t>(value);
1195 }
1196
1197 // limit confidence range
1198 if(confidence == std::numeric_limits<double>::infinity()) {
1199 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1200 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1201 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1202 } else {
1203 velocity.confidence.value = static_cast<uint8_t>(confidence);
1204 }
1205}
1206
1214inline void setCartesianAngle(CartesianAngle& angle, const double value,
1215 double confidence = std::numeric_limits<double>::infinity()) {
1216 // wrap angle to 0..2pi
1217 double wrapped_value = fmod(value, 2*M_PI);
1218 if (wrapped_value < 0.0) {
1219 wrapped_value += 2 * M_PI;
1220 }
1221 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1222
1223 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1224 // limit confidence range
1225 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1226 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1227 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1228 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1229 } else {
1230 angle.confidence.value = static_cast<uint8_t>(confidence);
1231 }
1232}
1233
1246inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1247 const double x_std = std::numeric_limits<double>::infinity(),
1248 const double y_std = std::numeric_limits<double>::infinity(),
1249 const double z_std = std::numeric_limits<double>::infinity()) {
1250 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1251 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1252 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1253 if (cartesian_velocity.z != 0.0) {
1254 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1255 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1256 } else {
1257 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1258 }
1259 object.velocity_is_present = true;
1260}
1261
1276inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1277 const double magnitude,
1278 const double angle,
1279 const double z = 0.0,
1280 const double magnitude_std = std::numeric_limits<double>::infinity(),
1281 const double angle_std = std::numeric_limits<double>::infinity(),
1282 const double z_std = std::numeric_limits<double>::infinity()) {
1283 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1284 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1285 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1286 if (z != 0.0) {
1287 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1288 object.velocity.polar_velocity.z_velocity_is_present = true;
1289 } else {
1290 object.velocity.polar_velocity.z_velocity_is_present = false;
1291 }
1292}
1293
1305inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1306 const double confidence = std::numeric_limits<double>::infinity()) {
1307 // limit value range
1308 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1309 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1310 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1311 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1312 } else {
1313 acceleration.value.value = static_cast<int16_t>(value);
1314 }
1315
1316 // limit confidence range
1317 if(confidence == std::numeric_limits<double>::infinity()) {
1318 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1319 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1320 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1321 } else {
1322 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1323 }
1324}
1325
1338inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
1339 const double x_std = std::numeric_limits<double>::infinity(),
1340 const double y_std = std::numeric_limits<double>::infinity(),
1341 const double z_std = std::numeric_limits<double>::infinity()) {
1342 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1343 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1344 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1345 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1346 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1347 if (cartesian_acceleration.z != 0.0) {
1348 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1349 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1350 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1351 }
1352 object.acceleration_is_present = true;
1353}
1354
1366inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1367 const double magnitude,
1368 const double angle,
1369 const double z = 0.0,
1370 const double magnitude_std = std::numeric_limits<double>::infinity(),
1371 const double angle_std = std::numeric_limits<double>::infinity(),
1372 const double z_std = std::numeric_limits<double>::infinity()) {
1373 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1374 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1375 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1376 if (z != 0.0) {
1377 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1378 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1379 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1380 } else {
1381 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1382 }
1383 object.acceleration_is_present = true;
1384}
1385
1396inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1397 double yaw_std = std::numeric_limits<double>::infinity()) {
1398 // wrap angle to range [0, 360)
1399 double yaw_in_degrees = yaw * 180 / M_PI;
1400 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1401 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1402 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1403
1404 if(yaw_std == std::numeric_limits<double>::infinity()) {
1405 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1406 } else {
1407 yaw_std *= 180.0 / M_PI;
1408 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1409
1410 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1411 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1412 } else {
1413 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1414 }
1415 }
1416 object.angles_is_present = true;
1417}
1418
1430inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1431 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1432 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1433 // limit value range
1434 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1435 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1436 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1437 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1438 }
1439
1440 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1441
1442 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1443 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1444 } else {
1445 yaw_rate_std *= 180.0 / M_PI;
1446 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1447 // How stupid is this?!
1448 static const std::map<double, uint8_t> confidence_map = {
1449 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1450 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1451 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1452 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1453 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1454 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1455 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1456 };
1457 for(const auto& [thresh, conf_val] : confidence_map) {
1458 if (yaw_rate_std <= thresh) {
1459 object.z_angular_velocity.confidence.value = conf_val;
1460 break;
1461 }
1462 }
1463 }
1464
1465 object.z_angular_velocity_is_present = true;
1466}
1467
1482inline void setObjectDimension(ObjectDimension& dimension, const double value,
1483 const double confidence = std::numeric_limits<double>::infinity()) {
1484 // limit value range
1485 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1486 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1487 } else {
1488 dimension.value.value = static_cast<uint16_t>(value);
1489 }
1490
1491 // limit confidence range
1492 if (confidence == std::numeric_limits<double>::infinity()) {
1493 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1494 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1495 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1496 } else {
1497 dimension.confidence.value = static_cast<uint8_t>(confidence);
1498 }
1499
1500}
1501
1512inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1513 const double std = std::numeric_limits<double>::infinity()) {
1514 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1515 object.object_dimension_x_is_present = true;
1516}
1517
1528inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1529 const double std = std::numeric_limits<double>::infinity()) {
1530 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1531 object.object_dimension_y_is_present = true;
1532}
1533
1544inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1545 const double std = std::numeric_limits<double>::infinity()) {
1546 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1547 object.object_dimension_z_is_present = true;
1548}
1549
1561inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1562 const double x_std = std::numeric_limits<double>::infinity(),
1563 const double y_std = std::numeric_limits<double>::infinity(),
1564 const double z_std = std::numeric_limits<double>::infinity()) {
1565 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1566 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1567 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1568}
1569
1579inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1580 setPositionOfPerceivedObject(object, point);
1581 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1582}
1583
1596inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1597 const gm::PointStamped& point, const int16_t delta_time = 0) {
1598 setUTMPositionOfPerceivedObject(cpm, object, point);
1599 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1600}
1601
1611inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1612 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1613 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1614}
1615
1629inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1630 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1631 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1632 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1633 container.container_data_perceived_object_container.perceived_objects.array.size();
1634 } else {
1635 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1636 }
1637}
1638
1651inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1652 // check for maximum number of containers
1653 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1654 cpm.payload.cpm_containers.value.array.push_back(container);
1655 } else {
1656 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1657 }
1658}
1659
1668inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1669 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1670 covariance_matrix);
1671}
1672
1681inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1682 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1683 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1684}
1685
1696inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1697 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1698 sensor_information.sensor_id.value = sensor_id;
1699}
1700
1711inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1712 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1713 sensor_information.sensor_type.value = sensor_type;
1714}
1715
1734inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1735 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1736 // check for maximum number of SensorInformation entries
1737 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1738 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1739 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1740 container.container_data_sensor_information_container.array.push_back(sensor_information);
1741 } else {
1742 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1743 }
1744 } else {
1745 throw std::invalid_argument("Container is not a SensorInformationContainer");
1746 }
1747}
1748
1763inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1764 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1765 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1766 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1767 );
1768 addContainerToCPM(cpm, container);
1769 } else {
1770 throw std::invalid_argument("Container is not a SensorInformationContainer");
1771 }
1772}
1773
1774} // namespace etsi_its_cpm_ts_msgs::access

◆ 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 477 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 539 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 899 of file cpm_ts_setters.h.

948inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
949 const uint8_t protocol_version = 0) {
950 setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
951}
952
964inline void setReferenceTime(
965 CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
966 const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
967 TimestampIts t_its;
968 setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
969 throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
970 cpm.payload.management_container.reference_time = t_its;
971}
972
984inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
985 const double altitude = AltitudeValue::UNAVAILABLE) {
986 setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
987}
988
1001inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
1002 const bool& northp) {
1003 setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
1004}
1005
1014inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
1015 object.object_id.value = id;
1016 object.object_id_is_present = true;
1017}
1018
1030inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
1031 if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
1032 throw std::invalid_argument("MeasurementDeltaTime out of range");
1033 } else {
1034 object.measurement_delta_time.value = delta_time;
1035 }
1036}
1037
1051inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
1052 const double confidence = std::numeric_limits<double>::infinity()) {
1053 // limit value range
1054 if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
1055 coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
1056 } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
1057 coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
1058 } else {
1059 coordinate.value.value = static_cast<int32_t>(value);
1060 }
1061
1062 // limit confidence range
1063 if (confidence == std::numeric_limits<double>::infinity()) {
1064 coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
1065 } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
1066 coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
1067 } else {
1068 coordinate.confidence.value = static_cast<uint16_t>(confidence);
1069 }
1070}
1071
1083inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point,
1084 const double x_std = std::numeric_limits<double>::infinity(),
1085 const double y_std = std::numeric_limits<double>::infinity(),
1086 const double z_std = std::numeric_limits<double>::infinity()) {
1087 setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1088 setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1089 if (point.z != 0.0) {
1090 setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1091 object.position.z_coordinate_is_present = true;
1092 }
1093}
1094
1110inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1111 const gm::PointStamped& utm_position,
1112 const double x_std = std::numeric_limits<double>::infinity(),
1113 const double y_std = std::numeric_limits<double>::infinity(),
1114 const double z_std = std::numeric_limits<double>::infinity()) {
1115 gm::PointStamped reference_position = getUTMPosition(cpm);
1116 if (utm_position.header.frame_id != reference_position.header.frame_id) {
1117 throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
1118 ") does not match the reference position frame_id (" + reference_position.header.frame_id +
1119 ")");
1120 }
1121 setCartesianCoordinateWithConfidence(object.position.x_coordinate,
1122 (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1123 setCartesianCoordinateWithConfidence(object.position.y_coordinate,
1124 (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1125 if (utm_position.point.z != 0.0) {
1126 setCartesianCoordinateWithConfidence(object.position.z_coordinate,
1127 (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1128 object.position.z_coordinate_is_present = true;
1129 }
1130}
1131
1143inline void setVelocityComponent(VelocityComponent& velocity, const double value,
1144 const double confidence = std::numeric_limits<double>::infinity()) {
1145 // limit value range
1146 if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
1147 velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
1148 } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1149 velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1150 } else {
1151 velocity.value.value = static_cast<int16_t>(value);
1152 }
1153
1154 // limit confidence range
1155 if(confidence == std::numeric_limits<double>::infinity()) {
1156 velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
1157 } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
1158 velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
1159 } else {
1160 velocity.confidence.value = static_cast<uint8_t>(confidence);
1161 }
1162}
1163
1171inline void setCartesianAngle(CartesianAngle& angle, const double value,
1172 double confidence = std::numeric_limits<double>::infinity()) {
1173 // wrap angle to 0..2pi
1174 double wrapped_value = fmod(value, 2*M_PI);
1175 if (wrapped_value < 0.0) {
1176 wrapped_value += 2 * M_PI;
1177 }
1178 angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
1179
1180 confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1181 // limit confidence range
1182 if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
1183 angle.confidence.value = AngleConfidence::UNAVAILABLE;
1184 } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
1185 angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1186 } else {
1187 angle.confidence.value = static_cast<uint8_t>(confidence);
1188 }
1189}
1190
1203inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity,
1204 const double x_std = std::numeric_limits<double>::infinity(),
1205 const double y_std = std::numeric_limits<double>::infinity(),
1206 const double z_std = std::numeric_limits<double>::infinity()) {
1207 object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
1208 setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1209 setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1210 if (cartesian_velocity.z != 0.0) {
1211 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1212 object.velocity.cartesian_velocity.z_velocity_is_present = true;
1213 } else {
1214 object.velocity.cartesian_velocity.z_velocity_is_present = false;
1215 }
1216 object.velocity_is_present = true;
1217}
1218
1233inline void setPolarVelocityOfPerceivedObject(PerceivedObject& object,
1234 const double magnitude,
1235 const double angle,
1236 const double z = 0.0,
1237 const double magnitude_std = std::numeric_limits<double>::infinity(),
1238 const double angle_std = std::numeric_limits<double>::infinity(),
1239 const double z_std = std::numeric_limits<double>::infinity()) {
1240 object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
1241 setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
1242 setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
1243 if (z != 0.0) {
1244 setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1245 object.velocity.polar_velocity.z_velocity_is_present = true;
1246 } else {
1247 object.velocity.polar_velocity.z_velocity_is_present = false;
1248 }
1249}
1250
1262inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
1263 const double confidence = std::numeric_limits<double>::infinity()) {
1264 // limit value range
1265 if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
1266 acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
1267 } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
1268 acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
1269 } else {
1270 acceleration.value.value = static_cast<int16_t>(value);
1271 }
1272
1273 // limit confidence range
1274 if(confidence == std::numeric_limits<double>::infinity()) {
1275 acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
1276 } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
1277 acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
1278 } else {
1279 acceleration.confidence.value = static_cast<uint8_t>(confidence);
1280 }
1281}
1282
1295inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration,
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 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
1300 setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
1301 x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1302 setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
1303 y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1304 if (cartesian_acceleration.z != 0.0) {
1305 setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
1306 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1307 object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
1308 }
1309 object.acceleration_is_present = true;
1310}
1311
1323inline void setPolarAccelerationOfPerceivedObject(PerceivedObject& object,
1324 const double magnitude,
1325 const double angle,
1326 const double z = 0.0,
1327 const double magnitude_std = std::numeric_limits<double>::infinity(),
1328 const double angle_std = std::numeric_limits<double>::infinity(),
1329 const double z_std = std::numeric_limits<double>::infinity()) {
1330 object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
1331 setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
1332 setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
1333 if (z != 0.0) {
1334 setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
1335 z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1336 object.acceleration.polar_acceleration.z_acceleration_is_present = true;
1337 } else {
1338 object.acceleration.polar_acceleration.z_acceleration_is_present = false;
1339 }
1340 object.acceleration_is_present = true;
1341}
1342
1353inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
1354 double yaw_std = std::numeric_limits<double>::infinity()) {
1355 // wrap angle to range [0, 360)
1356 double yaw_in_degrees = yaw * 180 / M_PI;
1357 while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
1358 while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
1359 object.angles.z_angle.value.value = yaw_in_degrees * 10;
1360
1361 if(yaw_std == std::numeric_limits<double>::infinity()) {
1362 object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
1363 } else {
1364 yaw_std *= 180.0 / M_PI;
1365 yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
1366
1367 if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
1368 object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
1369 } else {
1370 object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
1371 }
1372 }
1373 object.angles_is_present = true;
1374}
1375
1387inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
1388 double yaw_rate_std = std::numeric_limits<double>::infinity()) {
1389 double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
1390 // limit value range
1391 if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
1392 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
1393 } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
1394 yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
1395 }
1396
1397 object.z_angular_velocity.value.value = yaw_rate_in_degrees;
1398
1399 if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
1400 object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
1401 } else {
1402 yaw_rate_std *= 180.0 / M_PI;
1403 yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to degrees/s
1404 // How stupid is this?!
1405 static const std::map<double, uint8_t> confidence_map = {
1406 {1.0, AngularSpeedConfidence::DEG_SEC_01},
1407 {2.0, AngularSpeedConfidence::DEG_SEC_02},
1408 {5.0, AngularSpeedConfidence::DEG_SEC_05},
1409 {10.0, AngularSpeedConfidence::DEG_SEC_10},
1410 {20.0, AngularSpeedConfidence::DEG_SEC_20},
1411 {50.0, AngularSpeedConfidence::DEG_SEC_50},
1412 {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
1413 };
1414 for(const auto& [thresh, conf_val] : confidence_map) {
1415 if (yaw_rate_std <= thresh) {
1416 object.z_angular_velocity.confidence.value = conf_val;
1417 break;
1418 }
1419 }
1420 }
1421
1422 object.z_angular_velocity_is_present = true;
1423}
1424
1439inline void setObjectDimension(ObjectDimension& dimension, const double value,
1440 const double confidence = std::numeric_limits<double>::infinity()) {
1441 // limit value range
1442 if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
1443 dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
1444 } else {
1445 dimension.value.value = static_cast<uint16_t>(value);
1446 }
1447
1448 // limit confidence range
1449 if (confidence == std::numeric_limits<double>::infinity()) {
1450 dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
1451 } else if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
1452 dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
1453 } else {
1454 dimension.confidence.value = static_cast<uint8_t>(confidence);
1455 }
1456
1457}
1458
1469inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1470 const double std = std::numeric_limits<double>::infinity()) {
1471 setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1472 object.object_dimension_x_is_present = true;
1473}
1474
1485inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1486 const double std = std::numeric_limits<double>::infinity()) {
1487 setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1488 object.object_dimension_y_is_present = true;
1489}
1490
1501inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
1502 const double std = std::numeric_limits<double>::infinity()) {
1503 setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
1504 object.object_dimension_z_is_present = true;
1505}
1506
1518inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions,
1519 const double x_std = std::numeric_limits<double>::infinity(),
1520 const double y_std = std::numeric_limits<double>::infinity(),
1521 const double z_std = std::numeric_limits<double>::infinity()) {
1522 setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
1523 setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
1524 setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
1525}
1526
1536inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
1537 setPositionOfPerceivedObject(object, point);
1538 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1539}
1540
1553inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
1554 const gm::PointStamped& point, const int16_t delta_time = 0) {
1555 setUTMPositionOfPerceivedObject(cpm, object, point);
1556 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
1557}
1558
1568inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
1569 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
1570 container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
1571}
1572
1586inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
1587 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
1588 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
1589 container.container_data_perceived_object_container.number_of_perceived_objects.value =
1590 container.container_data_perceived_object_container.perceived_objects.array.size();
1591 } else {
1592 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
1593 }
1594}
1595
1608inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1609 // check for maximum number of containers
1610 if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
1611 cpm.payload.cpm_containers.value.array.push_back(container);
1612 } else {
1613 throw std::invalid_argument("Maximum number of CPM-Containers reached");
1614 }
1615}
1616
1625inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
1626 setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
1627 covariance_matrix);
1628}
1629
1638inline void initSensorInformationContainer(WrappedCpmContainer& container) {
1639 container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
1640 container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
1641}
1642
1653inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
1654 throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1655 sensor_information.sensor_id.value = sensor_id;
1656}
1657
1668inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
1669 throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
1670 sensor_information.sensor_type.value = sensor_type;
1671}
1672
1691inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
1692 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1693 // check for maximum number of SensorInformation entries
1694 if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
1695 throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
1696 throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
1697 container.container_data_sensor_information_container.array.push_back(sensor_information);
1698 } else {
1699 throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
1700 }
1701 } else {
1702 throw std::invalid_argument("Container is not a SensorInformationContainer");
1703 }
1704}
1705
1720inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
1721 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
1722 throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
1723 SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
1724 );
1725 addContainerToCPM(cpm, container);
1726 } else {
1727 throw std::invalid_argument("Container is not a SensorInformationContainer");
1728 }
1729}
1730
1731} // 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 743 of file cpm_ts_setters.h.

◆ 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 627 of file cpm_ts_setters.h.

638 {
639 setPositionOfPerceivedObject(object, point);
640 setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
641}
642

◆ setYawRateCDD()

template<typename YawRate, typename YawRateValue = decltype(YawRate::yaw_rate_value), typename YawRateConfidence = decltype(YawRate::yaw_rate_confidence)>
void etsi_its_cpm_ts_msgs::access::setYawRateCDD ( YawRate & yaw_rate,
const double value,
double confidence = std::numeric_limits<double>::infinity() )
inline

Set the Yaw Rate object.

Parameters
yaw_rateobject to set
valueYaw rate in degrees per second as decimal number
confidencestandard deviation of yaw rate in degrees per second as decimal number (default: infinity, mapping to YawRateConfidence::UNAVAILABLE)

Definition at line 386 of file cpm_ts_setters.h.

◆ 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 661 of file cpm_ts_setters.h.

688inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
689 if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
690 container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
691 container.container_data_perceived_object_container.number_of_perceived_objects.value =
692 container.container_data_perceived_object_container.perceived_objects.array.size();
693 } else {
694 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
695 }
696}
697

◆ 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 759 of file cpm_ts_setters.h.

◆ 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 775 of file cpm_ts_setters.h.

◆ 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.