From 5c557b992925d7f710184a76b31616ff92293736 Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Thu, 19 Sep 2024 15:54:45 +0200 Subject: [PATCH] cleanup, formatting --- .../impl/cpm/cpm_ts_getters.h | 271 ++++++-- .../impl/cpm/cpm_ts_setters.h | 608 +++++++++++++----- .../test/impl/test_cpm_ts_access.cpp | 29 +- .../include/displays/CPM/cpm_display.hpp | 32 +- .../displays/CPM/cpm_render_object.hpp | 29 +- .../src/displays/CPM/cpm_display.cpp | 12 +- .../src/displays/CPM/cpm_render_object.cpp | 42 +- 7 files changed, 728 insertions(+), 295 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h index f360aa76..4592fbf2 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h @@ -43,15 +43,13 @@ namespace etsi_its_cpm_ts_msgs::access { * @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 uint32_t getStationID(const CollectivePerceptionMessage &cpm) { return getStationID(cpm.header); } /** * @brief Get the Reference Time object * * @param cpm CPM to get the ReferenceTime-Value from - * @return TimestampIts + * @return TimestampIts */ inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) { return cpm.payload.management_container.reference_time; @@ -61,7 +59,7 @@ inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) { * @brief Get the ReferenceTime-Value * * @param cpm CPM to get the ReferenceTime-Value from - * @return uint64_t the ReferenceTime-Value + * @return uint64_t the ReferenceTime-Value in milliseconds */ inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; } @@ -69,7 +67,7 @@ inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { * @brief Get the Latitude value of CPM * * @param cpm CPM to get the Latitude value from - * @return Latitude value in degree as decimal number + * @return Latitude value in 10^-7 degree */ inline double getLatitude(const CollectivePerceptionMessage &cpm) { return getLatitude(cpm.payload.management_container.reference_position.latitude); @@ -79,7 +77,7 @@ inline double getLatitude(const CollectivePerceptionMessage &cpm) { * @brief Get the Longitude value of CPM * * @param cpm CPM to get the Longitude value from - * @return Longitude value in degree as decimal number + * @return Longitude value in 10^-7 degree */ inline double getLongitude(const CollectivePerceptionMessage &cpm) { return getLongitude(cpm.payload.management_container.reference_position.longitude); @@ -89,7 +87,7 @@ inline double getLongitude(const CollectivePerceptionMessage &cpm) { * @brief Get the Altitude value of CPM * * @param cpm CPM to get the Altitude value from - * @return Altitude value (above the reference ellipsoid surface) in meter as decimal number + * @return Altitude value in centimeters */ inline double getAltitude(const CollectivePerceptionMessage &cpm) { return getAltitude(cpm.payload.management_container.reference_position.altitude); @@ -117,7 +115,7 @@ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, i * 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 + * @return geometry_msgs::PointStamped of the given position with the UTM position, int UTM zone and bool hemisphere */ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) { int zone; @@ -125,6 +123,12 @@ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) { return getUTMPosition(cpm, zone, northp); } +/** + * @brief Get the IDs of CPM-Containers in the CPM + * + * @param cpm CPM to get the number of CPM-Containers from + * @return uint8_t IDs of CPM-Containers + */ inline std::vector getCpmContainerIds(const CollectivePerceptionMessage &cpm) { std::vector container_ids; for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) { @@ -133,19 +137,38 @@ inline std::vector getCpmContainerIds(const CollectivePerceptionMessage 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){ +/** + * @brief Get the CPM-Container with the given ID + * + * @param cpm CPM to get the CPM-Container from + * @param container_id ID of the CPM-Container + * @return WrappedCpmContainer CPM-Container with the given ID + */ +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){ +/** + * @brief Get the Perceived Object Container from the CPM + * + * @param cpm CPM to get the CPM-Container from + * @return WrappedCpmContainer Perceived Object Container from the CPM + */ +inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm) { return getCpmContainer(cpm, CpmContainerId::PERCEIVED_OBJECT_CONTAINER); } +/** + * @brief Get the number of perceived objects in the PerceivedObjectContainer + * + * @param container PerceivedObjectContainer to get the number of perceived objects from + * @return uint8_t number of perceived objects + */ inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) { if (container.container_id.value != CpmContainerId::PERCEIVED_OBJECT_CONTAINER) { throw std::invalid_argument("Container is not a PerceivedObjectContainer"); @@ -154,12 +177,25 @@ inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) return number; } +/** + * @brief Get the number of perceived objects in the PerceivedObjectContainer + * + * @param cpm CPM to get the number of perceived objects from + * @return uint8_t number of perceived objects + */ inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) { return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm)); } // getters for the PerceivedObject +/** + * @brief Get the PerceivedObject with the given index + * + * @param container PerceivedObjectContainer to get the PerceivedObject from + * @param i index of the PerceivedObject + * @return PerceivedObject PerceivedObject with the given index + */ inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) { if (i >= getNumberOfPerceivedObjects(container)) { throw std::invalid_argument("Index out of range"); @@ -167,54 +203,95 @@ inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, return container.container_data.perceived_object_container.perceived_objects.array[i]; } -inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { - return object.object_id.value; -} +/** + * @brief Get the ID of PerceivedObject + * + * @param object PerceivedObject to get the ID from + * @return PerceivedObject ID + */ +inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { return object.object_id.value; } +/** + * @brief Get the CartesianCoordinate + * + * @param coordinate CartesianCoordinateWithConfidence to get the value from + * @return value of the CartesianCoordinate in cm + */ inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) { - return coordinate.value.value / 100; + return coordinate.value.value; } +/** + * @brief Get the confidence of the CartesianCoordinate + * + * @param coordinate CartesianCoordinateWithConfidence to get the confidence from + * @return value of the confidence of the CartesianCoordinate in cm + */ inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) { - return coordinate.confidence.value / 100; + return coordinate.confidence.value; } +/** + * @brief Get the position of the PerceivedObject + * + * @param object PerceivedObject to get the position from + * @return gm::Point position of the PerceivedObject in m + */ inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) { gm::Point point; - point.x = getCartesianCoordinate(object.position.x_coordinate); - point.y = getCartesianCoordinate(object.position.y_coordinate); + point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100; + point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100; if (object.position.z_coordinate_is_present) { - point.z = getCartesianCoordinate(object.position.z_coordinate); + point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100; } return point; } -inline uint16_t getCartesianAngle(const CartesianAngle &angle) { - return angle.value.value / 10; -} +/** + * @brief Get the CartesianAngle + * + * @param angle CartesianAngle to get the value from + * @return value of the CartesianAnglein 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 / 10; -} +/** + * @brief Get the confidence of the CartesianAngle + * + * @param angle CartesianAngle to get the confidence from + * @return value of the confidence of the CartesianAngle in 0,1 degrees + */ +inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { return angle.confidence.value; } +/** + * @brief Get the orientation of the PerceivedObject + * + * @param object PerceivedObject to get the orientation from + * @return gm::Quaternion orientation of the PerceivedObject in rad + */ 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 = ((static_cast(getCartesianAngle(object.angles.x_angle)) - 180) / 180) * M_PI; + roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10) - 180) / 180) * M_PI; } if (object.angles.y_angle_is_present) { - pitch = ((static_cast(getCartesianAngle(object.angles.y_angle)) - 180) / 180) * M_PI; + pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10) - 180) / 180) * M_PI; } - yaw = ((static_cast(getCartesianAngle(object.angles.z_angle)) - 180) / 180) * M_PI; + yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10) - 180) / 180) * M_PI; 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 rad + */ inline double getYawOfPerceivedObject(const PerceivedObject &object) { gm::Quaternion orientation = getOrientationOfPerceivedObject(object); tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); @@ -223,27 +300,54 @@ inline double getYawOfPerceivedObject(const PerceivedObject &object) { return yaw; } +/** + * @brief Get the pose of the PerceivedObject + * + * @param object PerceivedObject to get the pose from + * @return gm::Pose pose of the PerceivedObject + */ inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) { gm::Pose pose; pose.position = getPositionOfPerceivedObject(object); pose.orientation = getOrientationOfPerceivedObject(object); return pose; } -//inline gm::PoseWithCovariance getPoseWithCovarianceOfPerceivedObject(const PerceivedObject &object) {} +/** + * @brief Get the yaw rate of the PerceivedObject + * + * @param object PerceivedObject to get the yaw rate from + * @return int16_t yaw rate of the PerceivedObject in deg/s + */ inline int16_t 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; + return object.z_angular_velocity.value.value; } -inline int16_t getVelocityComponent(const VelocityComponent &velocity) { - return velocity.value.value / 100; -} +/** + * @brief Get the velocity component of the PerceivedObject + * + * @param velocity VelocityComponent to get the value from + * @return double value of the velocity component in m/s + */ +inline double getVelocityComponent(const VelocityComponent &velocity) { return double(velocity.value.value) / 100; } -inline uint8_t getVelocityComponentConfidence(const VelocityComponent &velocity) { - return velocity.confidence.value / 100; +/** + * @brief Get the confidence of the velocity component + * + * @param velocity VelocityComponent to get the confidence from + * @return double value of the confidence of the velocity component in m/s + */ +inline double getVelocityComponentConfidence(const VelocityComponent &velocity) { + return double(velocity.confidence.value) / 100; } +/** + * @brief Get the Cartesian velocity of the PerceivedObject + * + * @param object PerceivedObject to get the Cartesian velocity from + * @return gm::Vector3 Cartesian velocity of the PerceivedObject in m/s + */ 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) { @@ -258,19 +362,37 @@ inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject & return velocity; } -inline int16_t getAccelerationComponent(const AccelerationComponent &acceleration) { - return acceleration.value.value / 10; +/** + * @brief Get the acceleration component of the PerceivedObject + * + * @param acceleration AccelerationComponent to get the value from + * @return double value of the acceleration component in m/s^2 + */ +inline double getAccelerationComponent(const AccelerationComponent &acceleration) { + return double(acceleration.value.value) / 10; } -inline uint8_t getAccelerationComponentConfidence(const AccelerationComponent &acceleration) { - return acceleration.confidence.value / 10; +/** + * @brief Get the confidence of the acceleration component + * + * @param acceleration AccelerationComponent to get the confidence from + * @return double value of the confidence of the acceleration component in m/s^2 + */ +inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) { + return double(acceleration.confidence.value) / 10; } +/** + * @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 + */ 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"); - } + 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); @@ -280,30 +402,73 @@ inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObje return acceleration; } +/** + * @brief Get the x-dimensions of the PerceivedObject + * + * @param object PerceivedObject to get the dimensions from + * @return uint16_t x-dimensions of the PerceivedObject in dm + * @throws std::invalid_argument If the Y dimension is not present in the object. + */ 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 / 10; + return object.object_dimension_x.value.value; } +/** + * @brief Retrieves the Y dimension of a perceived object. + * + * This function extracts the Y dimension from a given PerceivedObject. + * If the Y dimension is not present in the object, it throws an + * std::invalid_argument exception. + * + * @param object The PerceivedObject from which to retrieve the Y dimension. + * @return uint16_t Y dimension of the perceived object in dm. + * @throws std::invalid_argument If the Y dimension is not present in the object. + */ 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 / 10; + return object.object_dimension_y.value.value; } +/** + * @brief Retrieves the Z dimension of a perceived object. + * + * This function extracts the Z dimension from a given PerceivedObject. + * If the Z dimension is not present in the object, it throws an + * std::invalid_argument exception. + * + * @param object The PerceivedObject from which to retrieve the Z dimension. + * @return uint16_t Z dimension of the perceived object in dm. + * @throws std::invalid_argument If the Z dimension is not present in the object. + */ 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 / 10; + return object.object_dimension_z.value.value; } +/** + * @brief Get the dimensions of the PerceivedObject + * + * @param object PerceivedObject to get the dimensions from + * @return gm::Vector3 dimensions of the PerceivedObject in m + */ inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) { gm::Vector3 dimensions; - dimensions.x = getXDimensionOfPerceivedObject(object); - dimensions.y = getYDimensionOfPerceivedObject(object); - dimensions.z = getZDimensionOfPerceivedObject(object); + dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10; + dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10; + dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10; return dimensions; } -inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object) { +/** + * @brief Get the position of the PerceivedObject in UTM + * + * @param cpm CPM to get the ReferencePosition from + * @param object PerceivedObject to get the position from + * @return gm::PointStamped position of the PerceivedObject in m + */ +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); diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h index bcd70b67..51003cea 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h @@ -37,32 +37,35 @@ namespace etsi_its_cpm_ts_msgs::access { #include - /** +/** * @brief Set the ItsPduHeader-object for a CPM * * @param cpm CPM-Message to set the ItsPduHeader * @param station_id * @param protocol_version */ - 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); - } +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); +} - /** +/** * @brief Set the ReferenceTime-value * * @param cpm CPM to set the ReferenceTime-Value for * @param unix_nanosecs Timestamp in unix-nanoseconds to set the ReferenceTime-Value from * @param n_leap_seconds Number of leap seconds since 2004 for the given timestamp (Default: etsi_its_msgs::N_LEAP_SECONDS) */ - inline void setReferenceTime(CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->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; - } +inline void setReferenceTime( + CollectivePerceptionMessage& cpm, const uint64_t unix_nanosecs, + const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->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; +} - /** +/** * @brief Set the ReferencePositionWithConfidence for a CPM TS * * This function sets the latitude, longitude, and altitude of the CPMs reference position. @@ -73,11 +76,12 @@ namespace etsi_its_cpm_ts_msgs::access { * @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); - } +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); +} - /** +/** * @brief Set the ReferencePosition of a CPM from a given UTM-Position * * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS @@ -89,200 +93,450 @@ namespace etsi_its_cpm_ts_msgs::access { * @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); - } +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); +} - inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id){ - object.object_id.value = id; - object.object_id_is_present = true; - } +/** + * @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; +} - 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 Set the measurement_delta_time of a PerceivedObject + * + * Sets the object_type of the PerceivedObject to the given type. + * + * @param object PerceivedObject to set the type for + * @param delta_time Delta time to set in milliseconds + */ +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; } +} - inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const int32_t value, const uint16_t confidence = CoordinateConfidence::UNAVAILABLE){ - // limit value range - int32_t limited_value = std::max(CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE, std::min(CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE, value)); - coordinate.value.value = limited_value; - - // limit confidence range - if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN){ - coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE; - } else { - coordinate.confidence.value = confidence; - } +/** + * @brief Set cartesian coordinate with confidence + * + * Sets the object_type of the PerceivedObject to the given type. + * + * @param coordinate CartesianCoordinateWithConfidence to set + * @param value Value to set in cm + * @param confidence Confidence to set cm (optional) + */ +inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const int32_t value, + const uint16_t confidence = CoordinateConfidence::UNAVAILABLE) { + // limit value range + int32_t limited_value = std::max(CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE, + std::min(CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE, value)); + coordinate.value.value = limited_value; + + // limit confidence range + if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) { + coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE; + } else { + coordinate.confidence.value = confidence; } +} - inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point, const uint16_t x_confidence = CoordinateConfidence::UNAVAILABLE, const uint16_t y_confidence = CoordinateConfidence::UNAVAILABLE, const uint16_t z_confidence = CoordinateConfidence::UNAVAILABLE){ - setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_confidence); - setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_confidence); - if (point.z != 0.0){ - setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_confidence); - object.position.z_coordinate_is_present = true; - } +/** + * @brief Set the position of the PerceivedObject + * + * Sets the position of the PerceivedObject to the given point. + * If the z-coordinate is not provided, it is set to 0. + * + * @param object PerceivedObject to set the position for + * @param point Point to set the position to in m + * @param x_confidence Confidence of the x-coordinate in m (optional) + * @param y_confidence Confidence of the y-coordinate in m (optional) + * @param z_confidence Confidence of the z-coordinate in m (optional) + */ +inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point, + const uint16_t x_confidence = CoordinateConfidence::UNAVAILABLE, + const uint16_t y_confidence = CoordinateConfidence::UNAVAILABLE, + const uint16_t z_confidence = CoordinateConfidence::UNAVAILABLE) { + setCartesianCoordinateWithConfidence(object.position.x_coordinate, point.x * 100, x_confidence * 100); + setCartesianCoordinateWithConfidence(object.position.y_coordinate, point.y * 100, y_confidence * 100); + if (point.z != 0.0) { + setCartesianCoordinateWithConfidence(object.position.z_coordinate, point.z * 100, z_confidence * 100); + object.position.z_coordinate_is_present = true; } +} - inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object, const gm::PointStamped& utm_position, const uint16_t x_confidence = CoordinateConfidence::UNAVAILABLE, const uint16_t y_confidence = CoordinateConfidence::UNAVAILABLE, const uint16_t z_confidence = CoordinateConfidence::UNAVAILABLE){ - 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 does not match the reference position frame_id"); - } - setCartesianCoordinateWithConfidence(object.position.x_coordinate, (utm_position.point.x - reference_position.point.x) * 100, x_confidence); - setCartesianCoordinateWithConfidence(object.position.y_coordinate, (utm_position.point.y - reference_position.point.y) * 100, y_confidence); - if (utm_position.point.z != 0.0){ - setCartesianCoordinateWithConfidence(object.position.z_coordinate, (utm_position.point.z - reference_position.point.z) * 100, z_confidence); - object.position.z_coordinate_is_present = true; - } +/** + * @brief Set the position of the PerceivedObject in UTM + * + * Sets the position of the PerceivedObject to the given UTM position. + * The UTM position is transformed to a relative position to the reference position of the CPM. + * + * @param cpm CPM to get the reference position from + * @param object PerceivedObject to set the position for + * @param utm_position UTM position to set the position to + * @param x_confidence Confidence of the x-coordinate in m (optional) + * @param y_confidence Confidence of the y-coordinate in m (optional) + * @param z_confidence Confidence of the z-coordinate in m (optional) + */ +inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object, + const gm::PointStamped& utm_position, + const uint16_t x_confidence = CoordinateConfidence::UNAVAILABLE, + const uint16_t y_confidence = CoordinateConfidence::UNAVAILABLE, + const uint16_t z_confidence = CoordinateConfidence::UNAVAILABLE) { + 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 does not match the reference position frame_id"); } - - inline void setVelocityComponent(VelocityComponent& velocity, const int16_t value, const uint8_t confidence = SpeedConfidence::UNAVAILABLE){ - // limit value range - int16_t limited_value = std::max(VelocityComponentValue::NEGATIVE_OUT_OF_RANGE, std::min(VelocityComponentValue::POSITIVE_OUT_OF_RANGE, value)); - velocity.value.value = limited_value; - - // limit confidence range - if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN){ - velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE; - } else { - velocity.confidence.value = confidence; - } + setCartesianCoordinateWithConfidence(object.position.x_coordinate, + (utm_position.point.x - reference_position.point.x) * 100, x_confidence); + setCartesianCoordinateWithConfidence(object.position.y_coordinate, + (utm_position.point.y - reference_position.point.y) * 100, y_confidence); + if (utm_position.point.z != 0.0) { + setCartesianCoordinateWithConfidence(object.position.z_coordinate, + (utm_position.point.z - reference_position.point.z) * 100, z_confidence); + object.position.z_coordinate_is_present = true; } +} - inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity, const uint8_t x_confidence = SpeedConfidence::UNAVAILABLE, const uint8_t y_confidence = SpeedConfidence::UNAVAILABLE, const uint8_t z_confidence = SpeedConfidence::UNAVAILABLE){ - object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY; - setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_confidence); - setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_confidence); - if (cartesian_velocity.z != 0.0){ - setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_confidence); - object.velocity.cartesian_velocity.z_velocity_is_present = true; - } - object.velocity_is_present = true; +/** + * @brief Set the orientation of the PerceivedObject + * + * Sets the orientation of the PerceivedObject to the given quaternion. + * If the roll and pitch angles are not provided, they are set to 0. + * + * @param velocity velocity (x,y,z) component to set + * @param value Value to set in cm/s + * @param confidence Confidence to set in cm/s (optional) + */ +inline void setVelocityComponent(VelocityComponent& velocity, const int16_t value, + const uint8_t confidence = SpeedConfidence::UNAVAILABLE) { + // limit value range + int16_t limited_value = std::max(VelocityComponentValue::NEGATIVE_OUT_OF_RANGE, + std::min(VelocityComponentValue::POSITIVE_OUT_OF_RANGE, value)); + velocity.value.value = limited_value; + + // limit confidence range + if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) { + velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE; + } else { + velocity.confidence.value = confidence; } +} - inline void setAccelerationComponent(AccelerationComponent& acceleration, const int16_t value, const uint8_t confidence = AccelerationConfidence::UNAVAILABLE){ - // limit value range - int16_t limited_value = std::max(AccelerationValue::NEGATIVE_OUT_OF_RANGE, std::min(AccelerationValue::POSITIVE_OUT_OF_RANGE, value)); - acceleration.value.value = limited_value; - - // limit confidence range - if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN){ - acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE; - } else { - acceleration.confidence.value = confidence; - } +/** + * @brief Set the velocity of the PerceivedObject + * + * Sets the velocity of the PerceivedObject to the given velocity. + * If the z-velocity is not provided, it is set to 0. + * + * @param object PerceivedObject to set the velocity for + * @param cartesian_velocity Velocity to set in m/s + * @param x_confidence Confidence of the x-velocity in m/s (optional) + * @param y_confidence Confidence of the y-velocity in m/s (optional) + * @param z_confidence Confidence of the z-velocity in m/s (optional) + */ +inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity, + const uint8_t x_confidence = SpeedConfidence::UNAVAILABLE, + const uint8_t y_confidence = SpeedConfidence::UNAVAILABLE, + const uint8_t z_confidence = SpeedConfidence::UNAVAILABLE) { + object.velocity.choice = Velocity3dWithConfidence::CHOICE_CARTESIAN_VELOCITY; + setVelocityComponent(object.velocity.cartesian_velocity.x_velocity, cartesian_velocity.x * 100, x_confidence * 100); + setVelocityComponent(object.velocity.cartesian_velocity.y_velocity, cartesian_velocity.y * 100, y_confidence * 100); + if (cartesian_velocity.z != 0.0) { + setVelocityComponent(object.velocity.cartesian_velocity.z_velocity, cartesian_velocity.z * 100, z_confidence * 100); + object.velocity.cartesian_velocity.z_velocity_is_present = true; } + object.velocity_is_present = true; +} - inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration, const uint8_t x_confidence = AccelerationConfidence::UNAVAILABLE, const uint8_t y_confidence = AccelerationConfidence::UNAVAILABLE, const uint8_t z_confidence = AccelerationConfidence::UNAVAILABLE){ - object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION; - setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10, x_confidence); - setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10, y_confidence); - if (cartesian_acceleration.z != 0.0){ - setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10, z_confidence); - object.acceleration.cartesian_acceleration.z_acceleration_is_present = true; - } - object.acceleration_is_present = true; +/** + * @brief Set the acceleration component of the PerceivedObject + * + * Sets the acceleration component of the PerceivedObject to the given value. + * + * @param acceleration AccelerationComponent to set the value for + * @param value Value to set in dm/s^2 + * @param confidence Confidence to set in dm/s^2 (optional) + */ +inline void setAccelerationComponent(AccelerationComponent& acceleration, const int16_t value, + const uint8_t confidence = AccelerationConfidence::UNAVAILABLE) { + // limit value range + int16_t limited_value = + std::max(AccelerationValue::NEGATIVE_OUT_OF_RANGE, std::min(AccelerationValue::POSITIVE_OUT_OF_RANGE, value)); + acceleration.value.value = limited_value; + + // limit confidence range + if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) { + acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE; + } else { + acceleration.confidence.value = confidence; } +} - inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw, const uint8_t confidence = AngleConfidence::UNAVAILABLE){ - // wrap angle to range [0, 360] - double yaw_in_degrees = yaw * 180 / M_PI + 180; - while (yaw_in_degrees > 360.0) yaw_in_degrees -= 360.0; - while (yaw_in_degrees < 0) yaw_in_degrees += 360.0; - object.angles.z_angle.value.value = yaw_in_degrees * 10; - - if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN){ - object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE; - } else { - object.angles.z_angle.confidence.value = confidence; - } - object.angles_is_present = true; +/** + * @brief Set the acceleration of the PerceivedObject + * + * Sets the acceleration of the PerceivedObject to the given acceleration. + * If the z-acceleration is not provided, it is set to 0. + * + * @param object PerceivedObject to set the acceleration for + * @param cartesian_acceleration Acceleration to set in m/s^2 + * @param x_confidence Confidence of the x-acceleration in m/s^2 (optional) + * @param y_confidence Confidence of the y-acceleration in m/s^2 (optional) + * @param z_confidence Confidence of the z-acceleration in m/s^2 (optional) + */ +inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration, + const uint8_t x_confidence = AccelerationConfidence::UNAVAILABLE, + const uint8_t y_confidence = AccelerationConfidence::UNAVAILABLE, + const uint8_t z_confidence = AccelerationConfidence::UNAVAILABLE) { + object.acceleration.choice = Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION; + setAccelerationComponent(object.acceleration.cartesian_acceleration.x_acceleration, cartesian_acceleration.x * 10, + x_confidence * 10); + setAccelerationComponent(object.acceleration.cartesian_acceleration.y_acceleration, cartesian_acceleration.y * 10, + y_confidence * 10); + if (cartesian_acceleration.z != 0.0) { + setAccelerationComponent(object.acceleration.cartesian_acceleration.z_acceleration, cartesian_acceleration.z * 10, + z_confidence * 10); + object.acceleration.cartesian_acceleration.z_acceleration_is_present = true; } + object.acceleration_is_present = true; +} - inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate, const uint8_t confidence = AngularSpeedConfidence::UNAVAILABLE){ - int16_t yaw_rate_in_degrees = yaw_rate * 180 / M_PI; - if (yaw_rate_in_degrees != CartesianAngularVelocityComponentValue::UNAVAILABLE) { - yaw_rate_in_degrees = std::max(CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE, std::min(CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE, yaw_rate_in_degrees)); - } - object.z_angular_velocity.value.value = yaw_rate_in_degrees; - object.z_angular_velocity.confidence.value = confidence; - object.z_angular_velocity_is_present = true; +/** + * @brief Set the yaw angle of the PerceivedObject + * + * Sets the yaw angle of the PerceivedObject to the given angle. + * + * @param object PerceivedObject to set the roll angle for + * @param roll Roll angle to set in rad + * @param confidence Confidence of the roll angle in 0,1 degrees (optional) + */ +inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw, + const uint8_t confidence = AngleConfidence::UNAVAILABLE) { + // wrap angle to range [0, 360] + double yaw_in_degrees = yaw * 180 / M_PI + 180; + while (yaw_in_degrees > 360.0) yaw_in_degrees -= 360.0; + while (yaw_in_degrees < 0) yaw_in_degrees += 360.0; + object.angles.z_angle.value.value = yaw_in_degrees * 10; + + if (confidence > AngleConfidence::MAX || confidence < AngleConfidence::MIN) { + object.angles.z_angle.confidence.value = AngleConfidence::OUT_OF_RANGE; + } else { + object.angles.z_angle.confidence.value = confidence; } + object.angles_is_present = true; +} - inline void setObjectDimension(ObjectDimension& dimension, const uint16_t value, const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE){ - // limit value range - if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX){ - dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE; - } else { - dimension.value.value = value; - } - - // limit confidence range - if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN){ - dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE; - } else { - dimension.confidence.value = confidence; - } +/** + * @brief Set the yaw rate of the PerceivedObject + * + * Sets the yaw rate of the PerceivedObject + * + * @param object PerceivedObject to set the orientation for + * @param yaw_rate Yaw rate to set in rad/s + * @param confidence Confidence of the yaw rate in degrees/s (optional) + */ +inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double yaw_rate, + const uint8_t confidence = AngularSpeedConfidence::UNAVAILABLE) { + int16_t yaw_rate_in_degrees = yaw_rate * 180 / M_PI; + if (yaw_rate_in_degrees != CartesianAngularVelocityComponentValue::UNAVAILABLE) { + yaw_rate_in_degrees = + std::max(CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE, + std::min(CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE, yaw_rate_in_degrees)); } + object.z_angular_velocity.value.value = yaw_rate_in_degrees; + object.z_angular_velocity.confidence.value = confidence; + object.z_angular_velocity_is_present = true; +} - inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const uint16_t value, const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE){ - setObjectDimension(object.object_dimension_x, value * 10, confidence); - object.object_dimension_x_is_present = true; +/** + * @brief Set the dimension value to a given dimension + * + * Sets the dimension of the PerceivedObject to the given dimensions. + * + * @param dimension ObjectDimension to set the value for + * @param value Value to set in cm + * @param confidence Confidence of the value in cm (optional) + */ +inline void setObjectDimension(ObjectDimension& dimension, const uint16_t value, + const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE) { + // limit value range + if (value < ObjectDimensionValue::MIN || value > ObjectDimensionValue::MAX) { + dimension.value.value = ObjectDimensionValue::OUT_OF_RANGE; + } else { + dimension.value.value = value; } - inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const uint16_t value, const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE){ - setObjectDimension(object.object_dimension_y, value * 10, confidence); - object.object_dimension_y_is_present = true; + // limit confidence range + if (confidence > ObjectDimensionConfidence::MAX || confidence < ObjectDimensionConfidence::MIN) { + dimension.confidence.value = ObjectDimensionConfidence::OUT_OF_RANGE; + } else { + dimension.confidence.value = confidence; } +} - inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const uint16_t value, const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE){ - setObjectDimension(object.object_dimension_z, value * 10, confidence); - object.object_dimension_z_is_present = true; - } +/** + * @brief Set the x-dimension of the PerceivedObject + * + * Sets the x-dimension of the PerceivedObject to the given value. + * + * @param object PerceivedObject to set the x-dimension for + * @param value Value to set in m + * @param confidence Confidence of the value in m (optional) + */ +inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double value, + const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE) { + setObjectDimension(object.object_dimension_x, (uint16_t)(value * 10), confidence * 10); + object.object_dimension_x_is_present = true; +} - inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions, const uint8_t x_confidence = ObjectDimensionConfidence::UNAVAILABLE, const uint8_t y_confidence = ObjectDimensionConfidence::UNAVAILABLE, const uint8_t z_confidence = ObjectDimensionConfidence::UNAVAILABLE){ - setXDimensionOfPerceivedObject(object, dimensions.x, x_confidence); - setYDimensionOfPerceivedObject(object, dimensions.y, y_confidence); - setZDimensionOfPerceivedObject(object, dimensions.z, z_confidence); - } +/** + * @brief Set the y-dimension of the PerceivedObject + * + * Sets the y-dimension of the PerceivedObject to the given value. + * + * @param object PerceivedObject to set the y-dimension for + * @param value Value to set in m + * @param confidence Confidence of the value in m (optional) + */ +inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double value, + const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE) { + setObjectDimension(object.object_dimension_y, (uint16_t)(value * 10), confidence * 10); + object.object_dimension_y_is_present = true; +} - inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0){ - setPositionOfPerceivedObject(object, point); - setMeasurementDeltaTimeOfPerceivedObject(object, delta_time); - } +/** + * @brief Set the z-dimension of the PerceivedObject + * + * Sets the z-dimension of the PerceivedObject to the given value. + * + * @param object PerceivedObject to set the z-dimension for + * @param value Value to set in m + * @param confidence Confidence of the value in m (optional) + */ +inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double value, + const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE) { + setObjectDimension(object.object_dimension_z, (uint16_t)(value * 10), confidence * 10); + object.object_dimension_z_is_present = true; +} - 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 Set the dimensions of the PerceivedObject + * + * Sets the dimensions of the PerceivedObject to the given dimensions. + * + * @param object PerceivedObject to set the dimensions for + * @param dimensions Dimensions to set in m + * @param x_confidence Confidence of the x-dimension in m (optional) + * @param y_confidence Confidence of the y-dimension in m (optional) + * @param z_confidence Confidence of the z-dimension in m (optional) + */ +inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions, + const uint8_t x_confidence = ObjectDimensionConfidence::UNAVAILABLE, + const uint8_t y_confidence = ObjectDimensionConfidence::UNAVAILABLE, + const uint8_t z_confidence = ObjectDimensionConfidence::UNAVAILABLE) { + setXDimensionOfPerceivedObject(object, dimensions.x, x_confidence); + setYDimensionOfPerceivedObject(object, dimensions.y, y_confidence); + setZDimensionOfPerceivedObject(object, dimensions.z, z_confidence); +} - inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const uint8_t n_objects = 0){ - container.container_id.value = CpmContainerId::PERCEIVED_OBJECT_CONTAINER; - container.container_data.choice = container.container_id; - container.container_data.perceived_object_container.number_of_perceived_objects.value = n_objects; - } +/** + * @brief initialize a PerceivedObject + * + * Initializes all necessary fields of a PerceivedObject. + * + * @param object PerceivedObject to be initialized + * @param point Point to set the position to in m + * @param delta_time Delta time to set in milliseconds + */ +inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) { + setPositionOfPerceivedObject(object, point); + setMeasurementDeltaTimeOfPerceivedObject(object, delta_time); +} - inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object){ - if (container.container_id.value == CpmContainerId::PERCEIVED_OBJECT_CONTAINER && container.container_data.choice.value == CpmContainerId::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"); - } +/** + * @brief initialize a PerceivedObject + * + * Initializes all necessary fields of a PerceivedObject. + * + * @param object PerceivedObject to be initialized + * @param point UTM Point to set the position to in m + * @param delta_time Delta time to set in milliseconds + */ +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 the Perceived Object Container within a WrappedCpmContainer. + * + * This function sets the container ID to 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. + * @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 = CpmContainerId::PERCEIVED_OBJECT_CONTAINER; + container.container_data.choice = container.container_id; + container.container_data.perceived_object_container.number_of_perceived_objects.value = n_objects; +} + +/** + * @brief Adds a PerceivedObject to the PerceivedObjectContainer within the given WrappedCpmContainer. + * + * This function checks if the provided container is a PerceivedObjectContainer. If it is, + * the function adds the given PerceivedObject to the container's perceived_objects array + * and updates the number_of_perceived_objects value. If the container is not a + * PerceivedObjectContainer, the function throws an std::invalid_argument exception. + * + * @param container The WrappedCpmContainer to which the PerceivedObject will be added. + * @param perceived_object The PerceivedObject to add to the container. + * + * @throws std::invalid_argument if the container is not a PerceivedObjectContainer. + */ +inline void addPerceivedObjectToContainer(WrappedCpmContainer& container, const PerceivedObject& perceived_object) { + if (container.container_id.value == CpmContainerId::PERCEIVED_OBJECT_CONTAINER && + container.container_data.choice.value == CpmContainerId::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"); - } +/** + * @brief Adds a container to the Collective Perception Message (CPM). + * + * This function adds a WrappedCpmContainer to the CPM's payload. It first checks + * if the current number of containers is less than the maximum allowed. If so, + * 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 maximum number of CPM-Containers is reached. + */ +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"); } +} -} // namespace etsi_its_cpm_ts_msgs::access +} // namespace etsi_its_cpm_ts_msgs::access diff --git a/etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp b/etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp index 9db66c15..b09bbfe2 100644 --- a/etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp @@ -60,5 +60,32 @@ TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { EXPECT_NEAR(longitude, cpm_ts_access::getLongitude(cpm), 1e-7); EXPECT_NEAR(altitude, cpm_ts_access::getAltitude(cpm), 1e-2); - // TODO: check object list access + etsi_its_cpm_ts_msgs::msg::PerceivedObject object; + gm::Vector3 dimensions; + dimensions.x = randomDouble(0.1, 25.6); + dimensions.y = randomDouble(0.1, 25.6); + dimensions.z = randomDouble(0.1, 25.6); + cpm_ts_access::setDimensionsOfPerceivedObject(object, dimensions); + EXPECT_NEAR(dimensions.x, cpm_ts_access::getDimensionsOfPerceivedObject(object).x, 1e-1); + EXPECT_NEAR(dimensions.y, cpm_ts_access::getDimensionsOfPerceivedObject(object).y, 1e-1); + EXPECT_NEAR(dimensions.z, cpm_ts_access::getDimensionsOfPerceivedObject(object).z, 1e-1); + + gm::Vector3 velocity; + velocity.x = randomDouble(-163.83, 163.83); + velocity.y = randomDouble(-163.83, 163.83); + velocity.z = randomDouble(-163.83, 163.83); + cpm_ts_access::setVelocityOfPerceivedObject(object, velocity); + EXPECT_NEAR(velocity.x, cpm_ts_access::getCartesianVelocityOfPerceivedObject(object).x, 1e-2); + EXPECT_NEAR(velocity.y, cpm_ts_access::getCartesianVelocityOfPerceivedObject(object).y, 1e-2); + EXPECT_NEAR(velocity.z, cpm_ts_access::getCartesianVelocityOfPerceivedObject(object).z, 1e-2); + + gm::Vector3 acceleration; + acceleration.x = randomDouble(-16.0, 16.0); + acceleration.y = randomDouble(-16.0, 16.0); + acceleration.z = randomDouble(-16.0, 16.0); + cpm_ts_access::setAccelerationOfPerceivedObject(object, acceleration); + EXPECT_NEAR(acceleration.x, cpm_ts_access::getCartesianAccelerationOfPerceivedObject(object).x, 1e-1); + EXPECT_NEAR(acceleration.y, cpm_ts_access::getCartesianAccelerationOfPerceivedObject(object).y, 1e-1); + EXPECT_NEAR(acceleration.z, cpm_ts_access::getCartesianAccelerationOfPerceivedObject(object).z, 1e-1); + } \ No newline at end of file diff --git a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp index f3be9220..479a39b5 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -34,35 +34,28 @@ SOFTWARE. #include -namespace Ogre -{ +namespace Ogre { class ManualObject; } -namespace rviz_common -{ -namespace properties -{ - class ColorProperty; - class FloatProperty; +namespace rviz_common { +namespace properties { +class ColorProperty; +class FloatProperty; } // namespace properties } // namespace rviz_common -namespace etsi_its_msgs -{ -namespace displays -{ +namespace etsi_its_msgs { +namespace displays { /** * @class CPMDisplay * @brief Displays an etsi_its_cpm_msgs::CollectivePerceptionMessage */ -class CPMDisplay : public - rviz_common::RosTopicDisplay -{ +class CPMDisplay : public rviz_common::RosTopicDisplay { Q_OBJECT -public: + public: CPMDisplay(); ~CPMDisplay() override; @@ -70,11 +63,11 @@ class CPMDisplay : public void reset() override; -protected: + protected: void processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) override; void update(float wall_dt, float ros_dt) override; - Ogre::ManualObject * manual_object_; + Ogre::ManualObject *manual_object_; rclcpp::Node::SharedPtr rviz_node_; @@ -88,9 +81,6 @@ class CPMDisplay : public // Rendering objects std::vector> cpm_render_objects_; - //vector of Poses - std::vector poses_; - std::vector> bboxs_; std::vector> texts_; }; diff --git a/etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp b/etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp index 34935e45..360b0146 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp @@ -45,9 +45,14 @@ namespace displays class CPMRenderObject { public: - // CPMRenderObject() { - // // Initialize members with default values if necessary - // } + /** + * @brief Construct a new CPMRenderObject object + * + * @param cpm + * @param receive_time + * @param n_leap_seconds + * @param number_of_object + */ CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second, uint8_t number_of_object=0); /** @@ -99,18 +104,20 @@ class CPMRenderObject */ geometry_msgs::msg::Vector3 getVelocity(); + /** + * @brief Get number of objects in CPM-object + * + * @return uint8_t + */ uint8_t getNumberOfObjects(); private: // member variables - std_msgs::msg::Header header; - uint8_t number_of_objects_; - uint32_t station_id; - int station_type; - geometry_msgs::msg::Pose pose; - geometry_msgs::msg::Vector3 dimensions; - geometry_msgs::msg::Vector3 velocity; - geometry_msgs::msg::Quaternion orientation; + std_msgs::msg::Header header_; + uint32_t station_id_; + geometry_msgs::msg::Pose pose_; + geometry_msgs::msg::Vector3 dimensions_; + geometry_msgs::msg::Vector3 velocity_; }; diff --git a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp index 621e9bc3..b32ab9c4 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -131,11 +131,6 @@ void CPMDisplay::update(float, float) { bboxs_.clear(); texts_.clear(); for (auto it = cpms_.begin(); it != cpms_.end(); ++it) { - //info logger for cpms size - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Size of cpms: %d", cpms_.size()); - - //logger for cpm_render_objects size - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Size of cpm_render_objects: %d", cpm_render_objects_.size()); for (const auto& cpm_ptr : cpm_render_objects_) { // Dereference the shared_ptr to access the CPMRenderObject CPMRenderObject& cpm = *cpm_ptr; @@ -153,8 +148,6 @@ void CPMDisplay::update(float, float) { std::string fixed_frame = fixed_frame_.toStdString(); geometry_msgs::msg::PoseStamped pose_origin; pose_origin.header = cpm.getHeader(); - //logger for the header frame id - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Header frame id: %s", cpm.getHeader().frame_id.c_str()); pose_origin.pose.position.x = 0; pose_origin.pose.position.y = 0; pose_origin.pose.position.z = 0; @@ -214,7 +207,10 @@ void CPMDisplay::update(float, float) { text += "\n"; } if (show_speed_->getBool()) { - // text+="Speed: " + std::to_string((int)(cpm.getSpeed()*3.6)) + " km/h"; + geometry_msgs::msg::Vector3 velocity = cpm.getVelocity(); + //get magnitude of velocity + double speed = sqrt(pow(velocity.x, 2) + pow(velocity.y, 2) + pow(velocity.z, 2)); + text += "Speed: " + std::to_string((int)(speed * 3.6)) + " km/h"; } if (!text.size()) return; std::shared_ptr text_render = diff --git a/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp b/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp index 59840ae3..a1807ced 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp @@ -33,45 +33,39 @@ CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerception etsi_its_cpm_ts_msgs::msg::PerceivedObject object = etsi_its_cpm_ts_msgs::access::getPerceivedObject( etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(cpm), number_of_object); - pose.position = etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject(object); - pose.orientation = etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject(object); - dimensions = etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject(object); - velocity = etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject(object); - //header.frame_id = position.header.frame_id; - header.frame_id = "map"; + geometry_msgs::msg::PointStamped utm_position = etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject(cpm, object); - uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_cpm_ts_msgs::access::getReferenceTime(cpm)); + pose_.position = utm_position.point; + pose_.orientation = etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject(object); + dimensions_ = etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject(object); + velocity_ = etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject(object); - header.stamp = rclcpp::Time(nanosecs); + header_.frame_id = utm_position.header.frame_id; + uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_cpm_ts_msgs::access::getReferenceTime(cpm)); + header_.stamp = rclcpp::Time(nanosecs); - uint32_t station_id = etsi_its_cpm_ts_msgs::access::getStationID(cpm); - //hardcoded station_id to 10 for testing - station_id = 10; + station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm); } bool CPMRenderObject::validateFloats() { bool valid = true; - valid = valid && rviz_common::validateFloats(pose); - valid = valid && rviz_common::validateFloats(dimensions); - valid = valid && rviz_common::validateFloats(velocity); + valid = valid && rviz_common::validateFloats(pose_); + valid = valid && rviz_common::validateFloats(dimensions_); + valid = valid && rviz_common::validateFloats(velocity_); return valid; } -double CPMRenderObject::getAge(rclcpp::Time now) { return (now - header.stamp).seconds(); } +double CPMRenderObject::getAge(rclcpp::Time now) { return (now - header_.stamp).seconds(); } -std_msgs::msg::Header CPMRenderObject::getHeader() { return header; } +std_msgs::msg::Header CPMRenderObject::getHeader() { return header_; } -uint32_t CPMRenderObject::getStationID() { return station_id; } +uint32_t CPMRenderObject::getStationID() { return station_id_; } -geometry_msgs::msg::Pose CPMRenderObject::getPose() { return pose; } - -uint8_t CPMRenderObject::getNumberOfObjects() { - return number_of_objects_; -} +geometry_msgs::msg::Pose CPMRenderObject::getPose() { return pose_; } -geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions() { return dimensions; } +geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions() { return dimensions_; } -geometry_msgs::msg::Vector3 CPMRenderObject::getVelocity() { return velocity; } +geometry_msgs::msg::Vector3 CPMRenderObject::getVelocity() { return velocity_; } } // namespace displays } // namespace etsi_its_msgs \ No newline at end of file