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; }
 
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);
}
 
inline double getAltitude(const CollectivePerceptionMessage &cpm) {
  return getAltitude(cpm.payload.management_container.reference_position.altitude);
}
 
inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) {
  return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
}
 
inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) {
  int zone;
  bool northp;
  return getUTMPosition(cpm, zone, northp);
}
 
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);
}
 
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;
}
 
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) { return object.object_id.value; }
 
inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) {
  return coordinate.value.value;
}
 
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;
}
 
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);
}
 
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");
  if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
    throw std::invalid_argument("Velocity is not Cartesian");
  }
  gm::Vector3 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);
  }
  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_CARTESIAN_VELOCITY) {
    throw std::invalid_argument("Velocity is not Cartesian");
  }
  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);
}
 
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");
  if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
    throw std::invalid_argument("Acceleration is not Cartesian");
  }
  gm::Vector3 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);
  }
  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) {
    throw std::invalid_argument("Acceleration is not Cartesian");
  }
  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);
}
 
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