cpm_ts_getters.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 <tuple>
 
namespace etsi_its_cpm_ts_msgs::access {
 
#include <etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h>

inline uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); }

inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
  return cpm.payload.management_container.reference_time;
}

inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }

   * @param cpm CPM to get the Latitude value from
   * @return Latitude value in degree as decimal number
   */
inline double getLatitude(const CollectivePerceptionMessage &cpm) {
  return getLatitude(cpm.payload.management_container.reference_position.latitude);
}

inline double getLongitude(const CollectivePerceptionMessage &cpm) {
  return getLongitude(cpm.payload.management_container.reference_position.longitude);
}

   * @return Altitude value in (above the reference ellipsoid surface) in meter as decimal number
   */
inline double getAltitude(const CollectivePerceptionMessage &cpm) {
  return getAltitude(cpm.payload.management_container.reference_position.altitude);
}

   *
   * @param[in] cpm CPM to get the UTM Position from
   * @param[out] zone the UTM zone (zero means UPS)
   * @param[out] northp hemisphere (true means north, false means south)
   * @return geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id
   */
inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
  return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
}

 * @brief Get the UTM Position defined within the ManagementContainer of the CPM
 *
 * The position is transformed into UTM by using GeographicLib::UTMUPS
 * The altitude value is directly used as z-Coordinate
 *
 * @param cpm CPM from which to extract the UTM position.
 * @return geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id
 */
inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
  int zone;
  bool northp;
  return getUTMPosition(cpm, zone, northp);
}

 *
 * This function iterates over the cpm_containers array in the given CPM
 * and extracts the container IDs into a vector of uint8_t.
 *
 * @param cpm The CPM from which to retrieve the container IDs.
 * @return A vector containing the container IDs.
 */
inline std::vector<uint8_t> getCpmContainerIds(const CollectivePerceptionMessage &cpm) {
  std::vector<uint8_t> container_ids;
  for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
    container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
  }
  return container_ids;
}


inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id) {
  for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
    if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
      return cpm.payload.cpm_containers.value.array[i];
    }
  }
  throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
}

inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) {
  return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
}

 * @throws std::invalid_argument if the container is not a PerceivedObjectContainer.
 */
inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) {
  if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
    throw std::invalid_argument("Container is not a PerceivedObjectContainer");
  }
  uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
  return number;
}

 * Retrieves the number of perceived objects from the given CPM.
 *
 * @param cpm The CPM from which to retrieve the number of perceived objects.
 * @return The number of perceived objects.
 */
inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) {
  return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm));
}



inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) {
  if (i >= getNumberOfPerceivedObjects(container)) {
    throw std::invalid_argument("Index out of range");
  }
  return container.container_data_perceived_object_container.perceived_objects.array[i];
}

 */
inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) {
  if (!object.object_id_is_present) {
    throw std::invalid_argument("No object_id present in PerceivedObject");
  }
  return object.object_id.value;
}

inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
  return coordinate.value.value;
}

 * @brief Retrieves the confidence value from a CartesianCoordinateWithConfidence object.
 *
 * @param coordinate The CartesianCoordinateWithConfidence object from which to retrieve the confidence value.
 * @return The confidence value of the CartesianCoordinateWithConfidence object in centimeters.
 */
inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) {
  return coordinate.confidence.value;
}

inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) {
  gm::Point point;
  point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100.0;
  point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0;
  if (object.position.z_coordinate_is_present) {
    point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0;
  }
  return point;
}

 * @return std::tuple<double, double, double> x,y and z standard deviations of the positions in meters. 
 *         The z standard deviation is infinity if the z coordinate is not present.
 */
inline std::tuple<double, double, double> getPositionConfidenceOfPerceivedObject(const PerceivedObject &object) {
  double x_confidence = double(getCartesianCoordinateConfidence(object.position.x_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
  double y_confidence = double(getCartesianCoordinateConfidence(object.position.y_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
  double z_confidence = object.position.z_coordinate_is_present
                            ? double(getCartesianCoordinateConfidence(object.position.z_coordinate)) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR
                            : std::numeric_limits<double>::infinity();
  return std::make_tuple(x_confidence, y_confidence, z_confidence);
}

 * @brief Get the Cartesian angle of the PerceivedObject
 * 
 * @param object PerceivedObject to get the Cartesian angle from
 * @return unit16_t Cartesian angle of the PerceivedObject in 0,1 degrees
 */
inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; }

inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; }


inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) {
  if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
  tf2::Quaternion q;
  double roll{0}, pitch{0}, yaw{0};
 
  if (object.angles.x_angle_is_present) {
    roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) ) / 180.0) *
           M_PI;
  }
  if (object.angles.y_angle_is_present) {
    pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) ) / 180.0) *
            M_PI;
  }
  yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0)) / 180.0) *
        M_PI;
  // This wraps from -pi ti pi because setRPY only uses cos and sin of the angles
  q.setRPY(roll, pitch, yaw);
 
  return tf2::toMsg(q);
}

/**
 * @brief Get the yaw of the PerceivedObject
 * 
 * @param object PerceivedObject to get the yaw from
 * @return double yaw of the PerceivedObject in radians from -pi to pi
 */
inline double getYawOfPerceivedObject(const PerceivedObject &object) {
  gm::Quaternion orientation = getOrientationOfPerceivedObject(object);
  tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
  double roll, pitch, yaw;
  tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
  return yaw;
}

inline double getYawConfidenceOfPerceivedObject(const PerceivedObject &object) {
  if(!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
  double yaw_confidence = object.angles.z_angle.confidence.value / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
  yaw_confidence *= M_PI / 180.0;  // convert to radians
  return yaw_confidence;
}


inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) {
  gm::Pose pose;
  pose.position = getPositionOfPerceivedObject(object);
  pose.orientation = getOrientationOfPerceivedObject(object);
  return pose;
}


inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {
  if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
  return object.z_angular_velocity.value.value * M_PI / 180.0;  // convert to rad/s
}


inline double getYawRateConfidenceOfPerceivedObject(const PerceivedObject &object) {
  if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
  auto val = object.z_angular_velocity.confidence.value;
  static const std::map<uint8_t, double> confidence_map = {
      {AngularSpeedConfidence::UNAVAILABLE, 0.0},
      {AngularSpeedConfidence::DEG_SEC_01, 1.0},
      {AngularSpeedConfidence::DEG_SEC_02, 2.0},
      {AngularSpeedConfidence::DEG_SEC_05, 5.0},
      {AngularSpeedConfidence::DEG_SEC_10, 10.0},
      {AngularSpeedConfidence::DEG_SEC_20, 20.0},
      {AngularSpeedConfidence::DEG_SEC_50, 50.0},
      {AngularSpeedConfidence::OUT_OF_RANGE, std::numeric_limits<double>::infinity()},
  };
  return confidence_map.at(val) * M_PI / 180.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;  // convert to rad/s
}


inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100.0; }

inline double getVelocityComponentConfidence(const VelocityComponent &velocity) {
  return double(velocity.confidence.value) / 100.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
}


inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object) {
  if (!object.velocity_is_present) {
    throw std::invalid_argument("No velocity present in PerceivedObject");
  }
  gm::Vector3 velocity;
  if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
    double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
    double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
    velocity.x = speed * cos(angle);
    velocity.y = speed * sin(angle);
    if (object.velocity.polar_velocity.z_velocity_is_present) {
      velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
    }
  } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
    velocity.x = getVelocityComponent(object.velocity.cartesian_velocity.x_velocity);
    velocity.y = getVelocityComponent(object.velocity.cartesian_velocity.y_velocity);
    if (object.velocity.cartesian_velocity.z_velocity_is_present) {
      velocity.z = getVelocityComponent(object.velocity.cartesian_velocity.z_velocity);
    }
  } else {
    throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
  }
  return velocity;
}

/**
 * @brief Get the Cartesian Velocity Confidence Of Perceived Object object
 * 
 * @param object PerceivedObject to get the Cartesian velocity from
 * @return std::tuple<double, double, double> the x,y and z standard deviations of the velocity in m/s
 *         The z standard deviation is infinity if the z velocity is not present.
 * @throws std::invalid_argument If the velocity is no cartesian velocity.
 */
inline std::tuple<double, double, double> getCartesianVelocityConfidenceOfPerceivedObject(const PerceivedObject &object) {
  if (!object.velocity_is_present) {
    throw std::invalid_argument("No velocity present in PerceivedObject");
  }
  if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_POLAR_VELOCITY) {
    double speed_confidence = getSpeedConfidence(object.velocity.polar_velocity.velocity_magnitude) / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
    double angle_confidence = getCartesianAngleConfidence(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
    double speed = getSpeed(object.velocity.polar_velocity.velocity_magnitude);
    double angle = getCartesianAngle(object.velocity.polar_velocity.velocity_direction) * M_PI / 180.0 / 10.0; // convert to radians
    double lateral_confidence = speed * angle_confidence; // not exactly, but best approximation
    double x_confidence = speed_confidence * cos(angle) * cos(angle) 
                          + lateral_confidence * sin(angle) * sin(angle);
    double y_confidence = speed_confidence * sin(angle) * sin(angle)
                          + lateral_confidence * cos(angle) * cos(angle);
    // neglect xy covariance, as it is not present in the output of this function
    double z_confidence = object.velocity.polar_velocity.z_velocity_is_present
                              ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
                              : std::numeric_limits<double>::infinity();
    return std::make_tuple(x_confidence, y_confidence, z_confidence);
  } else if (object.velocity.choice == Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
    double x_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.x_velocity);
    double y_confidence = getVelocityComponentConfidence(object.velocity.cartesian_velocity.y_velocity);
    double z_confidence = object.velocity.cartesian_velocity.z_velocity_is_present
                              ? getVelocityComponentConfidence(object.velocity.cartesian_velocity.z_velocity)
                              : std::numeric_limits<double>::infinity();
    return std::make_tuple(x_confidence, y_confidence, z_confidence);
  } else {
    throw std::invalid_argument("Velocity is neither Polar nor Cartesian");
  }
  
}

 * @return double value of the acceleration component in m/s^2
 */
inline double getAccelerationComponent(const AccelerationComponent &acceleration) {
  return double(acceleration.value.value) / 10.0;
}


inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) {
  return double(acceleration.confidence.value) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
}

 * @brief Get the Cartesian acceleration of the PerceivedObject
 * 
 * @param object PerceivedObject to get the Cartesian acceleration from
 * @return gm::Vector3 Cartesian acceleration of the PerceivedObject in m/s^2 (local coordinate system)
 * @throws std::invalid_argument If the acceleration is no cartesian acceleration.
 */
inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) {
  if (!object.acceleration_is_present) {
    throw std::invalid_argument("No acceleration present in PerceivedObject");
  }
  gm::Vector3 acceleration;
 
  if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
    acceleration.x = getAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration);
    acceleration.y = getAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration);
    if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
      acceleration.z = getAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration);
    }
  } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
    double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
    double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
    acceleration.x = magnitude * cos(angle);
    acceleration.y = magnitude * sin(angle);
    if (object.acceleration.polar_acceleration.z_acceleration_is_present) {
      acceleration.z = getAccelerationComponent(object.acceleration.polar_acceleration.z_acceleration);
    }
  } else {
    throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
  }
 
  return acceleration;
}

inline std::tuple<double, double, double> getCartesianAccelerationConfidenceOfPerceivedObject(const PerceivedObject &object) {
  if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
 
  if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
    double x_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.x_acceleration);
    double y_confidence = getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.y_acceleration);
    double z_confidence = object.acceleration.cartesian_acceleration.z_acceleration_is_present
                              ? getAccelerationComponentConfidence(object.acceleration.cartesian_acceleration.z_acceleration)
                              : std::numeric_limits<double>::infinity();
    return std::make_tuple(x_confidence, y_confidence, z_confidence);
  } else if (object.acceleration.choice == Acceleration3dWithConfidence::CHOICE_POLAR_ACCELERATION) {
    double magnitude_confidence = getAccelerationMagnitudeConfidence(object.acceleration.polar_acceleration.acceleration_magnitude);
    double angle_confidence = getCartesianAngleConfidence(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR; // convert to radians
    double magnitude = getAccelerationMagnitude(object.acceleration.polar_acceleration.acceleration_magnitude);
    double angle = getCartesianAngle(object.acceleration.polar_acceleration.acceleration_direction) * M_PI / 180.0 / 10.0; // convert to radians
    double lateral_confidence = magnitude * angle_confidence; // best approximation
    double x_confidence = magnitude_confidence * cos(angle) * cos(angle)
                          + lateral_confidence * sin(angle) * sin(angle);
    double y_confidence = magnitude_confidence * sin(angle) * sin(angle)
                          + lateral_confidence * cos(angle) * cos(angle);
    // neglect xy covariance, as it is not present in the output of this function
    double z_confidence = object.acceleration.polar_acceleration.z_acceleration_is_present
                              ? getAccelerationComponentConfidence(object.acceleration.polar_acceleration.z_acceleration)
                              : std::numeric_limits<double>::infinity();
    return std::make_tuple(x_confidence, y_confidence, z_confidence);
  } else {
    throw std::invalid_argument("Acceleration is neither Cartesian nor Polar");
  }
}


 * @return The x-dimension of the perceived object in decimeters.
 * @throws std::invalid_argument if the x-dimension is not present in the PerceivedObject.
 */
inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) {
  if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
  return object.object_dimension_x.value.value;
}

 * 
 * @param object  The PerceivedObject from which to retrieve the x-dimension confidence.
 * @return uint8_t The confidence of the x-dimension of the perceived object in decimeters.
 * @throws std::invalid_argument if the x-dimension is not present in the PerceivedObject.
 */
inline uint8_t getXDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
  if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
  return object.object_dimension_x.confidence.value;
}

 * @param object The PerceivedObject from which to retrieve the y-dimension.
 * @return uint16_t y-dimension of the perceived object in in decimeters.
 * @throws std::invalid_argument if the y-dimension is not present in the PerceivedObject.
 */
inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) {
  if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
  return object.object_dimension_y.value.value;
}

inline uint8_t getYDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
  if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
  return object.object_dimension_y.confidence.value;
}

inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) {
  if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
  return object.object_dimension_z.value.value;
}

inline uint8_t getZDimensionConfidenceOfPerceivedObject(const PerceivedObject &object) {
  if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
  return object.object_dimension_z.confidence.value;
}


inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) {
  gm::Vector3 dimensions;
  dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0;
  dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0;
  dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0;
  return dimensions;
}


inline std::tuple<double, double, double> getDimensionsConfidenceOfPerceivedObject(const PerceivedObject &object) {
  double conf_x = double(getXDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
  double conf_y = double(getYDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
  double conf_z = double(getZDimensionConfidenceOfPerceivedObject(object)) / 10.0 / etsi_its_msgs::ONE_D_GAUSSIAN_FACTOR;
  return {conf_x, conf_y, conf_z};
}


inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm,
                                                        const PerceivedObject &object) {
  gm::PointStamped utm_position;
  gm::PointStamped reference_position = getUTMPosition(cpm);
  gm::Point relative_position = getPositionOfPerceivedObject(object);
 
  utm_position.header.frame_id = reference_position.header.frame_id;
  utm_position.point.x = reference_position.point.x + relative_position.x;
  utm_position.point.y = reference_position.point.y + relative_position.y;
  utm_position.point.z = reference_position.point.z + relative_position.z;
 
  return utm_position;
}

 * @brief Get the confidence ellipse of the reference position as Covariance matrix
 * 
 * The covariance matrix will have the entries cov_xx, cov_xy, cov_yx, cov_yy
 * where x is WGS84 North and y is East
 * 
 * @param cpm The CPM message to get the reference position from
 * @return const std::array<double, 4> the covariance matrix, as specified above
 */
inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
  return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
}

 * 
 * @param sensor_information The SensorInformation to get the sensorId from
 * @return uint8_t The sensorId of a SensorInformation object
 */
inline uint8_t getSensorID(const SensorInformation &sensor_information) {
  return sensor_information.sensor_id.value;
}

inline uint8_t getSensorType(const SensorInformation &sensor_information) {
  return sensor_information.sensor_type.value;
}
 
}  // namespace etsi_its_cpm_ts_msgs::access