Retrieves the dimensions of a perceived object.
This function extracts the dimensions of a perceived object from the given PerceivedObject. The dimensions are returned as a gm::Vector3 object with the x, y, and z dimensions in meters.
- Parameters
-
| object | The PerceivedObject for which to calculate the dimensions. |
- Returns
- The dimensions of the perceived object as a
gm::Vector3 object in meters.
Definition at line 580 of file cpm_ts_getters.h.
626
633inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) {
634 return cpm.payload.management_container.reference_time;
635}
636
643inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; }
644
651inline double getLatitude(
const CollectivePerceptionMessage &cpm) {
652 return getLatitude(cpm.payload.management_container.reference_position.latitude);
653}
654
661inline double getLongitude(
const CollectivePerceptionMessage &cpm) {
662 return getLongitude(cpm.payload.management_container.reference_position.longitude);
663}
664
671inline double getAltitude(
const CollectivePerceptionMessage &cpm) {
672 return getAltitude(cpm.payload.management_container.reference_position.altitude);
673}
674
686inline gm::PointStamped
getUTMPosition(
const CollectivePerceptionMessage &cpm,
int &zone,
bool &northp) {
687 return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp);
688}
689
699inline gm::PointStamped
getUTMPosition(
const CollectivePerceptionMessage &cpm) {
700 int zone;
701 bool northp;
703}
704
714inline std::vector<uint8_t>
getCpmContainerIds(
const CollectivePerceptionMessage &cpm) {
715 std::vector<uint8_t> container_ids;
716 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
717 container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value);
718 }
719 return container_ids;
720}
721
730inline WrappedCpmContainer
getCpmContainer(
const CollectivePerceptionMessage &cpm,
const uint8_t container_id) {
731 for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) {
732 if (cpm.payload.cpm_containers.value.array[i].container_id.value == container_id) {
733 return cpm.payload.cpm_containers.value.array[i];
734 }
735 }
736 throw std::invalid_argument("No Container with ID " + std::to_string(container_id) + " found in CPM");
737}
738
748 return getCpmContainer(cpm, WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER);
749}
750
759 if (container.container_id.value != WrappedCpmContainer::CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER) {
760 throw std::invalid_argument("Container is not a PerceivedObjectContainer");
761 }
762 uint8_t number = container.container_data_perceived_object_container.number_of_perceived_objects.value;
763 return number;
764}
765
774}
775
779
788inline PerceivedObject
getPerceivedObject(
const WrappedCpmContainer &container,
const uint8_t i) {
790 throw std::invalid_argument("Index out of range");
791 }
792 return container.container_data_perceived_object_container.perceived_objects.array[i];
793}
794
804
812 return coordinate.value.value;
813}
814
822 return coordinate.confidence.value;
823}
824
832 gm::Point point;
835 if (object.position.z_coordinate_is_present) {
837 }
838 return point;
839}
840
847inline uint16_t
getCartesianAngle(
const CartesianAngle &angle) {
return angle.value.value; }
848
856
868 if (!object.angles_is_present) throw std::invalid_argument("No angles present in PerceivedObject");
869 tf2::Quaternion q;
870 double roll{0}, pitch{0}, yaw{0};
871
872 if (object.angles.x_angle_is_present) {
873 roll = (((double(
getCartesianAngle(
object.angles.x_angle)) / 10.0) - 180.0) / 180.0) *
874 M_PI;
875 }
876 if (object.angles.y_angle_is_present) {
877 pitch = (((double(
getCartesianAngle(
object.angles.y_angle)) / 10.0) - 180.0) / 180.0) *
878 M_PI;
879 }
880 yaw = (((double(
getCartesianAngle(
object.angles.z_angle)) / 10.0) - 180.0) / 180.0) *
881 M_PI;
882 q.setRPY(roll, pitch, yaw);
883
884 return tf2::toMsg(q);
885}
886
895 tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
896 double roll, pitch, yaw;
897 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
898 return yaw;
899}
900
908 gm::Pose pose;
911 return pose;
912}
913
922 if (!object.z_angular_velocity_is_present) throw std::invalid_argument("No yaw rate present in PerceivedObject");
923 return object.z_angular_velocity.value.value;
924}
925
932inline double getVelocityComponent(
const VelocityComponent &velocity) {
return double(velocity.value.value) / 100.0; }
933
941 return double(velocity.confidence.value) / 100.0;
942}
943
952 if (!object.velocity_is_present) throw std::invalid_argument("No velocity present in PerceivedObject");
953 if (object.velocity.choice != Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY) {
954 throw std::invalid_argument("Velocity is not Cartesian");
955 }
956 gm::Vector3 velocity;
959 if (object.velocity.cartesian_velocity.z_velocity_is_present) {
961 }
962 return velocity;
963}
964
972 return double(acceleration.value.value) / 10.0;
973}
974
982 return double(acceleration.confidence.value) / 10.0;
983}
984
993 if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject");
994 if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) {
995 throw std::invalid_argument("Acceleration is not Cartesian");
996 }
997 gm::Vector3 acceleration;
1000 if (object.acceleration.cartesian_acceleration.z_acceleration_is_present) {
1002 }
1003 return acceleration;
1004}
1005
1018 if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject");
1019 return object.object_dimension_x.value.value;
1020}
1021
1034 if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject");
1035 return object.object_dimension_y.value.value;
1036}
1037
1050 if (!object.object_dimension_z_is_present) throw std::invalid_argument("No z-dimension present in PerceivedObject");
1051 return object.object_dimension_z.value.value;
1052}
1053
1064 gm::Vector3 dimensions;
1068 return dimensions;
1069}
1070
1082 const PerceivedObject &object) {
1083 gm::PointStamped utm_position;
1086
1087 utm_position.header.frame_id = reference_position.header.frame_id;
1088 utm_position.point.x = reference_position.point.x + relative_position.x;
1089 utm_position.point.y = reference_position.point.y + relative_position.y;
1090 utm_position.point.z = reference_position.point.z + relative_position.z;
1091
1092 return utm_position;
1093}
1094
1095}
Getter functions for the ETSI ITS Common Data Dictionary (CDD) v2.1.1.
double getAltitude(const Altitude &altitude)
Get the Altitude value.
uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object)
Retrieves the z-dimension of a perceived object.
uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container)
uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate)
Retrieves the confidence value from a CartesianCoordinateWithConfidence object.
uint8_t getCartesianAngleConfidence(const CartesianAngle &angle)
Get the confidence of the Cartesian angle.
uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object)
Retrieves the y-dimension of a perceived object.
double getVelocityComponent(const VelocityComponent &velocity)
Get the velocity component of the PerceivedObject.
WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm)
Retrieves the perceived object container from a CPM.
gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object)
Retrieves the dimensions of a perceived object.
gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object)
Get the pose of the PerceivedObject.
int16_t getYawRateOfPerceivedObject(const PerceivedObject &object)
Get the yaw rate of the PerceivedObject.
double getYawOfPerceivedObject(const PerceivedObject &object)
Get the yaw of the PerceivedObject.
double getLongitude(const Longitude &longitude)
Get the Longitude value.
uint32_t getStationID(const ItsPduHeader &header)
Get the StationID of ItsPduHeader.
gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object)
Calculates the UTM position of a perceived object based on the CPMs referencep position.
double getLatitude(const Latitude &latitude)
Get the Latitude value.
uint16_t getCartesianAngle(const CartesianAngle &angle)
Get the Cartesian angle of the PerceivedObject.
double getVelocityComponentConfidence(const VelocityComponent &velocity)
Get the confidence of the velocity component.
uint16_t getIdOfPerceivedObject(const PerceivedObject &object)
Retrieves the ID of a perceived object.
gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian velocity of the PerceivedObject.
uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object)
Gets the x-dimension of a perceived object.
std::vector< uint8_t > getCpmContainerIds(const CollectivePerceptionMessage &cpm)
Retrieves the container IDs from a CPM.
double getAccelerationComponentConfidence(const AccelerationComponent &acceleration)
Get the confidence of the acceleration component.
WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const uint8_t container_id)
PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i)
Retrieves the PerceivedObject at the specified index from the given WrappedCpmContainer.
gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object)
Get the Cartesian acceleration of the PerceivedObject.
double getAccelerationComponent(const AccelerationComponent &acceleration)
Get the acceleration component of the PerceivedObject.
gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object)
Calculates the orientation of a perceived object.
gm::PointStamped getUTMPosition(const T &reference_position, int &zone, bool &northp)
Get the UTM Position defined by the given ReferencePosition.
1.13.2