cpm_ts_getters.h

Go to the documentation of this file.

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

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

   * @param cpm The CPM from which to retrieve the station ID.
   * @return The station ID extracted from the header of the CPM.
   */
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);
}

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

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);
    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.polar_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");
  }
 
}

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

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

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

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

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

inline const std::array<double, 4> getWGSRefPosConfidence(const CollectivePerceptionMessage &cpm) {
  return getWGSPosConfidenceEllipse(cpm.payload.management_container.reference_position.position_confidence_ellipse);
}

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