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

/**
 * @brief Set a Cartesian Angle
 * 
 * @param angle The CartesianAngle object to be set.
 * @param value Angle in radians to be set in the CartesianAngle object.
 * @param confidence standard deviation of the angle in radians (default: infinity, which will map to AngleConfidence::UNAVAILABLE).
 */
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;
}

 * Optionally, confidence values can be specified for each velocity component.
 *
 * @param object The perceived object for which the velocity is being set.
 * @param magnitude The magnitude of the velocity in m/s in the xy plane.
 * @param angle The angle of the velocity in radians in the xy plane.
 * @param z The z component of the velocity in m/s (default: 0.0).
 * @param magnitude_std The standard deviation in m/s for the velocity 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 for the z velocity component (default: infinity).
 */
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.cartesian_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;
  }
}

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

 * @param object PerceivedObject to set the Polar Acceleration for
 * @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;
}

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


 * 
 * @param dimension The object dimension to be set.
 * @param value The value of the object dimension in decimeters.
 * @param confidence The confidence of the object dimension in decimeters (optional, default is infinty, mapping to ObjectDimensionConfidence::UNAVAILABLE).
 */
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 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;
}

 * The z-dimension usually represents the height of the object.
 *
 * @param object The `PerceivedObject` to modify.
 * @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);
}


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

/**
 * @brief 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.
 *
 *
 * @param container The WrappedCpmContainer to which the SensorInformation will be added.
 * @param sensor_information The SensorInformation to add to the container.
 *
 * @throws std::invalid_argument if the container is not a SensorInformationContainer.
 * @throws std::invalid_argument if the maximum number of SensorInformation entries is reached.
 * @throws std::invalid_argument if the sensor_id or sensor_type is out of range.
 */
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");
  }
}

 *
 * 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.
 *
 * @param cpm The Collective Perception Message to which the container will be added.
 * @param container The WrappedCpmContainer to be added to the CPM.
 *
 * @throws std::invalid_argument if the container is not a SensorInformationContainer.
 * @throws std::invalid_argument if the number of SensorInformation entries is out of range.
 */
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