cpm_ts_setters.h

Go to the documentation of this file.

// SPDX-License-Identifier: MIT
// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University

 
#pragma once
 
#include <etsi_its_msgs_utils/impl/checks.h>
#include <etsi_its_msgs_utils/impl/constants.h>
 
namespace etsi_its_cpm_ts_msgs::access {
 
#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h>

 * @param cpm The CPM to set the ITS PDU header for.
 * @param station_id The station ID to set in the ITS PDU header.
 * @param protocol_version The protocol version to set in the ITS PDU header. Default is 0.
 */
inline void setItsPduHeader(CollectivePerceptionMessage& cpm, const uint32_t station_id,
                            const uint8_t protocol_version = 0) {
  setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version);
}


 * 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.
 *
 * @param cpm The CPM object to set the reference time in.
 * @param unix_nanosecs The Unix timestamp in nanoseconds representing the reference time.
 * @param n_leap_seconds The number of leap seconds to be considered. Defaults to the todays number of leap seconds since 2004.
 */
inline void setReferenceTime(
    CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs,
    const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.rbegin()->second) {
  TimestampIts t_its;
  setTimestampITS(t_its, unix_nanosecs, n_leap_seconds);
  throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts");
  cpm.payload.management_container.reference_time = t_its;
}

 * @brief 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.
 *
 * @param cpm CPM to set the ReferencePosition
 * @param latitude The latitude value position in degree as decimal number.
 * @param longitude The longitude value in degree as decimal number.
 * @param altitude The altitude value (above the reference ellipsoid surface) in meter as decimal number (optional).
 */
inline void setReferencePosition(CollectivePerceptionMessage& cpm, const double latitude, const double longitude,
                                 const double altitude = AltitudeValue::UNAVAILABLE) {
  setReferencePosition(cpm.payload.management_container.reference_position, latitude, longitude, altitude);
}


 *
 * 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>'
 *
 * @param[out] cpm CPM for which to set the ReferencePosition
 * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position in meters
 * @param[in] zone the UTM zone (zero means UPS) of the given position
 * @param[in] northp hemisphere (true means north, false means south)
 */
inline void setFromUTMPosition(CollectivePerceptionMessage& cpm, const gm::PointStamped& utm_position, const int& zone,
                               const bool& northp) {
  setFromUTMPosition(cpm.payload.management_container.reference_position, utm_position, zone, northp);
}

 *
 * @param object PerceivedObject to set the ID for
 * @param id ID to set
 */
inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) {
  object.object_id.value = id;
  object.object_id_is_present = true;
}


 * between the reference time of the CPM and the measurement of the object.
 *
 * @param object The PerceivedObject to set the measurement delta time for.
 * @param delta_time The measurement delta time to set in milliseconds. Default value is 0.
 * @throws std::invalid_argument if the delta_time is out of range.
 */
inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, const int16_t delta_time = 0) {
  if (delta_time < DeltaTimeMilliSecondSigned::MIN || delta_time > DeltaTimeMilliSecondSigned::MAX) {
    throw std::invalid_argument("MeasurementDeltaTime out of range");
  } else {
    object.measurement_delta_time.value = delta_time;
  }
}

 * 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.
 *
 * @param coordinate The CartesianCoordinateWithConfidence object to be modified.
 * @param value The value to be set in centimeters.
 * @param confidence The confidence to be set in centimeters (default: infinity, which will map to CoordinateConfidence::UNAVAILABLE).
 */
inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const double value,
                                                 const double confidence = std::numeric_limits<double>::infinity()) {
  // limit value range
  if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) {
    coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE;
  } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) {
    coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE;
  } else {
    coordinate.value.value = static_cast<int32_t>(value);
  }
 
  // limit confidence range
  if (confidence == std::numeric_limits<double>::infinity()) {
    coordinate.confidence.value = CoordinateConfidence::UNAVAILABLE;
  } else if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) {
    coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE;
  } else {
    coordinate.confidence.value = static_cast<uint16_t>(confidence);
  }
}

 * @brief 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.
 *
 * @param object The PerceivedObject to set the position for.
 * @param point The gm::Point representing the coordinates of the object in meters.
 * @param x_std The standard deviation in meters for the x-coordinate (default: infinity).
 * @param y_std The standard deviation in meters for the y-coordinate (default: infinity).
 * @param z_std The standard deviation in meters for the z-coordinate (default: infinity).
 */
inline 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()) {
  setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (point.z != 0.0) {
    setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
    object.position.z_coordinate_is_present = true;
  }
}

 * 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.
 *
 * @param cpm The CPM to get the reference position from.
 * @param object The PerceivedObject to set the position for.
 * @param utm_position gm::PointStamped representing the UTM position of the object including the frame_id (utm_<zone><N/S>).
 * @param x_confidence The standard deviation in meters for the x coordinate (default: CoordinateConfidence::UNAVAILABLE).
 * @param y_confidence The standard deviation in meters for the y coordinate (default: CoordinateConfidence::UNAVAILABLE).
 * @param z_confidence The standard deviation in meters for the z coordinate (default: CoordinateConfidence::UNAVAILABLE).
 *
 * @throws std::invalid_argument if the UTM-Position frame_id does not match the reference position frame_id.
 */
inline 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()) {
  gm::PointStamped reference_position = getUTMPosition(cpm);
  if (utm_position.header.frame_id != reference_position.header.frame_id) {
    throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id +
                                ") does not match the reference position frame_id (" + reference_position.header.frame_id +
                                ")");
  }
  setCartesianCoordinateWithConfidence(object.position.x_coordinate,
                                       (utm_position.point.x - reference_position.point.x) * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  setCartesianCoordinateWithConfidence(object.position.y_coordinate,
                                       (utm_position.point.y - reference_position.point.y) * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (utm_position.point.z != 0.0) {
    setCartesianCoordinateWithConfidence(object.position.z_coordinate,
                                         (utm_position.point.z - reference_position.point.z) * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
    object.position.z_coordinate_is_present = true;
  }
}


inline void setVelocityComponent(VelocityComponent& velocity, const double value,
                                 const double confidence = std::numeric_limits<double>::infinity()) {
  // limit value range
  if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) {
    velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE;
  } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
    velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE;
  } else {
    velocity.value.value = static_cast<int16_t>(value);
  }
 
  // limit confidence range
  if(confidence == std::numeric_limits<double>::infinity()) {
    velocity.confidence.value = SpeedConfidence::UNAVAILABLE;
  } else if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) {
    velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE;
  } else {
    velocity.confidence.value = static_cast<uint8_t>(confidence);
  }
}


inline void setCartesianAngle(CartesianAngle& angle, const double value,
                              double confidence = std::numeric_limits<double>::infinity()) {
  // wrap angle to 0..2pi
  double wrapped_value = fmod(value, 2*M_PI);
  if (wrapped_value < 0.0) {
    wrapped_value += 2 * M_PI;
  }
  angle.value.value = static_cast<uint16_t>(wrapped_value * 180 / M_PI * 10); // convert to 0.1 degrees
 
  confidence *= 180.0 / M_PI * 10.0 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to 0.1 degrees
  // limit confidence range
  if(confidence == std::numeric_limits<double>::infinity() || confidence <= 0.0) {
    angle.confidence.value = AngleConfidence::UNAVAILABLE;
  } else if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) {
    angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
  } else {
    angle.confidence.value = static_cast<uint8_t>(confidence);
  }
}


inline 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()) {
  object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY;
  setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (cartesian_velocity.z != 0.0) {
    setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
    object.velocity.cartesian_velocity.z_velocity_is_present = true;
  } else {
    object.velocity.cartesian_velocity.z_velocity_is_present = false;
  }
  object.velocity_is_present = true;
}

inline 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()) {
  object.velocity.choice = Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY;
  setSpeed(object.velocity.polar_velocity.velocity_magnitude, magnitude, magnitude_std);
  setCartesianAngle(object.velocity.polar_velocity.velocity_direction, angle, angle_std);
  if (z != 0.0) {
    setVelocityComponent(object.velocity.polar_velocity.z_velocity, z * 100, z_std * 100 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
    object.velocity.polar_velocity.z_velocity_is_present = true;
  } else {
    object.velocity.polar_velocity.z_velocity_is_present = false;
  }
}

/**
 * @brief 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.
 *
 * @param acceleration The AccelerationComponent object to set the value and confidence for.
 * @param value The value to set for the AccelerationComponent in dm/s^2.
 * @param confidence The confidence to set for the AccelerationComponent in dm/s^2. Default value is infinity, mapping to AccelerationConfidence::UNAVAILABLE.
 */
inline void setAccelerationComponent(AccelerationComponent& acceleration, const double value,
                                     const double confidence = std::numeric_limits<double>::infinity()) {
  // limit value range
  if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) {
    acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE;
  } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) {
    acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE;
  } else {
    acceleration.value.value = static_cast<int16_t>(value);
  }
 
  // limit confidence range
  if(confidence == std::numeric_limits<double>::infinity()) {
    acceleration.confidence.value = AccelerationConfidence::UNAVAILABLE;
  } else if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) {
    acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE;
  } else {
    acceleration.confidence.value = static_cast<uint8_t>(confidence);
  }
}


inline 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()) {
  object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION;
  setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10,
                           x_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10,
                           y_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  if (cartesian_acceleration.z != 0.0) {
    setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10,
                             z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
    object.acceleration.cartesian_acceleration.z_acceleration_is_present = true;
  }
  object.acceleration_is_present = true;
}

 * @param magnitude The magnitude of the acceleration in m/s^2 in the xy plane.
 * @param angle The angle of the acceleration in radians in the xy plane.
 * @param z The z component of the acceleration in m/s^2 (default: 0.0).
 * @param magnitude_std The standard deviation in m/s^2 for the acceleration magnitude (default: infinity).
 * @param angle_std The standard deviation in radians for the angle (default: infinity).
 * @param z_std The standard deviation in m/s^2 for the z acceleration component (default: infinity).
 */
inline 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()) {
  object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION;
  setAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude, magnitude, magnitude_std);
  setCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction, angle, angle_std);
  if (z != 0.0) {
    setAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration, z * 10,
                             z_std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
    object.acceleration.polar_acceleration.z_acceleration_is_present = true;
  } else {
    object.acceleration.polar_acceleration.z_acceleration_is_present = false;
  }
  object.acceleration_is_present = true;
}

inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw,
                                    double yaw_std = std::numeric_limits<double>::infinity()) {
  // wrap angle to range [0, 360)
  double yaw_in_degrees = yaw * 180 / M_PI;
  while (yaw_in_degrees >= 360.0) yaw_in_degrees -= 360.0;
  while (yaw_in_degrees < 0.0) yaw_in_degrees += 360.0;
  object.angles.z_angle.value.value = yaw_in_degrees * 10;
 
  if(yaw_std == std::numeric_limits<double>::infinity()) {
    object.angles.z_angle.confidence.value = AngleConfidence::UNAVAILABLE;
  } else {
    yaw_std *= 180.0 / M_PI;
    yaw_std *= 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;  // convert to 0.1 degrees
 
    if (yaw_std > AngleConfidence::MAX || yaw_std < AngleConfidence::MIN) {
      object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE;
    } else {
      object.angles.z_angle.confidence.value = static_cast<uint8_t>(yaw_std);
    }
  }
  object.angles_is_present = true;
}


inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate,
                                        double yaw_rate_std = std::numeric_limits<double>::infinity()) {
  double yaw_rate_in_degrees = yaw_rate * 180.0 / M_PI;
  // limit value range
  if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) {
    yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE;
  } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) {
    yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE;
  }
 
  object.z_angular_velocity.value.value = yaw_rate_in_degrees;
 
  if(yaw_rate_std == std::numeric_limits<double>::infinity()) {
    object.z_angular_velocity.confidence.value = AngularSpeedConfidence::UNAVAILABLE;
  } else {
    yaw_rate_std *= 180.0 / M_PI;
    yaw_rate_std *= etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;  // convert to degrees/s
    // How stupid is this?!
    static const std::map<double, uint8_t> confidence_map = {
        {1.0, AngularSpeedConfidence::DEG_SEC_01},
        {2.0, AngularSpeedConfidence::DEG_SEC_02},
        {5.0, AngularSpeedConfidence::DEG_SEC_05},
        {10.0, AngularSpeedConfidence::DEG_SEC_10},
        {20.0, AngularSpeedConfidence::DEG_SEC_20},
        {50.0, AngularSpeedConfidence::DEG_SEC_50},
        {std::numeric_limits<double>::infinity(), AngularSpeedConfidence::OUT_OF_RANGE},
    };
    for(const auto& [thresh, conf_val] : confidence_map) {
      if (yaw_rate_std <= thresh) {
        object.z_angular_velocity.confidence.value = conf_val;
        break;
      }
    }
  }
 
  object.z_angular_velocity_is_present = true;
}

inline void setObjectDimension(ObjectDimension& dimension, const double value,
                               const double confidence = std::numeric_limits<double>::infinity()) {
  // limit value range
  if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) {
    dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE;
  } else {
    dimension.value.value = static_cast<uint16_t>(value);
  }
 
  // limit confidence range
  if (confidence == std::numeric_limits<double>::infinity()) {
    dimension.confidence.value = ObjectDimensionConfidence::UNAVAILABLE;
  } else   if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) {
    dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE;
  } else {
    dimension.confidence.value = static_cast<uint8_t>(confidence);
  }
 
}

inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value,
                                           const double std = std::numeric_limits<double>::infinity()) {
  setObjectDimension(object.object_dimension_x, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  object.object_dimension_x_is_present = true;
}

inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value,
                                           const double std = std::numeric_limits<double>::infinity()) {
  setObjectDimension(object.object_dimension_y, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  object.object_dimension_y_is_present = true;
}

inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value,
                                           const double std = std::numeric_limits<double>::infinity()) {
  setObjectDimension(object.object_dimension_z, value * 10, std * 10 * etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR);
  object.object_dimension_z_is_present = true;
}

inline 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()) {
  setXDimensionOfPerceivedObject(object, dimensions.x, x_std);
  setYDimensionOfPerceivedObject(object, dimensions.y, y_std);
  setZDimensionOfPerceivedObject(object, dimensions.z, z_std);
}

inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) {
  setPositionOfPerceivedObject(object, point);
  setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
}

inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object,
                                               const gm::PointStamped& point, const int16_t delta_time = 0) {
  setUTMPositionOfPerceivedObject(cpm, object, point);
  setMeasurementDeltaTimeOfPerceivedObject(object, delta_time);
}

inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0) {
  container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER;
  container.container_data_perceived_object_container.number_of_perceived_objects.value = n_objects;
}

inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) {
  if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
    container.container_data_perceived_object_container.perceived_objects.array.push_back(perceived_object);
    container.container_data_perceived_object_container.number_of_perceived_objects.value =
        container.container_data_perceived_object_container.perceived_objects.array.size();
  } else {
    throw std::invalid_argument("Container is not a PerceivedObjectContainer");
  }
}

inline void addContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
  // check for maximum number of containers
  if (cpm.payload.cpm_containers.value.array.size() < WrappedCpmContainers::MAX_SIZE) {
    cpm.payload.cpm_containers.value.array.push_back(container);
  } else {
    throw std::invalid_argument("Maximum number of CPM-Containers reached");
  }
}

inline void setWGSRefPosConfidence(CollectivePerceptionMessage& cpm, const std::array<double, 4>& covariance_matrix) {
  setWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse,
                             covariance_matrix);
}

inline void initSensorInformationContainer(WrappedCpmContainer& container) {
  container.container_id.value = WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER;
  container.container_data_sensor_information_container.array = std::vector<SensorInformation>();
}

inline void setSensorID(SensorInformation& sensor_information, const uint8_t sensor_id = 0) {
  throwIfOutOfRange(sensor_id, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
  sensor_information.sensor_id.value = sensor_id;
}

inline void setSensorType(SensorInformation& sensor_information, const uint8_t sensor_type = SensorType::UNDEFINED) {
  throwIfOutOfRange(sensor_type, SensorType::MIN, SensorType::MAX, "SensorType");
  sensor_information.sensor_type.value = sensor_type;
}

inline void addSensorInformationToContainer(WrappedCpmContainer& container, const SensorInformation& sensor_information) {
  if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
    // check for maximum number of SensorInformation entries
    if (container.container_data_sensor_information_container.array.size() < SensorInformationContainer::MAX_SIZE ) {
      throwIfOutOfRange(sensor_information.sensor_id.value, Identifier1B::MIN, Identifier1B::MAX, "SensorID");
      throwIfOutOfRange(sensor_information.sensor_type.value, SensorType::MIN, SensorType::MAX, "SensorType");
      container.container_data_sensor_information_container.array.push_back(sensor_information);
    } else {
      throw std::invalid_argument("Maximum number of entries SensorInformationContainers reached");
    }
  } else {
    throw std::invalid_argument("Container is not a SensorInformationContainer");
  }
}

inline void addSensorInformationContainerToCPM(CollectivePerceptionMessage& cpm, const WrappedCpmContainer& container) {
  if (container.container_id.value == WrappedCpmContainer::CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER) {
    throwIfOutOfRange(container.container_data_sensor_information_container.array.size(),
      SensorInformationContainer::MIN_SIZE, SensorInformationContainer::MAX_SIZE, "SensorInformationContainer array size"
    );
    addContainerToCPM(cpm, container);
  } else {
    throw std::invalid_argument("Container is not a SensorInformationContainer");
  }
}
 
}  // namespace etsi_its_cpm_ts_msgs::access