cpm_ts_setters.h

Go to the documentation of this file.

/*
=============================================================================
MIT License
 
Copyright (c) 2023-2025 Institute for Automotive Engineering (ika), RWTH Aachen University
 
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
 
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
=============================================================================
*/

 
#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;
}


 * @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);
}



 * @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);
}

 * @brief Set the ID of a PerceivedObject
 *
 * Sets the object_id of the PerceivedObject to the given ID.
 *
 * @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;
}


 * 
 * 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.
 * 
 * @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;
  }
}


 * @brief 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.
 * 
 * @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;
  }
}


 * @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;
  }
}

 * 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 velocity The VelocityComponent object to set the value and confidence for.
 * @param value The value to set for the VelocityComponent in cm/s.
 * @param confidence The confidence to set for the VelocityComponent in cm/s. Default value is infinity, mapping to SpeedConfidence::UNAVAILABLE.
 */
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);
  }
}

 *
 * 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.
 *
 * @param object The perceived object for which the velocity is being set.
 * @param cartesian_velocity The Cartesian velocity components (x, y, z) of the object (in m/s).
 * @param x_std The standard deviation in m/s for the x velocity component (default: infinity).
 * @param y_std The standard deviation in m/s for the y velocity component (default: infinity).
 * @param z_std The standard deviation in m/s for the z velocity component (default: infinity).
 */
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;
  }
  object.velocity_is_present = true;
}


 * @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);
  }
}

 * @brief 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.
 *
 * @param object The perceived object for which the acceleration is being set.
 * @param cartesian_acceleration The Cartesian acceleration components (x, y, z) of the object (in m/s^2).
 * @param x_std The standard deviation in m/s^2 for the x acceleration component (default: infinity).
 * @param y_std The standard deviation in m/s^2 for the y acceleration component (default: infinity).
 * @param z_std The standard deviation in m/s^2 for the z acceleration component (default: infinity).
 */
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;
}

 * The function also allows specifying the confidence level of the yaw angle.
 *
 * @param object The PerceivedObject to set the yaw angle for.
 * @param yaw The yaw angle in radians.
 * @param yaw_std The standard deviation of the yaw angle in radians (optional, default is Infinity).
 */
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);
  }
 
}

 * @param object The `PerceivedObject` to modify.
 * @param value The value to set as the x-dimension in meters.
 * @param std The standard deviation of the x-dimension value in meters (optional, default is infinity).
 */
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;
}

 * @param value The value to set as the z-dimension in meters.
 * @param std The standard deviation of the z-dimension value in meters (optional, default is infinity).
 */
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);
}

/**
 * @brief 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.
 *
 * @param container A reference to the WrappedCpmContainer to be initialized as a PerceivedObjectContainer.
 * @param n_objects The number of perceived objects to initialize in the container. Default is 0.
 */
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);
}
 
}  // namespace etsi_its_cpm_ts_msgs::access