From 282aed161f651101e1003eccee464cf405944cf8 Mon Sep 17 00:00:00 2001 From: Lukas Zanger Date: Thu, 29 Aug 2024 15:26:16 +0000 Subject: [PATCH 01/52] add utils functions for CPM --- .../etsi_its_msgs_utils/cpm_ts_access.hpp | 44 ++++++++++++++++ .../etsi_its_msgs_utils/cpm_ts_acess.h | 43 ++++++++++++++++ .../impl/cpm/cpm_ts_access.h | 45 ++++++++++++++++ .../impl/cpm/cpm_ts_getters.h | 0 .../impl/cpm/cpm_ts_setters.h | 51 +++++++++++++++++++ .../impl/cpm/cpm_ts_utils.h | 0 6 files changed, 183 insertions(+) create mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp create mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h create mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h create mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h create mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h create mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp new file mode 100644 index 000000000..60b66dcd7 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp @@ -0,0 +1,44 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= +*/ + +/** + * @file cpm_ts_access.hpp + * @brief Main CPM access header to include in ROS 2 projects + */ + +#pragma once + +// Messages +#include +#include + +namespace etsi_its_cpm_ts_msgs { + using namespace msg; + namespace gm = geometry_msgs::msg; +} + +// Implementation +#include \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h new file mode 100644 index 000000000..00a9b6b80 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h @@ -0,0 +1,43 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= +*/ + +/** + * @file cpm_ts_access.h + * @brief Main CPM access header to include in ROS 1 projects + */ + +#pragma once + +// Messages +#include +#include + +namespace etsi_its_cpm_ts_msgs { + namespace gm = geometry_msgs; +} + +// Implementation +#include \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h new file mode 100644 index 000000000..156ef8ddf --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h @@ -0,0 +1,45 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= +*/ + +/** + * @file impl/cpm/cpm_access.h + * @brief Main CPM access implementation header + */ + +#pragma once + +#include +#include +#include + +#include + +#include +#include + +namespace etsi_its_cpm_ts_msgs::access { +#include +} // namespace etsi_its_cpm_ts_msgs::access \ No newline at end of file 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 new file mode 100644 index 000000000..e69de29bb 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 new file mode 100644 index 000000000..76cfd41f5 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h @@ -0,0 +1,51 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= +*/ + +/** + * @file impl/cpm/cpm_ts_setters.h + * @brief Setter functions for the ETSI ITS CPM (TS) + */ + +#pragma once + +#include + +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(CollectivePerceptionMessag& cpm, const uint32_t station_id, const uint8_t protocol_version = 0){ + setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version); + } + +} // namespace etsi_its_cpm_ts_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h new file mode 100644 index 000000000..e69de29bb From 8d0be1bb0eea2f5e17090cf499a5da667ea42320 Mon Sep 17 00:00:00 2001 From: Lukas Zanger Date: Fri, 30 Aug 2024 13:23:44 +0000 Subject: [PATCH 02/52] Fix namespaces of setter functions to match existing message types --- .../impl/cam/cam_setters_common.h | 85 +++++++++++++ .../impl/cam/cam_ts_setters.h | 60 ++++++++++ .../impl/cdd/cdd_setters_common.h | 113 ------------------ .../impl/cdd/cdd_v1-3-1_setters.h | 28 +++++ .../impl/cdd/cdd_v2-1-1_setters.h | 60 ---------- .../impl/cpm/cpm_ts_setters.h | 62 +++++++++- .../impl/denm/denm_setters.h | 20 ++++ 7 files changed, 254 insertions(+), 174 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h index 77e1953d0..bbc9a298d 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h @@ -109,6 +109,31 @@ inline void setHeading(CAM& cam, const double heading_val){ setHeading(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.heading, heading_val); } +/** + * @brief Set the VehicleLengthValue object + * + * @param vehicle_length object to set + * @param value VehicleLengthValue in meter as decimal number + */ +inline void setVehicleLengthValue(VehicleLengthValue& vehicle_length, const double value) { + int64_t length = (int64_t)std::round(value*1e1); + throwIfOutOfRange(length, VehicleLengthValue::MIN, VehicleLengthValue::MAX, "VehicleLengthValue"); + vehicle_length.value = length; +} + +/** + * @brief Set the VehicleLength object + * + * VehicleLengthConfidenceIndication is set to UNAVAILABLE + * + * @param vehicle_length object to set + * @param value VehicleLengthValue in meter as decimal number + */ +inline void setVehicleLength(VehicleLength& vehicle_length, const double value) { + vehicle_length.vehicle_length_confidence_indication.value = VehicleLengthConfidenceIndication::UNAVAILABLE; + setVehicleLengthValue(vehicle_length.vehicle_length_value, value); +} + /** * @brief Set the vehicle dimensions * @@ -190,4 +215,64 @@ inline void setExteriorLights(CAM& cam, const std::vector& exterior_lights else { throw std::invalid_argument("LowFrequencyContainer is not present!"); } +} + +/** + * @brief Set the Acceleration Control by a vector of bools + * + * @param acceleration_control + * @param bits + */ +inline void setAccelerationControl(AccelerationControl& acceleration_control, const std::vector& bits) { + setBitString(acceleration_control, bits); +} + +/** + * @brief Set the Driving Lane Status by a vector of bools + * + * @param driving_lane_status + * @param bits + */ +inline void setDrivingLaneStatus(DrivingLaneStatus& driving_lane_status, const std::vector& bits) { + setBitString(driving_lane_status, bits); +} + +/** + * @brief Set the Exterior Lights by a vector of bools + * + * @param exterior_lights + * @param bits + */ +inline void setExteriorLights(ExteriorLights& exterior_lights, const std::vector& bits) { + setBitString(exterior_lights, bits); +} + +/** + * @brief Set the Special Transport Type by a vector of bools + * + * @param special_transport_type + * @param bits + */ +inline void setSpecialTransportType(SpecialTransportType& special_transport_type, const std::vector& bits) { + setBitString(special_transport_type, bits); +} + +/** + * @brief Set the Lightbar Siren In Use by a vector of bools + * + * @param light_bar_siren_in_use + * @param bits + */ +inline void setLightBarSirenInUse(LightBarSirenInUse& light_bar_siren_in_use, const std::vector& bits) { + setBitString(light_bar_siren_in_use, bits); +} + +/** + * @brief Set the Emergency Priority by a vector of bools + * + * @param emergency_priority + * @param bits + */ +inline void setEmergencyPriority(EmergencyPriority& emergency_priority, const std::vector& bits) { + setBitString(emergency_priority, bits); } \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h index e3ed27a27..5b16a6fad 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h @@ -49,4 +49,64 @@ namespace etsi_its_cam_ts_msgs::access { setItsPduHeader(cam.header, MessageId::CAM, station_id, protocol_version); } + /** + * @brief Set the Reference Position object + * + * Altitude is set to UNAVAILABLE + * + * @param ref_position object to set + * @param latitude Latitude value in degree as decimal number + * @param longitude Longitude value in degree as decimal number + */ + inline void setReferencePosition(ReferencePositionWithConfidence& ref_position, const double latitude, const double longitude) + { + setLatitude(ref_position.latitude, latitude); + setLongitude(ref_position.longitude, longitude); + ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE; + ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE; + } + + /** + * @brief Set the Reference Position object + * + * @param ref_position object to set + * @param latitude Latitude value in degree as decimal number + * @param longitude Longitude value in degree as decimal number + * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number + */ + inline void setReferencePosition(ReferencePositionWithConfidence& ref_position, const double latitude, const double longitude, const double altitude) + { + setLatitude(ref_position.latitude, latitude); + setLongitude(ref_position.longitude, longitude); + setAltitude(ref_position.altitude, altitude); + } + +/** + * @brief Set the ReferencePosition from a given UTM-Position + * + * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS + * The z-Coordinate is directly used as altitude value + * The frame_id of the given utm_position must be set to 'utm_' + * + * @param[out] reference_position ReferencePosition to set + * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position + * @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(ReferencePositionWithConfidence& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) +{ + std::string required_frame_prefix = "utm_"; + if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) + { + throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); + } + double latitude, longitude; + try { + GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude); + } catch (GeographicLib::GeographicErr& e) { + throw std::invalid_argument(e.what()); + } + setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); +} + } // namespace etsi_its_cam_ts_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h index 1f957139c..4cee30f3f 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h @@ -102,59 +102,6 @@ inline void setAltitude(Altitude& altitude, const double value) { setAltitudeValue(altitude.altitude_value, value); } -/** - * @brief Set the HeadingValue object - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * - * @param heading object to set - * @param value Heading value in degree as decimal number - */ -inline void setHeadingValue(HeadingValue& heading, const double value) { - int64_t deg = (int64_t)std::round(value*1e1); - throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); - heading.value = deg; -} - -/** - * @brief Set the Heading object - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * HeadingConfidence is set to UNAVAILABLE - * - * @param heading object to set - * @param value Heading value in degree as decimal number - */ -inline void setHeading(Heading& heading, const double value) { - heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; - setHeadingValue(heading.heading_value, value); -} - -/** - * @brief Set the VehicleLengthValue object - * - * @param vehicle_length object to set - * @param value VehicleLengthValue in meter as decimal number - */ -inline void setVehicleLengthValue(VehicleLengthValue& vehicle_length, const double value) { - int64_t length = (int64_t)std::round(value*1e1); - throwIfOutOfRange(length, VehicleLengthValue::MIN, VehicleLengthValue::MAX, "VehicleLengthValue"); - vehicle_length.value = length; -} - -/** - * @brief Set the VehicleLength object - * - * VehicleLengthConfidenceIndication is set to UNAVAILABLE - * - * @param vehicle_length object to set - * @param value VehicleLengthValue in meter as decimal number - */ -inline void setVehicleLength(VehicleLength& vehicle_length, const double value) { - vehicle_length.vehicle_length_confidence_indication.value = VehicleLengthConfidenceIndication::UNAVAILABLE; - setVehicleLengthValue(vehicle_length.vehicle_length_value, value); -} - /** * @brief Set the VehicleWidth object * @@ -224,64 +171,4 @@ inline void setBitString(T& bitstring, const std::vector& bits) { bitstring.value[byte_idx] |= bits[bit_idx] << bit_idx_in_byte; } } -} - -/** - * @brief Set the Acceleration Control by a vector of bools - * - * @param acceleration_control - * @param bits - */ -inline void setAccelerationControl(AccelerationControl& acceleration_control, const std::vector& bits) { - setBitString(acceleration_control, bits); -} - -/** - * @brief Set the Driving Lane Status by a vector of bools - * - * @param driving_lane_status - * @param bits - */ -inline void setDrivingLaneStatus(DrivingLaneStatus& driving_lane_status, const std::vector& bits) { - setBitString(driving_lane_status, bits); -} - -/** - * @brief Set the Exterior Lights by a vector of bools - * - * @param exterior_lights - * @param bits - */ -inline void setExteriorLights(ExteriorLights& exterior_lights, const std::vector& bits) { - setBitString(exterior_lights, bits); -} - -/** - * @brief Set the Special Transport Type by a vector of bools - * - * @param special_transport_type - * @param bits - */ -inline void setSpecialTransportType(SpecialTransportType& special_transport_type, const std::vector& bits) { - setBitString(special_transport_type, bits); -} - -/** - * @brief Set the Lightbar Siren In Use by a vector of bools - * - * @param light_bar_siren_in_use - * @param bits - */ -inline void setLightBarSirenInUse(LightBarSirenInUse& light_bar_siren_in_use, const std::vector& bits) { - setBitString(light_bar_siren_in_use, bits); -} - -/** - * @brief Set the Emergency Priority by a vector of bools - * - * @param emergency_priority - * @param bits - */ -inline void setEmergencyPriority(EmergencyPriority& emergency_priority, const std::vector& bits) { - setBitString(emergency_priority, bits); } \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h index 6dfc54752..3cb932437 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h @@ -184,4 +184,32 @@ inline void setFromUTMPosition(ReferencePosition& reference_position, const gm:: throw std::invalid_argument(e.what()); } setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); +} + +/** + * @brief Set the HeadingValue object + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * + * @param heading object to set + * @param value Heading value in degree as decimal number + */ +inline void setHeadingValue(HeadingValue& heading, const double value) { + int64_t deg = (int64_t)std::round(value*1e1); + throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); + heading.value = deg; +} + +/** + * @brief Set the Heading object + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * HeadingConfidence is set to UNAVAILABLE + * + * @param heading object to set + * @param value Heading value in degree as decimal number + */ +inline void setHeading(etsi_its_cpm_ts_msgs::msg::Heading& heading, const double value) { + heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; + setHeadingValue(heading.heading_value, value); } \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h index 1b482ddc7..c9d3eab19 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h @@ -76,38 +76,6 @@ inline void setStationType(TrafficParticipantType& station_type, const uint8_t v station_type.value = value; } -/** - * @brief Set the Reference Position object - * - * Altitude is set to UNAVAILABLE - * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - */ -inline void setReferencePosition(ReferencePositionWithConfidence& ref_position, const double latitude, const double longitude) -{ - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE; - ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE; -} - -/** - * @brief Set the Reference Position object - * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number - */ -inline void setReferencePosition(ReferencePositionWithConfidence& ref_position, const double latitude, const double longitude, const double altitude) -{ - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - setAltitude(ref_position.altitude, altitude); -} - /** * @brief Set the LongitudinalAccelerationValue object * @@ -158,32 +126,4 @@ inline void setLateralAccelerationValue(AccelerationValue& accel, const double v inline void setLateralAcceleration(AccelerationComponent& accel, const double value) { accel.confidence.value = AccelerationConfidence::UNAVAILABLE; setLateralAccelerationValue(accel.value, value); -} - -/** - * @brief Set the ReferencePosition from a given UTM-Position - * - * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS - * The z-Coordinate is directly used as altitude value - * The frame_id of the given utm_position must be set to 'utm_' - * - * @param[out] reference_position ReferencePosition to set - * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position - * @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(ReferencePositionWithConfidence& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) -{ - std::string required_frame_prefix = "utm_"; - if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) - { - throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); - } - double latitude, longitude; - try { - GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude); - } catch (GeographicLib::GeographicErr& e) { - throw std::invalid_argument(e.what()); - } - setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); } \ No newline at end of file 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 76cfd41f5..4fdd930b8 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 @@ -44,8 +44,68 @@ namespace etsi_its_cpm_ts_msgs::access { * @param station_id * @param protocol_version */ - inline void setItsPduHeader(CollectivePerceptionMessag& cpm, const uint32_t station_id, const uint8_t protocol_version = 0){ + 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 Reference Position object + * + * Altitude is set to UNAVAILABLE + * + * @param ref_position object to set + * @param latitude Latitude value in degree as decimal number + * @param longitude Longitude value in degree as decimal number + */ + inline void setReferencePosition(ReferencePosition& ref_position, const double latitude, const double longitude) + { + setLatitude(ref_position.latitude, latitude); + setLongitude(ref_position.longitude, longitude); + ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE; + ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE; + } + + /** + * @brief Set the Reference Position object + * + * @param ref_position object to set + * @param latitude Latitude value in degree as decimal number + * @param longitude Longitude value in degree as decimal number + * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number + */ + inline void setReferencePosition(ReferencePosition& ref_position, const double latitude, const double longitude, const double altitude) + { + setLatitude(ref_position.latitude, latitude); + setLongitude(ref_position.longitude, longitude); + setAltitude(ref_position.altitude, altitude); + } + + /** + * @brief Set the ReferencePosition from a given UTM-Position + * + * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS + * The z-Coordinate is directly used as altitude value + * The frame_id of the given utm_position must be set to 'utm_' + * + * @param[out] reference_position ReferencePosition to set + * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position + * @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(ReferencePosition& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) + { + std::string required_frame_prefix = "utm_"; + if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) + { + throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); + } + double latitude, longitude; + try { + GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude); + } catch (GeographicLib::GeographicErr& e) { + throw std::invalid_argument(e.what()); + } + setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); + } + } // namespace etsi_its_cpm_ts_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h index 492b14d92..600114e6f 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h @@ -181,4 +181,24 @@ namespace etsi_its_denm_msgs::access { setFromUTMPosition(denm.denm.management.event_position, utm_position, zone, northp); } + /** + * @brief Set the Driving Lane Status by a vector of bools + * + * @param driving_lane_status + * @param bits + */ + inline void setDrivingLaneStatus(DrivingLaneStatus& driving_lane_status, const std::vector& bits) { + setBitString(driving_lane_status, bits); + } + + /** + * @brief Set the Lightbar Siren In Use by a vector of bools + * + * @param light_bar_siren_in_use + * @param bits + */ + inline void setLightBarSirenInUse(LightBarSirenInUse& light_bar_siren_in_use, const std::vector& bits) { + setBitString(light_bar_siren_in_use, bits); + } + } // namespace etsi_its_denm_msgs::access From f3766a2203cdde9e99ea91f62458b20b038ee9f0 Mon Sep 17 00:00:00 2001 From: Lukas Zanger Date: Fri, 30 Aug 2024 14:24:22 +0000 Subject: [PATCH 03/52] Fix setExteriorLights --- .../impl/cam/cam_setters_common.h | 20 +++++++++---------- .../impl/cdd/cdd_v1-3-1_setters.h | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h index bbc9a298d..4f5d92ef2 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h @@ -194,6 +194,16 @@ inline void setFromUTMPosition(CAM& cam, const gm::PointStamped& utm_position, c setFromUTMPosition(cam.cam.cam_parameters.basic_container.reference_position, utm_position, zone, northp); } +/** + * @brief Set the Exterior Lights by a vector of bools + * + * @param exterior_lights + * @param bits + */ +inline void setExteriorLights(ExteriorLights& exterior_lights, const std::vector& bits) { + setBitString(exterior_lights, bits); +} + /** * @brief Set the Exterior Lights by using a vector of bools * @@ -237,16 +247,6 @@ inline void setDrivingLaneStatus(DrivingLaneStatus& driving_lane_status, const s setBitString(driving_lane_status, bits); } -/** - * @brief Set the Exterior Lights by a vector of bools - * - * @param exterior_lights - * @param bits - */ -inline void setExteriorLights(ExteriorLights& exterior_lights, const std::vector& bits) { - setBitString(exterior_lights, bits); -} - /** * @brief Set the Special Transport Type by a vector of bools * diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h index 3cb932437..9799f98e7 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h @@ -209,7 +209,7 @@ inline void setHeadingValue(HeadingValue& heading, const double value) { * @param heading object to set * @param value Heading value in degree as decimal number */ -inline void setHeading(etsi_its_cpm_ts_msgs::msg::Heading& heading, const double value) { +inline void setHeading(Heading& heading, const double value) { heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; setHeadingValue(heading.heading_value, value); } \ No newline at end of file From 9767c99983e83fe8dae036e39f8f439a45b591e1 Mon Sep 17 00:00:00 2001 From: Lukas Zanger Date: Fri, 30 Aug 2024 14:35:59 +0000 Subject: [PATCH 04/52] First draft using include guards --- .../include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h | 8 ++++++-- .../etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h | 7 +++++-- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 3 +++ 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h index 16e371f77..fee0b9ea0 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h @@ -29,9 +29,13 @@ SOFTWARE. * @brief Sanity-check functions etc. for the ETSI ITS Common Data Dictionary (CDD) */ -#pragma once +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H + template void throwIfOutOfRange(const T1& val, const T2& min, const T2& max, const std::string val_desc) { if (val < min || val > max) throw std::invalid_argument(val_desc+" value is out of range ("+std::to_string(min)+"..."+std::to_string(max)+")!"); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h index 4cee30f3f..2cdf48ccc 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h @@ -29,7 +29,8 @@ SOFTWARE. * @brief Common setter functions for the ETSI ITS Common Data Dictionary (CDD) v1.3.1 and v2.1.1 */ -#pragma once +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H #include #include @@ -171,4 +172,6 @@ inline void setBitString(T& bitstring, const std::vector& bits) { bitstring.value[byte_idx] |= bits[bit_idx] << bit_idx_in_byte; } } -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H \ No newline at end of file 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 4fdd930b8..6c0fe736d 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 @@ -29,6 +29,9 @@ SOFTWARE. * @brief Setter functions for the ETSI ITS CPM (TS) */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H + #pragma once #include From d780b9606b8dbdcad32d656497fb96f0eef6b5b4 Mon Sep 17 00:00:00 2001 From: Lukas Zanger Date: Fri, 30 Aug 2024 14:48:41 +0000 Subject: [PATCH 05/52] Add include guards to CAM, CPM and DENM --- .../include/etsi_its_msgs_utils/impl/cam/cam_access.h | 8 ++++++++ .../etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h | 8 ++++++-- .../etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h | 8 ++++++-- .../etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h | 8 ++++++-- .../etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h | 8 ++++++-- .../etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h | 7 +++++-- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h | 8 ++++++++ .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 3 --- .../include/etsi_its_msgs_utils/impl/denm/denm_access.h | 8 ++++++++ 9 files changed, 53 insertions(+), 13 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h index 0ded5ced3..1ed70d7e9 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h @@ -29,6 +29,14 @@ SOFTWARE. * @brief Main CAM access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H + #pragma once #include diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h index 432b58f9a..a8a14686e 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h @@ -28,7 +28,9 @@ SOFTWARE. * @file impl/cdd/cdd_getters_common.h * @brief Common getter functions for the ETSI ITS Common Data Dictionary (CDD) v1.3.1 and v2.1.1 */ -#pragma once + +# ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H +# define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H #include @@ -206,4 +208,6 @@ inline std::vector getLightBarSirenInUse(const LightBarSirenInUse& light_b */ inline std::vector getEmergencyPriority(const EmergencyPriority& emergency_priority) { return getBitString(emergency_priority.value, emergency_priority.bits_unused); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h index 3c008b332..b3c349465 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h @@ -28,7 +28,9 @@ SOFTWARE. * @file impl/cdd/cdd_v1-3-1_getters.h * @brief Getter functions for the ETSI ITS Common Data Dictionary (CDD) v1.3.1 */ -#pragma once + +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H #include #include @@ -80,4 +82,6 @@ inline gm::PointStamped getUTMPosition(const ReferencePosition& reference_positi throw std::invalid_argument(e.what()); } return utm_point; -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h index 9799f98e7..c45e2dc6b 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h @@ -28,7 +28,9 @@ SOFTWARE. * @file impl/cdd/cdd_v1-3-1_setters.h * @brief Setter functions for the ETSI ITS Common Data Dictionary (CDD) v1.3.1 */ -#pragma once + +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H #include #include @@ -212,4 +214,6 @@ inline void setHeadingValue(HeadingValue& heading, const double value) { inline void setHeading(Heading& heading, const double value) { heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; setHeadingValue(heading.heading_value, value); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h index 174594fbd..cd6c72297 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h @@ -28,7 +28,9 @@ SOFTWARE. * @file impl/cdd/cdd_v2-1-1_getters.h * @brief Getter functions for the ETSI ITS Common Data Dictionary (CDD) v2.1.1 */ -#pragma once + +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H #include #include @@ -79,4 +81,6 @@ inline gm::PointStamped getUTMPosition(const ReferencePositionWithConfidence& re throw std::invalid_argument(e.what()); } return utm_point; -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h index c9d3eab19..b44bdbf77 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h @@ -29,7 +29,8 @@ SOFTWARE. * @brief Setter functions for the ETSI ITS Common Data Dictionary (CDD) v2.1.1 */ -#pragma once +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H #include #include @@ -126,4 +127,6 @@ inline void setLateralAccelerationValue(AccelerationValue& accel, const double v inline void setLateralAcceleration(AccelerationComponent& accel, const double value) { accel.confidence.value = AccelerationConfidence::UNAVAILABLE; setLateralAccelerationValue(accel.value, value); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h index 156ef8ddf..f55fac2a1 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h @@ -29,6 +29,14 @@ SOFTWARE. * @brief Main CPM access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H + #pragma once #include 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 6c0fe736d..4fdd930b8 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 @@ -29,9 +29,6 @@ SOFTWARE. * @brief Setter functions for the ETSI ITS CPM (TS) */ -#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H -#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H - #pragma once #include diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h index 3f66aab84..1d7778858 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h @@ -29,6 +29,14 @@ SOFTWARE. * @brief Main DENM access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H + #pragma once #include From 8be1f93b95e506d60cbfb26ef044fa2cee3454d9 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 08:13:21 +0000 Subject: [PATCH 06/52] add missing undefs --- .../include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h index 8f4e3a9a3..014ef0992 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h @@ -29,6 +29,14 @@ SOFTWARE. * @brief Main CAM TS access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H + #pragma once #include From d0fc01b5497f2de3743ff1326a90c6a4c34566ac Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 08:56:53 +0000 Subject: [PATCH 07/52] use template function for ReferencePosition --- .../impl/cam/cam_setters_common.h | 15 +++++ .../impl/cam/cam_ts_setters.h | 59 ------------------- .../impl/cdd/cdd_setters_common.h | 53 +++++++++++++++++ .../impl/cdd/cdd_v1-3-1_setters.h | 47 --------------- .../impl/cpm/cpm_ts_setters.h | 57 +++++------------- 5 files changed, 82 insertions(+), 149 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h index 4f5d92ef2..836c23840 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h @@ -177,6 +177,21 @@ inline void setLateralAcceleration(CAM& cam, const double lat_accel){ cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.lateral_acceleration_is_present = true; } +/** + * @brief Set the ReferencePosition for a CAM + * + * This function sets the latitude, longitude, and altitude of the CAMs reference position. + * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. + * + * @param cam CAM to set the ReferencePosition + * @param latitude The latitude value position in degree as decimal number. + * @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(CAM& cam, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE) { + setReferencePosition(cam.cam.cam_parameters.basic_container.reference_position, latitude, longitude); +} + /** * @brief Set the ReferencePosition of a CAM from a given UTM-Position * diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h index 5b16a6fad..84d4cd539 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h @@ -49,64 +49,5 @@ namespace etsi_its_cam_ts_msgs::access { setItsPduHeader(cam.header, MessageId::CAM, station_id, protocol_version); } - /** - * @brief Set the Reference Position object - * - * Altitude is set to UNAVAILABLE - * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - */ - inline void setReferencePosition(ReferencePositionWithConfidence& ref_position, const double latitude, const double longitude) - { - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE; - ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE; - } - - /** - * @brief Set the Reference Position object - * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number - */ - inline void setReferencePosition(ReferencePositionWithConfidence& ref_position, const double latitude, const double longitude, const double altitude) - { - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - setAltitude(ref_position.altitude, altitude); - } - -/** - * @brief Set the ReferencePosition from a given UTM-Position - * - * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS - * The z-Coordinate is directly used as altitude value - * The frame_id of the given utm_position must be set to 'utm_' - * - * @param[out] reference_position ReferencePosition to set - * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position - * @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(ReferencePositionWithConfidence& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) -{ - std::string required_frame_prefix = "utm_"; - if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) - { - throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); - } - double latitude, longitude; - try { - GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude); - } catch (GeographicLib::GeographicErr& e) { - throw std::invalid_argument(e.what()); - } - setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); -} } // namespace etsi_its_cam_ts_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h index 2cdf48ccc..6b83d81ff 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h @@ -140,6 +140,59 @@ inline void setSpeed(Speed& speed, const double value) { setSpeedValue(speed.speed_value, value); } +/** + * @brief Sets the reference position in the given ReferencePostion object. + * + * This function sets the latitude, longitude, and altitude of the reference position. + * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. + * + * @param ref_position ReferencePostion or ReferencePositionWithConfidence object to set the reference position in. + * @param latitude The latitude value position in degree as decimal number. + * @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). + */ +template +inline void setReferencePosition(T& ref_position, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE) { + setLatitude(ref_position.latitude, latitude); + setLongitude(ref_position.longitude, longitude); + if (altitude != AltitudeValue::UNAVAILABLE) { + setAltitude(ref_position.altitude, altitude); + } else { + ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE; + ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE; + } + // TODO: set confidence values +} + +/** + * @brief Set the ReferencePosition from a given UTM-Position + * + * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS + * The z-Coordinate is directly used as altitude value + * The frame_id of the given utm_position must be set to 'utm_' + * + * @param[out] reference_position ReferencePostion or ReferencePositionWithConfidence to set + * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position + * @param[in] zone the UTM zone (zero means UPS) of the given position + * @param[in] northp hemisphere (true means north, false means south) + */ +template +inline void setFromUTMPosition(T& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) +{ + std::string required_frame_prefix = "utm_"; + if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) + { + throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); + } + double latitude, longitude; + try { + GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude); + } catch (GeographicLib::GeographicErr& e) { + throw std::invalid_argument(e.what()); + } + setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); +} + /** * @brief Set a Bit String by a vector of bools * diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h index c45e2dc6b..4169a616e 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h @@ -76,38 +76,6 @@ inline void setStationType(StationType& station_type, const uint8_t value) { station_type.value = value; } -/** - * @brief Set the Reference Position object - * - * Altitude is set to UNAVAILABLE - * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - */ -inline void setReferencePosition(ReferencePosition& ref_position, const double latitude, const double longitude) -{ - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE; - ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE; -} - -/** - * @brief Set the Reference Position object - * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number - */ -inline void setReferencePosition(ReferencePosition& ref_position, const double latitude, const double longitude, const double altitude) -{ - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - setAltitude(ref_position.altitude, altitude); -} - /** * @brief Set the LongitudinalAccelerationValue object * @@ -172,21 +140,6 @@ inline void setLateralAcceleration(LateralAcceleration& accel, const double valu * @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(ReferencePosition& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) -{ - std::string required_frame_prefix = "utm_"; - if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) - { - throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); - } - double latitude, longitude; - try { - GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude); - } catch (GeographicLib::GeographicErr& e) { - throw std::invalid_argument(e.what()); - } - setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); -} /** * @brief Set the HeadingValue 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 4fdd930b8..931ccac35 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 @@ -48,64 +48,35 @@ namespace etsi_its_cpm_ts_msgs::access { setItsPduHeader(cpm.header, MessageId::CPM, station_id, protocol_version); } - /** - * @brief Set the Reference Position object - * - * Altitude is set to UNAVAILABLE + /** + * @brief Set the ReferencePositionWithConfidence for a CPM TS * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - */ - inline void setReferencePosition(ReferencePosition& ref_position, const double latitude, const double longitude) - { - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - ref_position.altitude.altitude_value.value = AltitudeValue::UNAVAILABLE; - ref_position.altitude.altitude_confidence.value = AltitudeConfidence::UNAVAILABLE; - } - - /** - * @brief Set the Reference Position object + * This function sets the latitude, longitude, and altitude of the CPMs reference position. + * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. * - * @param ref_position object to set - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number + * @param cpm CPM to set the ReferencePosition + * @param latitude The latitude value position in degree as decimal number. + * @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(ReferencePosition& ref_position, const double latitude, const double longitude, const double altitude) - { - setLatitude(ref_position.latitude, latitude); - setLongitude(ref_position.longitude, longitude); - setAltitude(ref_position.altitude, 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 from a given UTM-Position + * @brief Set the ReferencePosition of a CPM from a given UTM-Position * * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS * The z-Coordinate is directly used as altitude value * The frame_id of the given utm_position must be set to 'utm_' * - * @param[out] reference_position ReferencePosition to set + * @param[out] cpm CPM for which to set the ReferencePosition * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position * @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(ReferencePosition& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) - { - std::string required_frame_prefix = "utm_"; - if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) - { - throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); - } - double latitude, longitude; - try { - GeographicLib::UTMUPS::Reverse(zone, northp, utm_position.point.x, utm_position.point.y, latitude, longitude); - } catch (GeographicLib::GeographicErr& e) { - throw std::invalid_argument(e.what()); - } - setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); + 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); } } // namespace etsi_its_cpm_ts_msgs::access From 6a22b2f0b4cf6a83c2a2c448f05f29b701fb6c95 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 08:59:20 +0000 Subject: [PATCH 08/52] move heading setters explicit to CAM and DENM --- .../impl/cam/cam_setters_common.h | 33 ++++++++++--------- .../impl/cdd/cdd_v1-3-1_setters.h | 28 ---------------- .../impl/denm/denm_setters.h | 28 ++++++++++++++++ 3 files changed, 45 insertions(+), 44 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h index 836c23840..1bd983486 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h @@ -70,30 +70,31 @@ inline void setStationType(CAM& cam, const uint8_t value){ } /** - * @brief Set the ReferencePosition for a CAM + * @brief Set the HeadingValue object * - * Altitude is set to UNAVAILABLE + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West * - * @param cam CAM to set the ReferencePosition - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number + * @param heading object to set + * @param value Heading value in degree as decimal number */ -inline void setReferencePosition(CAM& cam, const double latitude, const double longitude) -{ - setReferencePosition(cam.cam.cam_parameters.basic_container.reference_position, latitude, longitude); +inline void setHeadingValue(HeadingValue& heading, const double value) { + int64_t deg = (int64_t)std::round(value*1e1); + throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); + heading.value = deg; } /** - * @brief Set the ReferencePosition for a CAM + * @brief Set the Heading object * - * @param cam CAM to set the ReferencePosition - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * HeadingConfidence is set to UNAVAILABLE + * + * @param heading object to set + * @param value Heading value in degree as decimal number */ -inline void setReferencePosition(CAM& cam, const double latitude, const double longitude, const double altitude) -{ - setReferencePosition(cam.cam.cam_parameters.basic_container.reference_position, latitude, longitude, altitude); +inline void setHeading(Heading& heading, const double value) { + heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; + setHeadingValue(heading.heading_value, value); } /** diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h index 4169a616e..10fce1816 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h @@ -141,32 +141,4 @@ inline void setLateralAcceleration(LateralAcceleration& accel, const double valu * @param[in] northp hemisphere (true means north, false means south) */ -/** - * @brief Set the HeadingValue object - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * - * @param heading object to set - * @param value Heading value in degree as decimal number - */ -inline void setHeadingValue(HeadingValue& heading, const double value) { - int64_t deg = (int64_t)std::round(value*1e1); - throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); - heading.value = deg; -} - -/** - * @brief Set the Heading object - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * HeadingConfidence is set to UNAVAILABLE - * - * @param heading object to set - * @param value Heading value in degree as decimal number - */ -inline void setHeading(Heading& heading, const double value) { - heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; - setHeadingValue(heading.heading_value, value); -} - #endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h index 600114e6f..a298c6af0 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h @@ -99,6 +99,34 @@ namespace etsi_its_denm_msgs::access { setReferencePosition(denm.denm.management.event_position, latitude, longitude, altitude); } + /** + * @brief Set the HeadingValue object + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * + * @param heading object to set + * @param value Heading value in degree as decimal number + */ + inline void setHeadingValue(HeadingValue& heading, const double value) { + int64_t deg = (int64_t)std::round(value*1e1); + throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); + heading.value = deg; + } + + /** + * @brief Set the Heading object + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * HeadingConfidence is set to UNAVAILABLE + * + * @param heading object to set + * @param value Heading value in degree as decimal number + */ + inline void setHeading(Heading& heading, const double value) { + heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; + setHeadingValue(heading.heading_value, value); + } + /** * @brief Set the IsHeadingPresent object for DENM * From 8fbef130b6e1b384a4d60ccb988c04e6a954f17a Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 09:02:00 +0000 Subject: [PATCH 09/52] move getters from cdd common explicit to CAM and DENM --- .../impl/cam/cam_getters_common.h | 82 +++++++++++++++ .../impl/cdd/cdd_getters_common.h | 99 +++++-------------- .../impl/cdd/cdd_v1-3-1_getters.h | 28 ------ .../impl/cdd/cdd_v2-1-1_getters.h | 28 ------ .../impl/denm/denm_getters.h | 32 ++++++ 5 files changed, 137 insertions(+), 132 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h index 0ffeb51ff..b323aed20 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h @@ -101,6 +101,18 @@ inline double getAltitude(const CAM& cam){ return getAltitude(cam.cam.cam_parameters.basic_container.reference_position.altitude); } +/** + * @brief Get the Heading value + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * + * @param heading to get the Heading value from + * @return Heading value in degree as decimal number + */ +inline double getHeading(const Heading& heading){ + return ((double)heading.heading_value.value)*1e-1; +} + /** * @brief Get the Heading value of CAM * @@ -113,6 +125,16 @@ inline double getHeading(const CAM& cam){ return getHeading(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.heading); } +/** + * @brief Get the Vehicle Length + * + * @param vehicleLength to get the vehicle length value from + * @return vehicle length value in meter as decimal number + */ +inline double getVehicleLength(const VehicleLength& vehicle_length){ + return ((double)vehicle_length.vehicle_length_value.value)*1e-1; +} + /** * @brief Get the Vehicle Length * @@ -185,6 +207,16 @@ inline gm::PointStamped getUTMPosition(const CAM& cam, int& zone, bool& northp){ return getUTMPosition(cam.cam.cam_parameters.basic_container.reference_position, zone, northp); } +/** + * @brief Get the Exterior Lights in form of bool vector + * + * @param exterior_lights + * @return std::vector + */ +inline std::vector getExteriorLights(const ExteriorLights& exterior_lights){ + return getBitString(exterior_lights.value, exterior_lights.bits_unused); +} + /** * @brief Get Exterior Lights as bool vector * @@ -203,4 +235,54 @@ inline std::vector getExteriorLights(const CAM& cam){ else { throw std::invalid_argument("LowFrequencyContainer is not present!"); } +} + +/** + * @brief Get Acceleration Control in form of bool vector + * + * @param acceleration_control + * @return std::vector + */ +inline std::vector getAccelerationControl(const AccelerationControl& acceleration_control){ + return getBitString(acceleration_control.value, acceleration_control.bits_unused); +} + +/** + * @brief Get the Driving Lane Status in form of bool vector + * + * @param driving_lane_status + * @return std::vector + */ +inline std::vector getDrivingLaneStatus(const DrivingLaneStatus& driving_lane_status){ + return getBitString(driving_lane_status.value, driving_lane_status.bits_unused); +} + +/** + * @brief Get the Special Transport Type in form of bool vector + * + * @param special_transport_type + * @return std::vector + */ +inline std::vector getSpecialTransportType(const SpecialTransportType& special_transport_type) { + return getBitString(special_transport_type.value, special_transport_type.bits_unused); +} + +/** + * @brief Get the Lightbar Siren In Use in form of bool vector + * + * @param light_bar_siren_in_use + * @return std::vector + */ +inline std::vector getLightBarSirenInUse(const LightBarSirenInUse& light_bar_siren_in_use) { + return getBitString(light_bar_siren_in_use.value, light_bar_siren_in_use.bits_unused); +} + +/** + * @brief Get the Vehicle Role in form of bool vector + * + * @param vehicle_role + * @return std::vector + */ +inline std::vector getEmergencyPriority(const EmergencyPriority& emergency_priority) { + return getBitString(emergency_priority.value, emergency_priority.bits_unused); } \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h index a8a14686e..c6c447d2f 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h @@ -75,28 +75,6 @@ inline double getAltitude(const Altitude& altitude){ return ((double)altitude.altitude_value.value)*1e-2; } -/** - * @brief Get the Heading value - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * - * @param heading to get the Heading value from - * @return Heading value in degree as decimal number - */ -inline double getHeading(const Heading& heading){ - return ((double)heading.heading_value.value)*1e-1; -} - -/** - * @brief Get the Vehicle Length - * - * @param vehicleLength to get the vehicle length value from - * @return vehicle length value in meter as decimal number - */ -inline double getVehicleLength(const VehicleLength& vehicle_length){ - return ((double)vehicle_length.vehicle_length_value.value)*1e-1; -} - /** * @brief Get the Vehicle Width * @@ -151,63 +129,32 @@ inline std::vector getBitString(const std::vector& buffer, const } /** - * @brief Get Acceleration Control in form of bool vector - * - * @param acceleration_control - * @return std::vector - */ -inline std::vector getAccelerationControl(const AccelerationControl& acceleration_control){ - return getBitString(acceleration_control.value, acceleration_control.bits_unused); -} - -/** - * @brief Get the Driving Lane Status in form of bool vector - * - * @param driving_lane_status - * @return std::vector - */ -inline std::vector getDrivingLaneStatus(const DrivingLaneStatus& driving_lane_status){ - return getBitString(driving_lane_status.value, driving_lane_status.bits_unused); -} - -/** - * @brief Get the Exterior Lights in form of bool vector + * @brief Get the UTM Position defined by the given ReferencePosition * - * @param exterior_lights - * @return std::vector - */ -inline std::vector getExteriorLights(const ExteriorLights& exterior_lights){ - return getBitString(exterior_lights.value, exterior_lights.bits_unused); -} - -/** - * @brief Get the Special Transport Type in form of bool vector + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate * - * @param special_transport_type - * @return std::vector + * @param[in] reference_position ReferencePosition or ReferencePositionWithConfidence to get the UTM Position from + * @param[out] zone the UTM zone (zero means UPS) + * @param[out] northp hemisphere (true means north, false means south) + * @return gm::PointStamped geometry_msgs::PointStamped of the given position */ -inline std::vector getSpecialTransportType(const SpecialTransportType& special_transport_type) { - return getBitString(special_transport_type.value, special_transport_type.bits_unused); -} - -/** - * @brief Get the Lightbar Siren In Use in form of bool vector - * - * @param light_bar_siren_in_use - * @return std::vector - */ -inline std::vector getLightBarSirenInUse(const LightBarSirenInUse& light_bar_siren_in_use) { - return getBitString(light_bar_siren_in_use.value, light_bar_siren_in_use.bits_unused); -} - -/** - * @brief Get the Vehicle Role in form of bool vector - * - * @param vehicle_role - * @return std::vector - */ -inline std::vector getEmergencyPriority(const EmergencyPriority& emergency_priority) { - return getBitString(emergency_priority.value, emergency_priority.bits_unused); +template +inline gm::PointStamped getUTMPosition(const T& reference_position, int& zone, bool& northp){ + gm::PointStamped utm_point; + double latitude = getLatitude(reference_position.latitude); + double longitude = getLongitude(reference_position.longitude); + utm_point.point.z = getAltitude(reference_position.altitude); + try { + GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y); + std::string hemisphere; + if(northp) hemisphere="N"; + else hemisphere="S"; + utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere; + } catch (GeographicLib::GeographicErr& e) { + throw std::invalid_argument(e.what()); + } + return utm_point; } #endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h index b3c349465..20a21bb7d 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h @@ -56,32 +56,4 @@ inline double getLateralAcceleration(const LateralAcceleration& lateral_accelera return ((double)lateral_acceleration.lateral_acceleration_value.value)*1e-1; } -/** - * @brief Get the UTM Position defined by the given ReferencePosition - * - * The position is transformed into UTM by using GeographicLib::UTMUPS - * The altitude value is directly used as z-Coordinate - * - * @param[in] reference_position ReferencePosition to get the UTM Position from - * @param[out] zone the UTM zone (zero means UPS) - * @param[out] northp hemisphere (true means north, false means south) - * @return gm::PointStamped geometry_msgs::PointStamped of the given position - */ -inline gm::PointStamped getUTMPosition(const ReferencePosition& reference_position, int& zone, bool& northp){ - gm::PointStamped utm_point; - double latitude = getLatitude(reference_position.latitude); - double longitude = getLongitude(reference_position.longitude); - utm_point.point.z = getAltitude(reference_position.altitude); - try { - GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y); - std::string hemisphere; - if(northp) hemisphere="N"; - else hemisphere="S"; - utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere; - } catch (GeographicLib::GeographicErr& e) { - throw std::invalid_argument(e.what()); - } - return utm_point; -} - #endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h index cd6c72297..027f153e8 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h @@ -55,32 +55,4 @@ inline double getLateralAcceleration(const AccelerationComponent& lateral_accele return ((int16_t)lateral_acceleration.value.value)*1e-1; } -/** - * @brief Get the UTM Position defined by the given ReferencePosition - * - * The position is transformed into UTM by using GeographicLib::UTMUPS - * The altitude value is directly used as z-Coordinate - * - * @param[in] reference_position ReferencePosition to get the UTM Position from - * @param[out] zone the UTM zone (zero means UPS) - * @param[out] northp hemisphere (true means north, false means south) - * @return gm::PointStamped geometry_msgs::PointStamped of the given position - */ -inline gm::PointStamped getUTMPosition(const ReferencePositionWithConfidence& reference_position, int& zone, bool& northp){ - gm::PointStamped utm_point; - double latitude = getLatitude(reference_position.latitude); - double longitude = getLongitude(reference_position.longitude); - utm_point.point.z = getAltitude(reference_position.altitude); - try { - GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y); - std::string hemisphere; - if(northp) hemisphere="N"; - else hemisphere="S"; - utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere; - } catch (GeographicLib::GeographicErr& e) { - throw std::invalid_argument(e.what()); - } - return utm_point; -} - #endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h index 772b6a7ad..50de03ff1 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h @@ -105,6 +105,18 @@ namespace etsi_its_denm_msgs::access { return getAltitude(denm.denm.management.event_position.altitude); } + /** + * @brief Get the Heading value + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * + * @param heading to get the Heading value from + * @return Heading value in degree as decimal number + */ + inline double getHeading(const Heading& heading){ + return ((double)heading.heading_value.value)*1e-1; + } + /** * @brief Get the Heading object * @@ -415,4 +427,24 @@ namespace etsi_its_denm_msgs::access { } } + /** + * @brief Get the Driving Lane Status in form of bool vector + * + * @param driving_lane_status + * @return std::vector + */ + inline std::vector getDrivingLaneStatus(const DrivingLaneStatus& driving_lane_status){ + return getBitString(driving_lane_status.value, driving_lane_status.bits_unused); + } + + /** + * @brief Get the Lightbar Siren In Use in form of bool vector + * + * @param light_bar_siren_in_use + * @return std::vector + */ + inline std::vector getLightBarSirenInUse(const LightBarSirenInUse& light_bar_siren_in_use) { + return getBitString(light_bar_siren_in_use.value, light_bar_siren_in_use.bits_unused); + } + } // namespace etsi_its_denm_msgs::access From 40656abe5bf520f1dd9bde665387fb1d1943fee5 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 09:02:34 +0000 Subject: [PATCH 10/52] implement first cpm getters --- .../impl/cpm/cpm_ts_getters.h | 65 +++++++++++++++++++ 1 file changed, 65 insertions(+) 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 e69de29bb..f9a7fda79 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 @@ -0,0 +1,65 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= +*/ + +/** + * @file impl/cpm/cpm_ts_getters.h + * @brief Getter functions for the ETSI ITS CPM (TS) + */ + +#pragma once + +namespace etsi_its_cpm_ts_msgs::access { + +#include + + /** + * @brief Retrieves the station ID from the given CPM. + * + * This function extracts the station ID from the header of the provided CPM. + * + * @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); + } + +/** + * @brief Get the UTM Position defined within the BasicContainer of the CPM + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param[in] cpm CPM to get the UTM Position from + * @param[out] zone the UTM zone (zero means UPS) + * @param[out] northp hemisphere (true means north, false means south) + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ +inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage& cpm, int& zone, bool& northp){ + return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); +} + +} // namespace etsi_its_cpm_ts_msgs::access From 22baf89c5cdc0dde2b6b6d6565958b4d068a63b5 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 09:23:28 +0000 Subject: [PATCH 11/52] implement access functions for CPM reference time --- .../impl/cpm/cpm_ts_getters.h | 48 +++++++++++++------ .../impl/cpm/cpm_ts_setters.h | 16 ++++++- .../impl/cpm/cpm_ts_utils.h | 47 ++++++++++++++++++ 3 files changed, 96 insertions(+), 15 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 f9a7fda79..330de8946 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 @@ -47,19 +47,39 @@ namespace etsi_its_cpm_ts_msgs::access { return getStationID(cpm.header); } -/** - * @brief Get the UTM Position defined within the BasicContainer of the CPM - * - * The position is transformed into UTM by using GeographicLib::UTMUPS - * The altitude value is directly used as z-Coordinate - * - * @param[in] cpm CPM to get the UTM Position from - * @param[out] zone the UTM zone (zero means UPS) - * @param[out] northp hemisphere (true means north, false means south) - * @return gm::PointStamped geometry_msgs::PointStamped of the given position - */ -inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage& cpm, int& zone, bool& northp){ - return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); -} + /** + * @brief Get the Reference Time object + * + * @param cpm CPM to get the ReferenceTime-Value from + * @return TimestampIts + */ + inline TimestampIts getReferenceTime(const CollectivePerceptionMessage& cpm){ + return cpm.payload.management_container.reference_time; + } + + /** + * @brief Get the ReferenceTime-Value + * + * @param cpm CPM to get the ReferenceTime-Value from + * @return uint64_t the ReferenceTime-Value + */ + inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage& cpm){ + return getReferenceTime(cpm).value; + } + + /** + * @brief Get the UTM Position defined within the BasicContainer of the CPM + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param[in] cpm CPM to get the UTM Position from + * @param[out] zone the UTM zone (zero means UPS) + * @param[out] northp hemisphere (true means north, false means south) + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ + inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage& cpm, int& zone, bool& northp){ + return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); + } } // namespace etsi_its_cpm_ts_msgs::access 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 931ccac35..0dfb122fd 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 @@ -48,7 +48,21 @@ namespace etsi_its_cpm_ts_msgs::access { 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; + } + + /** * @brief Set the ReferencePositionWithConfidence for a CPM TS * * This function sets the latitude, longitude, and altitude of the CPMs reference position. diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h index e69de29bb..7b6567d66 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h @@ -0,0 +1,47 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= +*/ + +/** + * @file impl/cpm/cpm_ts_utils.h + * @brief Utility functions for the ETSI ITS CPM (TS) + */ + +#include +#include + +#pragma once + +/** + * @brief Get the Unix-Nanoseconds from a given ReferenceTime object + * + * @param reference_time the ReferenceTime object to get the Unix-Nanoseconds from + * @param n_leap_seconds number of leap-seconds since 2004. (Default: etsi_its_msgs::N_LEAP_SECONDS) + * @return uint64_t the corresponding Unix-Nanoseconds + */ +inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) +{ + return reference_time.value*1e6+etsi_its_msgs::UNIX_SECONDS_2004*1e9-n_leap_seconds*1e9; +} \ No newline at end of file From 57094cfcfc88b3aba6d623a785fc022059cc9272 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 09:41:35 +0000 Subject: [PATCH 12/52] add more cpm getters --- .../impl/cpm/cpm_ts_getters.h | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) 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 330de8946..4e28cfab5 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 @@ -67,6 +67,36 @@ namespace etsi_its_cpm_ts_msgs::access { return getReferenceTime(cpm).value; } + /** + * @brief Get the Latitude value of CPM + * + * @param cpm CPM to get the Latitude value from + * @return Latitude value in degree as decimal number + */ + inline double getLatitude(const CollectivePerceptionMessage& cpm){ + return getLatitude(cpm.payload.management_container.reference_position.latitude); + } + + /** + * @brief Get the Longitude value of CPM + * + * @param cpm CPM to get the Longitude value from + * @return Longitude value in degree as decimal number + */ + inline double getLongitude(const CollectivePerceptionMessage& cpm){ + return getLongitude(cpm.payload.management_container.reference_position.longitude); + } + + /** + * @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 + */ + inline double getAltitude(const CollectivePerceptionMessage& cpm){ + return getAltitude(cpm.payload.management_container.reference_position.altitude); + } + /** * @brief Get the UTM Position defined within the BasicContainer of the CPM * From ec4b34ce47a6019ba5c2bd900f13b6a0de2c65a7 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 09:41:58 +0000 Subject: [PATCH 13/52] implement tests for CPM access --- etsi_its_msgs_utils/CMakeLists.txt | 12 ++- .../test/impl/test_cpm_ts_access.cpp | 85 +++++++++++++++++++ .../test/test_cpm_ts_access.ros2.cpp | 10 +++ 3 files changed, 106 insertions(+), 1 deletion(-) create mode 100644 etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp create mode 100644 etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp diff --git a/etsi_its_msgs_utils/CMakeLists.txt b/etsi_its_msgs_utils/CMakeLists.txt index 7fe623f33..ddc25e9f6 100644 --- a/etsi_its_msgs_utils/CMakeLists.txt +++ b/etsi_its_msgs_utils/CMakeLists.txt @@ -55,7 +55,6 @@ if(${ROS_VERSION} EQUAL 2) GeographicLib geometry_msgs ) - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}-cam_ts_test test/test_cam_ts_access.ros2.cpp) target_include_directories(${PROJECT_NAME}-cam_ts_test PUBLIC $ @@ -67,6 +66,17 @@ if(${ROS_VERSION} EQUAL 2) GeographicLib geometry_msgs ) + ament_add_gtest(${PROJECT_NAME}-cpm_ts_test test/test_cpm_ts_access.ros2.cpp) + target_include_directories(${PROJECT_NAME}-cpm_ts_test PUBLIC + $ + $ + ) + target_include_directories(${PROJECT_NAME}-cpm_ts_test PUBLIC test) + ament_target_dependencies(${PROJECT_NAME}-cpm_ts_test + etsi_its_msgs + GeographicLib + geometry_msgs + ) ament_add_gtest(${PROJECT_NAME}-denm_test test/test_denm_access.ros2.cpp) target_include_directories(${PROJECT_NAME}-denm_test PUBLIC $ 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 new file mode 100644 index 000000000..e0cdf45d4 --- /dev/null +++ b/etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp @@ -0,0 +1,85 @@ +#include +#include +#include +#include +#include + +#include + +std::default_random_engine random_engine; + + +double randomDouble(double min, double max) { + std::uniform_real_distribution uniform_distribution_double(min, max); + return uniform_distribution_double(random_engine); +} + +int randomInt(int min, int max) { + std::uniform_int_distribution uniform_distribution_int(min, max); + return uniform_distribution_int(random_engine); +} + +TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { + + CollectivePerceptionMessage cpm; + + int station_id = randomInt(StationId::MIN,StationId::MAX); + int protocol_version = randomInt(OrdinalNumber1B::MIN,OrdinalNumber1B::MAX); + setItsPduHeader(cpm, station_id, protocol_version); + EXPECT_EQ(MessageId::CPM, cpm.header.message_id.value); + EXPECT_EQ(protocol_version, cpm.header.protocol_version.value); + EXPECT_EQ(station_id, getStationID(cpm)); + + // https://www.etsi.org/deliver/etsi_ts/102800_102899/10289402/01.02.01_60/ts_10289402v010201p.pdf + // DE_TimestampITS + // The value for TimestampIts for 2007-01-01T00:00:00.000Z is + // 94694401000 milliseconds, which includes one leap second insertion + // since 2004-01-01T00:00:00.000Z. + + uint64_t t_2007 = ((uint64_t)1167609600)*1e9; + TimestampIts t_its; + EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + etsi_its_cpm_ts_msgs::access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(94694401000, t_its.value); + setReferenceTime(cpm, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(94694401000, getReferenceTimeValue(cpm)); + EXPECT_EQ(t_2007, getUnixNanosecondsFromReferenceTime(getReferenceTime(cpm), 1)); + + double latitude = randomDouble(-90.0, 90.0); + double longitude = randomDouble(-180.0, 180.0); + setReferencePosition(cpm, latitude, longitude); + EXPECT_NEAR(latitude, getLatitude(cpm), 1e-7); + EXPECT_NEAR(longitude, getLongitude(cpm), 1e-7); + latitude = randomDouble(-90.0, 90.0); + longitude = randomDouble(-180.0, 180.0); + double altitude = randomDouble(-1000.0, 8000.0); + setReferencePosition(cpm, latitude, longitude, altitude); + EXPECT_NEAR(latitude, getLatitude(cpm), 1e-7); + EXPECT_NEAR(longitude, getLongitude(cpm), 1e-7); + EXPECT_NEAR(altitude, getAltitude(cpm), 1e-2); + + // Set specific position to test utm projection + latitude = 50.787467; + longitude = 6.046498; + altitude = 209.0; + setReferencePosition(cpm, latitude, longitude, altitude); + int zone; + bool northp; + gm::PointStamped utm = getUTMPosition(cpm, zone, northp); + EXPECT_NEAR(291827.02, utm.point.x, 1e-1); + EXPECT_NEAR(5630349.72, utm.point.y, 1e-1); + EXPECT_EQ(altitude, utm.point.z); + EXPECT_EQ(32, zone); + EXPECT_EQ(true, northp); + setFromUTMPosition(cpm, utm, zone, northp); + EXPECT_NEAR(latitude, getLatitude(cpm), 1e-7); + EXPECT_NEAR(longitude, getLongitude(cpm), 1e-7); + EXPECT_NEAR(altitude, getAltitude(cpm), 1e-2); + + // TODO: check object list access +} + +int main(int argc, char *argv[]) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp b/etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp new file mode 100644 index 000000000..ce54047d4 --- /dev/null +++ b/etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp @@ -0,0 +1,10 @@ + +#include + +#include + +using namespace etsi_its_cpm_ts_msgs::msg; +using namespace etsi_its_cpm_ts_msgs::access; +namespace gm = geometry_msgs::msg; + +#include From 8d23b97f7a05c8ce6384a78f51375cc8a14e8447 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 09:59:15 +0000 Subject: [PATCH 14/52] fix bug in altitute for reference position --- .../impl/cam/cam_setters_common.h | 2 +- .../impl/denm/denm_setters.h | 26 +++++-------------- 2 files changed, 8 insertions(+), 20 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h index 1bd983486..06815a3c3 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h @@ -190,7 +190,7 @@ inline void setLateralAcceleration(CAM& cam, const double lat_accel){ * @param altitude The altitude value (above the reference ellipsoid surface) in meter as decimal number (optional). */ inline void setReferencePosition(CAM& cam, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE) { - setReferencePosition(cam.cam.cam_parameters.basic_container.reference_position, latitude, longitude); + setReferencePosition(cam.cam.cam_parameters.basic_container.reference_position, latitude, longitude, altitude); } /** diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h index a298c6af0..313c9122e 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h @@ -73,29 +73,17 @@ namespace etsi_its_denm_msgs::access { } /** - * @brief Set the ReferencePosition for a DENM + * @brief Set the ReferencePositionWithConfidence for a DENM * - * Altitude is set to UNAVAILABLE + * This function sets the latitude, longitude, and altitude of the DENMs reference position. + * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. * * @param denm DENM to set the ReferencePosition - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number + * @param latitude The latitude value position in degree as decimal number. + * @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(DENM& denm, const double latitude, const double longitude) - { - setReferencePosition(denm.denm.management.event_position, latitude, longitude); - } - - /** - * @brief Set the ReferencePosition for a DENM - * - * @param denm DENM to set the ReferencePosition - * @param latitude Latitude value in degree as decimal number - * @param longitude Longitude value in degree as decimal number - * @param altitude Altitude value (above the reference ellipsoid surface) in meter as decimal number - */ - inline void setReferencePosition(DENM& denm, const double latitude, const double longitude, const double altitude) - { + inline void setReferencePosition(DENM& denm, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE) { setReferencePosition(denm.denm.management.event_position, latitude, longitude, altitude); } From 18f8443670e48ecc3ec0348db7c1ed675978013e Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 10:53:18 +0000 Subject: [PATCH 15/52] add include guard for common CAM files --- .../include/etsi_its_msgs_utils/impl/cam/cam_access.h | 3 +++ .../etsi_its_msgs_utils/impl/cam/cam_getters_common.h | 7 +++++-- .../etsi_its_msgs_utils/impl/cam/cam_setters_common.h | 7 +++++-- .../include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h | 3 +++ .../include/etsi_its_msgs_utils/impl/cam/cam_utils.h | 7 +++++-- 5 files changed, 21 insertions(+), 6 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h index 1ed70d7e9..194972d8d 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h @@ -29,6 +29,9 @@ SOFTWARE. * @brief Main CAM access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h index b323aed20..073b51523 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h @@ -29,7 +29,8 @@ SOFTWARE. * @brief Common getter functions for the ETSI ITS CAM (EN and TS) */ -#pragma once +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H /** * @brief Get the Station ID object @@ -285,4 +286,6 @@ inline std::vector getLightBarSirenInUse(const LightBarSirenInUse& light_b */ inline std::vector getEmergencyPriority(const EmergencyPriority& emergency_priority) { return getBitString(emergency_priority.value, emergency_priority.bits_unused); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h index 06815a3c3..e12869942 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h @@ -29,7 +29,8 @@ SOFTWARE. * @brief Common setter functions for the ETSI ITS CAM (EN and TS) */ -#pragma once +# ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H +# define ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H #include @@ -291,4 +292,6 @@ inline void setLightBarSirenInUse(LightBarSirenInUse& light_bar_siren_in_use, co */ inline void setEmergencyPriority(EmergencyPriority& emergency_priority, const std::vector& bits) { setBitString(emergency_priority, bits); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h index 014ef0992..0c11afd65 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h @@ -29,6 +29,9 @@ SOFTWARE. * @brief Main CAM TS access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h index ba54b749a..8de6e1158 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h @@ -32,7 +32,8 @@ SOFTWARE. #include #include -#pragma once +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H /** * @brief Get the TimestampITS from a given GenerationDeltaTime object @@ -75,4 +76,6 @@ inline uint64_t getUnixNanosecondsFromGenerationDeltaTime(const GenerationDeltaT TimestampIts t_its; setTimestampITS(t_its, unix_timestamp_estimate, n_leap_seconds); return getUnixNanosecondsFromGenerationDeltaTime(generation_delta_time, t_its, n_leap_seconds); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H \ No newline at end of file From 87f815b436f414650ebac2ade8329c8c11e62c5e Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 11:01:05 +0000 Subject: [PATCH 16/52] include tests for different msg types in one test_access.cpp --- etsi_its_msgs_utils/CMakeLists.txt | 53 ++------ .../test/impl/test_cam_access.cpp | 119 ++++++++---------- .../test/impl/test_cam_ts_access.cpp | 119 ++++++++---------- .../test/impl/test_cpm_ts_access.cpp | 73 ++++------- .../test/impl/test_denm_access.cpp | 85 +++++-------- etsi_its_msgs_utils/test/test_access.cpp | 46 +++++++ etsi_its_msgs_utils/test/test_access.ros2.cpp | 46 +++++++ etsi_its_msgs_utils/test/test_cam_access.cpp | 10 -- .../test/test_cam_access.ros2.cpp | 10 -- .../test/test_cam_ts_access.ros2.cpp | 10 -- .../test/test_cpm_ts_access.ros2.cpp | 10 -- etsi_its_msgs_utils/test/test_denm_access.cpp | 10 -- .../test/test_denm_access.ros2.cpp | 10 -- 13 files changed, 255 insertions(+), 346 deletions(-) create mode 100644 etsi_its_msgs_utils/test/test_access.cpp create mode 100644 etsi_its_msgs_utils/test/test_access.ros2.cpp delete mode 100644 etsi_its_msgs_utils/test/test_cam_access.cpp delete mode 100644 etsi_its_msgs_utils/test/test_cam_access.ros2.cpp delete mode 100644 etsi_its_msgs_utils/test/test_cam_ts_access.ros2.cpp delete mode 100644 etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp delete mode 100644 etsi_its_msgs_utils/test/test_denm_access.cpp delete mode 100644 etsi_its_msgs_utils/test/test_denm_access.ros2.cpp diff --git a/etsi_its_msgs_utils/CMakeLists.txt b/etsi_its_msgs_utils/CMakeLists.txt index ddc25e9f6..235f10320 100644 --- a/etsi_its_msgs_utils/CMakeLists.txt +++ b/etsi_its_msgs_utils/CMakeLists.txt @@ -44,46 +44,12 @@ if(${ROS_VERSION} EQUAL 2) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}-cam_test test/test_cam_access.ros2.cpp) - target_include_directories(${PROJECT_NAME}-cam_test PUBLIC + ament_add_gtest(${PROJECT_NAME}-test test/test_access.ros2.cpp) + target_include_directories(${PROJECT_NAME}-test PUBLIC test $ $ ) - target_include_directories(${PROJECT_NAME}-cam_test PUBLIC test) - ament_target_dependencies(${PROJECT_NAME}-cam_test - etsi_its_msgs - GeographicLib - geometry_msgs - ) - ament_add_gtest(${PROJECT_NAME}-cam_ts_test test/test_cam_ts_access.ros2.cpp) - target_include_directories(${PROJECT_NAME}-cam_ts_test PUBLIC - $ - $ - ) - target_include_directories(${PROJECT_NAME}-cam_ts_test PUBLIC test) - ament_target_dependencies(${PROJECT_NAME}-cam_ts_test - etsi_its_msgs - GeographicLib - geometry_msgs - ) - ament_add_gtest(${PROJECT_NAME}-cpm_ts_test test/test_cpm_ts_access.ros2.cpp) - target_include_directories(${PROJECT_NAME}-cpm_ts_test PUBLIC - $ - $ - ) - target_include_directories(${PROJECT_NAME}-cpm_ts_test PUBLIC test) - ament_target_dependencies(${PROJECT_NAME}-cpm_ts_test - etsi_its_msgs - GeographicLib - geometry_msgs - ) - ament_add_gtest(${PROJECT_NAME}-denm_test test/test_denm_access.ros2.cpp) - target_include_directories(${PROJECT_NAME}-denm_test PUBLIC - $ - $ - ) - target_include_directories(${PROJECT_NAME}-denm_test PUBLIC test) - ament_target_dependencies(${PROJECT_NAME}-denm_test + ament_target_dependencies(${PROJECT_NAME}-test etsi_its_msgs GeographicLib geometry_msgs @@ -123,15 +89,10 @@ elseif(${ROS_VERSION} EQUAL 1) ) if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}-cam_test test/test_cam_access.cpp) - catkin_add_gtest(${PROJECT_NAME}-denm_test test/test_denm_access.cpp) - if(TARGET ${PROJECT_NAME}-cam_test) - target_include_directories(${PROJECT_NAME}-cam_test PUBLIC test) - target_link_libraries(${PROJECT_NAME}-cam_test ${GeographicLib_LIBRARIES} ${catkin_LIBRARIES}) - endif() - if(TARGET ${PROJECT_NAME}-denm_test) - target_include_directories(${PROJECT_NAME}-denm_test PUBLIC test) - target_link_libraries(${PROJECT_NAME}-denm_test ${GeographicLib_LIBRARIES} ${catkin_LIBRARIES}) + catkin_add_gtest(${PROJECT_NAME}-test test/test_access.cpp) + if(TARGET ${PROJECT_NAME}-test) + target_include_directories(${PROJECT_NAME}-test PUBLIC test) + target_link_libraries(${PROJECT_NAME}-test ${GeographicLib_LIBRARIES} ${catkin_LIBRARIES}) endif() endif() diff --git a/etsi_its_msgs_utils/test/impl/test_cam_access.cpp b/etsi_its_msgs_utils/test/impl/test_cam_access.cpp index 2458a9eec..ea5042140 100644 --- a/etsi_its_msgs_utils/test/impl/test_cam_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_cam_access.cpp @@ -1,34 +1,18 @@ #include -#include -#include -#include -#include - #include -std::default_random_engine random_engine; - - -double randomDouble(double min, double max) { - std::uniform_real_distribution uniform_distribution_double(min, max); - return uniform_distribution_double(random_engine); -} - -int randomInt(int min, int max) { - std::uniform_int_distribution uniform_distribution_int(min, max); - return uniform_distribution_int(random_engine); -} +namespace cam_access = etsi_its_cam_msgs::access; TEST(etsi_its_cam_msgs, test_set_get_cam) { - CAM cam; + cam_msgs::CAM cam; - int station_id = randomInt(StationID::MIN,StationID::MAX); - int protocol_version = randomInt(ItsPduHeader::PROTOCOL_VERSION_MIN,ItsPduHeader::PROTOCOL_VERSION_MAX); - setItsPduHeader(cam, station_id, protocol_version); - EXPECT_EQ(ItsPduHeader::MESSAGE_ID_CAM, cam.header.message_id); + int station_id = randomInt(cam_msgs::StationID::MIN,cam_msgs::StationID::MAX); + int protocol_version = randomInt(cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MIN,cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MAX); + cam_access::setItsPduHeader(cam, station_id, protocol_version); + EXPECT_EQ(cam_msgs::ItsPduHeader::MESSAGE_ID_CAM, cam.header.message_id); EXPECT_EQ(protocol_version, cam.header.protocol_version); - EXPECT_EQ(station_id, getStationID(cam)); + EXPECT_EQ(station_id, cam_access::getStationID(cam)); // https://www.etsi.org/deliver/etsi_ts/102800_102899/10289402/01.02.01_60/ts_10289402v010201p.pdf // DE_TimestampITS @@ -36,86 +20,81 @@ TEST(etsi_its_cam_msgs, test_set_get_cam) { // 94694401000 milliseconds, which includes one leap second insertion // since 2004-01-01T00:00:00.000Z. uint64_t t_2007 = ((uint64_t)1167609600)*1e9; - TimestampIts t_its; + cam_msgs::TimestampIts t_its; EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - etsi_its_cam_msgs::access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + cam_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); EXPECT_EQ(94694401000, t_its.value); - setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000%65536, getGenerationDeltaTimeValue(cam)); - TimestampIts t_its2; + cam_access::setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(94694401000%65536, cam_access::getGenerationDeltaTimeValue(cam)); + cam_msgs::TimestampIts t_its2; uint64_t t_2007_off = t_2007 + 5*1e9; - etsi_its_cam_msgs::access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000, getTimestampITSFromGenerationDeltaTime(getGenerationDeltaTime(cam), t_its2).value); - EXPECT_EQ(t_2007, getUnixNanosecondsFromGenerationDeltaTime(getGenerationDeltaTime(cam), t_its2, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); - EXPECT_EQ(t_2007, getUnixNanosecondsFromGenerationDeltaTime(getGenerationDeltaTime(cam), t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); + cam_access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(94694401000, cam_access::getTimestampITSFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_its2).value); + EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_its2, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); + EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); - int stationType_val = randomInt(StationType::MIN, StationType::MAX); - setStationType(cam, stationType_val); - EXPECT_EQ(stationType_val, getStationType(cam)); + int stationType_val = randomInt(cam_msgs::StationType::MIN, cam_msgs::StationType::MAX); + cam_access::setStationType(cam, stationType_val); + EXPECT_EQ(stationType_val, cam_access::getStationType(cam)); double latitude = randomDouble(-90.0, 90.0); double longitude = randomDouble(-180.0, 180.0); - setReferencePosition(cam, latitude, longitude); - EXPECT_NEAR(latitude, getLatitude(cam), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cam), 1e-7); + cam_access::setReferencePosition(cam, latitude, longitude); + EXPECT_NEAR(latitude, cam_access::getLatitude(cam), 1e-7); + EXPECT_NEAR(longitude, cam_access::getLongitude(cam), 1e-7); latitude = randomDouble(-90.0, 90.0); longitude = randomDouble(-180.0, 180.0); double altitude = randomDouble(-1000.0, 8000.0); - setReferencePosition(cam, latitude, longitude, altitude); - EXPECT_NEAR(latitude, getLatitude(cam), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cam), 1e-7); - EXPECT_NEAR(altitude, getAltitude(cam), 1e-2); + cam_access::setReferencePosition(cam, latitude, longitude, altitude); + EXPECT_NEAR(latitude, cam_access::getLatitude(cam), 1e-7); + EXPECT_NEAR(longitude, cam_access::getLongitude(cam), 1e-7); + EXPECT_NEAR(altitude, cam_access::getAltitude(cam), 1e-2); // Set specific position to test utm projection latitude = 50.787467; longitude = 6.046498; altitude = 209.0; - setReferencePosition(cam, latitude, longitude, altitude); + cam_access::setReferencePosition(cam, latitude, longitude, altitude); int zone; bool northp; - gm::PointStamped utm = getUTMPosition(cam, zone, northp); + gm::PointStamped utm = cam_access::getUTMPosition(cam, zone, northp); EXPECT_NEAR(291827.02, utm.point.x, 1e-1); EXPECT_NEAR(5630349.72, utm.point.y, 1e-1); EXPECT_EQ(altitude, utm.point.z); EXPECT_EQ(32, zone); EXPECT_EQ(true, northp); - setFromUTMPosition(cam, utm, zone, northp); - EXPECT_NEAR(latitude, getLatitude(cam), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cam), 1e-7); - EXPECT_NEAR(altitude, getAltitude(cam), 1e-2); + cam_access::setFromUTMPosition(cam, utm, zone, northp); + EXPECT_NEAR(latitude, cam_access::getLatitude(cam), 1e-7); + EXPECT_NEAR(longitude, cam_access::getLongitude(cam), 1e-7); + EXPECT_NEAR(altitude, cam_access::getAltitude(cam), 1e-2); double heading_val = randomDouble(0.0, 360.0); - setHeading(cam, heading_val); - EXPECT_NEAR(heading_val, getHeading(cam), 1e-1); + cam_access::setHeading(cam, heading_val); + EXPECT_NEAR(heading_val, cam_access::getHeading(cam), 1e-1); double length = randomDouble(0.0, 102.2); double width = randomDouble(0.0, 6.2); - setVehicleDimensions(cam, length, width); - EXPECT_NEAR(length, getVehicleLength(cam), 1e-1); - EXPECT_NEAR(width, getVehicleWidth(cam), 1e-1); + cam_access::setVehicleDimensions(cam, length, width); + EXPECT_NEAR(length, cam_access::getVehicleLength(cam), 1e-1); + EXPECT_NEAR(width, cam_access::getVehicleWidth(cam), 1e-1); double speed_val = randomDouble(0.0, 163.82); - setSpeed(cam, speed_val); - EXPECT_NEAR(speed_val, getSpeed(cam), 1e-2); + cam_access::setSpeed(cam, speed_val); + EXPECT_NEAR(speed_val, cam_access::getSpeed(cam), 1e-2); double lon_accel = randomDouble(-16.0, 16.0); double lat_accel = randomDouble(-16.0, 16.0); - setLongitudinalAcceleration(cam, lon_accel); - EXPECT_NEAR(lon_accel, getLongitudinalAcceleration(cam), 1e-1); - setLateralAcceleration(cam, lat_accel); - EXPECT_NEAR(lat_accel, getLateralAcceleration(cam), 1e-1); + cam_access::setLongitudinalAcceleration(cam, lon_accel); + EXPECT_NEAR(lon_accel, cam_access::getLongitudinalAcceleration(cam), 1e-1); + cam_access::setLateralAcceleration(cam, lat_accel); + EXPECT_NEAR(lat_accel, cam_access::getLateralAcceleration(cam), 1e-1); - std::vector exterior_lights(ExteriorLights::SIZE_BITS); - for (int i = 0; i < ExteriorLights::SIZE_BITS; i++) { + std::vector exterior_lights(cam_msgs::ExteriorLights::SIZE_BITS); + for (int i = 0; i < cam_msgs::ExteriorLights::SIZE_BITS; i++) { exterior_lights.at(i) = randomInt(0, 1); } cam.cam.cam_parameters.low_frequency_container_is_present = true; - cam.cam.cam_parameters.low_frequency_container.choice = LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; - setExteriorLights(cam, exterior_lights); - EXPECT_EQ(exterior_lights, getExteriorLights(cam)); -} - -int main(int argc, char *argv[]) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} + cam.cam.cam_parameters.low_frequency_container.choice = cam_msgs::LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; + cam_access::setExteriorLights(cam, exterior_lights); + EXPECT_EQ(exterior_lights, cam_access::getExteriorLights(cam)); +} \ No newline at end of file diff --git a/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp b/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp index c6b7a57bb..1be165756 100644 --- a/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp @@ -1,34 +1,18 @@ #include -#include -#include -#include -#include - #include -std::default_random_engine random_engine; - - -double randomDouble(double min, double max) { - std::uniform_real_distribution uniform_distribution_double(min, max); - return uniform_distribution_double(random_engine); -} - -int randomInt(int min, int max) { - std::uniform_int_distribution uniform_distribution_int(min, max); - return uniform_distribution_int(random_engine); -} +namespace cam_ts_access = etsi_its_cam_ts_msgs::access; TEST(etsi_its_cam_ts_msgs, test_set_get_cam) { - CAM cam; + cam_ts_msgs::CAM cam; - int station_id = randomInt(StationId::MIN,StationId::MAX); - int protocol_version = randomInt(OrdinalNumber1B::MIN,OrdinalNumber1B::MAX); - setItsPduHeader(cam, station_id, protocol_version); - EXPECT_EQ(MessageId::CAM, cam.header.message_id.value); + int station_id = randomInt(cam_ts_msgs::StationId::MIN,cam_ts_msgs::StationId::MAX); + int protocol_version = randomInt(cam_ts_msgs::OrdinalNumber1B::MIN,cam_ts_msgs::OrdinalNumber1B::MAX); + cam_ts_access::setItsPduHeader(cam, station_id, protocol_version); + EXPECT_EQ(cam_ts_msgs::MessageId::CAM, cam.header.message_id.value); EXPECT_EQ(protocol_version, cam.header.protocol_version.value); - EXPECT_EQ(station_id, getStationID(cam)); + EXPECT_EQ(station_id, cam_ts_access::getStationID(cam)); // https://www.etsi.org/deliver/etsi_ts/102800_102899/10289402/01.02.01_60/ts_10289402v010201p.pdf // DE_TimestampITS @@ -36,86 +20,81 @@ TEST(etsi_its_cam_ts_msgs, test_set_get_cam) { // 94694401000 milliseconds, which includes one leap second insertion // since 2004-01-01T00:00:00.000Z. uint64_t t_2007 = ((uint64_t)1167609600)*1e9; - TimestampIts t_its; + cam_ts_msgs::TimestampIts t_its; EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - etsi_its_cam_ts_msgs::access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + cam_ts_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); EXPECT_EQ(94694401000, t_its.value); - setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000%65536, getGenerationDeltaTimeValue(cam)); - TimestampIts t_its2; + cam_ts_access::setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(94694401000%65536, cam_ts_access::getGenerationDeltaTimeValue(cam)); + cam_ts_msgs::TimestampIts t_its2; uint64_t t_2007_off = t_2007 + 5*1e9; - etsi_its_cam_ts_msgs::access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000, getTimestampITSFromGenerationDeltaTime(getGenerationDeltaTime(cam), t_its2).value); - EXPECT_EQ(t_2007, getUnixNanosecondsFromGenerationDeltaTime(getGenerationDeltaTime(cam), t_its2, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); - EXPECT_EQ(t_2007, getUnixNanosecondsFromGenerationDeltaTime(getGenerationDeltaTime(cam), t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); + cam_ts_access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(94694401000, cam_ts_access::getTimestampITSFromGenerationDeltaTime(cam_ts_access::getGenerationDeltaTime(cam), t_its2).value); + EXPECT_EQ(t_2007, cam_ts_access::getUnixNanosecondsFromGenerationDeltaTime(cam_ts_access::getGenerationDeltaTime(cam), t_its2, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); + EXPECT_EQ(t_2007, cam_ts_access::getUnixNanosecondsFromGenerationDeltaTime(cam_ts_access::getGenerationDeltaTime(cam), t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); - int stationType_val = randomInt(TrafficParticipantType::MIN, TrafficParticipantType::MAX); - setStationType(cam, stationType_val); - EXPECT_EQ(stationType_val, getStationType(cam)); + int stationType_val = randomInt(cam_ts_msgs::TrafficParticipantType::MIN, cam_ts_msgs::TrafficParticipantType::MAX); + cam_ts_access::setStationType(cam, stationType_val); + EXPECT_EQ(stationType_val, cam_ts_access::getStationType(cam)); double latitude = randomDouble(-90.0, 90.0); double longitude = randomDouble(-180.0, 180.0); - setReferencePosition(cam, latitude, longitude); - EXPECT_NEAR(latitude, getLatitude(cam), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cam), 1e-7); + cam_ts_access::setReferencePosition(cam, latitude, longitude); + EXPECT_NEAR(latitude, cam_ts_access::getLatitude(cam), 1e-7); + EXPECT_NEAR(longitude, cam_ts_access::getLongitude(cam), 1e-7); latitude = randomDouble(-90.0, 90.0); longitude = randomDouble(-180.0, 180.0); double altitude = randomDouble(-1000.0, 8000.0); - setReferencePosition(cam, latitude, longitude, altitude); - EXPECT_NEAR(latitude, getLatitude(cam), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cam), 1e-7); - EXPECT_NEAR(altitude, getAltitude(cam), 1e-2); + cam_ts_access::setReferencePosition(cam, latitude, longitude, altitude); + EXPECT_NEAR(latitude, cam_ts_access::getLatitude(cam), 1e-7); + EXPECT_NEAR(longitude, cam_ts_access::getLongitude(cam), 1e-7); + EXPECT_NEAR(altitude, cam_ts_access::getAltitude(cam), 1e-2); // Set specific position to test utm projection latitude = 50.787467; longitude = 6.046498; altitude = 209.0; - setReferencePosition(cam, latitude, longitude, altitude); + cam_ts_access::setReferencePosition(cam, latitude, longitude, altitude); int zone; bool northp; - gm::PointStamped utm = getUTMPosition(cam, zone, northp); + gm::PointStamped utm = cam_ts_access::getUTMPosition(cam, zone, northp); EXPECT_NEAR(291827.02, utm.point.x, 1e-1); EXPECT_NEAR(5630349.72, utm.point.y, 1e-1); EXPECT_EQ(altitude, utm.point.z); EXPECT_EQ(32, zone); EXPECT_EQ(true, northp); - setFromUTMPosition(cam, utm, zone, northp); - EXPECT_NEAR(latitude, getLatitude(cam), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cam), 1e-7); - EXPECT_NEAR(altitude, getAltitude(cam), 1e-2); + cam_ts_access::setFromUTMPosition(cam, utm, zone, northp); + EXPECT_NEAR(latitude, cam_ts_access::getLatitude(cam), 1e-7); + EXPECT_NEAR(longitude, cam_ts_access::getLongitude(cam), 1e-7); + EXPECT_NEAR(altitude, cam_ts_access::getAltitude(cam), 1e-2); double heading_val = randomDouble(0.0, 360.0); - setHeading(cam, heading_val); - EXPECT_NEAR(heading_val, getHeading(cam), 1e-1); + cam_ts_access::setHeading(cam, heading_val); + EXPECT_NEAR(heading_val, cam_ts_access::getHeading(cam), 1e-1); double length = randomDouble(0.0, 102.2); double width = randomDouble(0.0, 6.2); - setVehicleDimensions(cam, length, width); - EXPECT_NEAR(length, getVehicleLength(cam), 1e-1); - EXPECT_NEAR(width, getVehicleWidth(cam), 1e-1); + cam_ts_access::setVehicleDimensions(cam, length, width); + EXPECT_NEAR(length, cam_ts_access::getVehicleLength(cam), 1e-1); + EXPECT_NEAR(width, cam_ts_access::getVehicleWidth(cam), 1e-1); double speed_val = randomDouble(0.0, 163.82); - setSpeed(cam, speed_val); - EXPECT_NEAR(speed_val, getSpeed(cam), 1e-2); + cam_ts_access::setSpeed(cam, speed_val); + EXPECT_NEAR(speed_val, cam_ts_access::getSpeed(cam), 1e-2); double lon_accel = randomDouble(-16.0, 16.0); double lat_accel = randomDouble(-16.0, 16.0); - setLongitudinalAcceleration(cam, lon_accel); - EXPECT_NEAR(lon_accel, getLongitudinalAcceleration(cam), 1e-1); - setLateralAcceleration(cam, lat_accel); - EXPECT_NEAR(lat_accel, getLateralAcceleration(cam), 1e-1); + cam_ts_access::setLongitudinalAcceleration(cam, lon_accel); + EXPECT_NEAR(lon_accel, cam_ts_access::getLongitudinalAcceleration(cam), 1e-1); + cam_ts_access::setLateralAcceleration(cam, lat_accel); + EXPECT_NEAR(lat_accel, cam_ts_access::getLateralAcceleration(cam), 1e-1); - std::vector exterior_lights(ExteriorLights::SIZE_BITS); - for (int i = 0; i < ExteriorLights::SIZE_BITS; i++) { + std::vector exterior_lights(cam_ts_msgs::ExteriorLights::SIZE_BITS); + for (int i = 0; i < cam_ts_msgs::ExteriorLights::SIZE_BITS; i++) { exterior_lights.at(i) = randomInt(0, 1); } cam.cam.cam_parameters.low_frequency_container_is_present = true; - cam.cam.cam_parameters.low_frequency_container.choice = LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; - setExteriorLights(cam, exterior_lights); - EXPECT_EQ(exterior_lights, getExteriorLights(cam)); -} - -int main(int argc, char *argv[]) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} + cam.cam.cam_parameters.low_frequency_container.choice = cam_ts_msgs::LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; + cam_ts_access::setExteriorLights(cam, exterior_lights); + EXPECT_EQ(exterior_lights, cam_ts_access::getExteriorLights(cam)); +} \ No newline at end of file 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 e0cdf45d4..91f2cb4b7 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 @@ -1,34 +1,18 @@ #include -#include -#include -#include -#include - #include -std::default_random_engine random_engine; - - -double randomDouble(double min, double max) { - std::uniform_real_distribution uniform_distribution_double(min, max); - return uniform_distribution_double(random_engine); -} - -int randomInt(int min, int max) { - std::uniform_int_distribution uniform_distribution_int(min, max); - return uniform_distribution_int(random_engine); -} +namespace cpm_ts_access = etsi_its_cpm_ts_msgs::access; TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { - CollectivePerceptionMessage cpm; + cpm_ts_msgs::CollectivePerceptionMessage cpm; - int station_id = randomInt(StationId::MIN,StationId::MAX); - int protocol_version = randomInt(OrdinalNumber1B::MIN,OrdinalNumber1B::MAX); - setItsPduHeader(cpm, station_id, protocol_version); - EXPECT_EQ(MessageId::CPM, cpm.header.message_id.value); + int station_id = randomInt(cpm_ts_msgs::StationId::MIN,cpm_ts_msgs::StationId::MAX); + int protocol_version = randomInt(cpm_ts_msgs::OrdinalNumber1B::MIN,cpm_ts_msgs::OrdinalNumber1B::MAX); + cpm_ts_access::setItsPduHeader(cpm, station_id, protocol_version); + EXPECT_EQ(cpm_ts_msgs::MessageId::CPM, cpm.header.message_id.value); EXPECT_EQ(protocol_version, cpm.header.protocol_version.value); - EXPECT_EQ(station_id, getStationID(cpm)); + EXPECT_EQ(station_id, cpm_ts_access::getStationID(cpm)); // https://www.etsi.org/deliver/etsi_ts/102800_102899/10289402/01.02.01_60/ts_10289402v010201p.pdf // DE_TimestampITS @@ -37,49 +21,44 @@ TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { // since 2004-01-01T00:00:00.000Z. uint64_t t_2007 = ((uint64_t)1167609600)*1e9; - TimestampIts t_its; + cpm_ts_msgs::TimestampIts t_its; EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - etsi_its_cpm_ts_msgs::access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + cpm_ts_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); EXPECT_EQ(94694401000, t_its.value); - setReferenceTime(cpm, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000, getReferenceTimeValue(cpm)); - EXPECT_EQ(t_2007, getUnixNanosecondsFromReferenceTime(getReferenceTime(cpm), 1)); + cpm_ts_access::setReferenceTime(cpm, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(94694401000, cpm_ts_access::getReferenceTimeValue(cpm)); + EXPECT_EQ(t_2007, cpm_ts_access::getUnixNanosecondsFromReferenceTime(cpm_ts_access::getReferenceTime(cpm), 1)); double latitude = randomDouble(-90.0, 90.0); double longitude = randomDouble(-180.0, 180.0); - setReferencePosition(cpm, latitude, longitude); - EXPECT_NEAR(latitude, getLatitude(cpm), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cpm), 1e-7); + cpm_ts_access::setReferencePosition(cpm, latitude, longitude); + EXPECT_NEAR(latitude, cpm_ts_access::getLatitude(cpm), 1e-7); + EXPECT_NEAR(longitude, cpm_ts_access::getLongitude(cpm), 1e-7); latitude = randomDouble(-90.0, 90.0); longitude = randomDouble(-180.0, 180.0); double altitude = randomDouble(-1000.0, 8000.0); - setReferencePosition(cpm, latitude, longitude, altitude); - EXPECT_NEAR(latitude, getLatitude(cpm), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cpm), 1e-7); - EXPECT_NEAR(altitude, getAltitude(cpm), 1e-2); + cpm_ts_access::setReferencePosition(cpm, latitude, longitude, altitude); + EXPECT_NEAR(latitude, cpm_ts_access::getLatitude(cpm), 1e-7); + EXPECT_NEAR(longitude, cpm_ts_access::getLongitude(cpm), 1e-7); + EXPECT_NEAR(altitude, cpm_ts_access::getAltitude(cpm), 1e-2); // Set specific position to test utm projection latitude = 50.787467; longitude = 6.046498; altitude = 209.0; - setReferencePosition(cpm, latitude, longitude, altitude); + cpm_ts_access::setReferencePosition(cpm, latitude, longitude, altitude); int zone; bool northp; - gm::PointStamped utm = getUTMPosition(cpm, zone, northp); + gm::PointStamped utm = cpm_ts_access::getUTMPosition(cpm, zone, northp); EXPECT_NEAR(291827.02, utm.point.x, 1e-1); EXPECT_NEAR(5630349.72, utm.point.y, 1e-1); EXPECT_EQ(altitude, utm.point.z); EXPECT_EQ(32, zone); EXPECT_EQ(true, northp); - setFromUTMPosition(cpm, utm, zone, northp); - EXPECT_NEAR(latitude, getLatitude(cpm), 1e-7); - EXPECT_NEAR(longitude, getLongitude(cpm), 1e-7); - EXPECT_NEAR(altitude, getAltitude(cpm), 1e-2); + cpm_ts_access::setFromUTMPosition(cpm, utm, zone, northp); + EXPECT_NEAR(latitude, cpm_ts_access::getLatitude(cpm), 1e-7); + 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 -} - -int main(int argc, char *argv[]) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +} \ No newline at end of file diff --git a/etsi_its_msgs_utils/test/impl/test_denm_access.cpp b/etsi_its_msgs_utils/test/impl/test_denm_access.cpp index af837f97b..d5bdbe266 100644 --- a/etsi_its_msgs_utils/test/impl/test_denm_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_denm_access.cpp @@ -1,34 +1,18 @@ #include -#include -#include -#include -#include - #include -std::default_random_engine random_engine; - - -double randomDouble(double min, double max) { - std::uniform_real_distribution uniform_distribution_double(min, max); - return uniform_distribution_double(random_engine); -} - -int randomInt(int min, int max) { - std::uniform_int_distribution uniform_distribution_int(min, max); - return uniform_distribution_int(random_engine); -} +namespace denm_access = etsi_its_denm_msgs::access; TEST(etsi_its_denm_msgs, test_set_get_denm) { - DENM denm; + denm_msgs::DENM denm; - int station_id = randomInt(StationID::MIN,StationID::MAX); - int protocol_version = randomInt(ItsPduHeader::PROTOCOL_VERSION_MIN,ItsPduHeader::PROTOCOL_VERSION_MAX); - setItsPduHeader(denm, station_id, protocol_version); - EXPECT_EQ(ItsPduHeader::MESSAGE_ID_DENM, denm.header.message_id); + int station_id = randomInt(denm_msgs::StationID::MIN,denm_msgs::StationID::MAX); + int protocol_version = randomInt(denm_msgs::ItsPduHeader::PROTOCOL_VERSION_MIN,denm_msgs::ItsPduHeader::PROTOCOL_VERSION_MAX); + denm_access::setItsPduHeader(denm, station_id, protocol_version); + EXPECT_EQ(denm_msgs::ItsPduHeader::MESSAGE_ID_DENM, denm.header.message_id); EXPECT_EQ(protocol_version, denm.header.protocol_version); - EXPECT_EQ(station_id, getStationID(denm)); + EXPECT_EQ(station_id, denm_access::getStationID(denm)); // https://www.etsi.org/deliver/etsi_ts/102800_102899/10289402/01.02.01_60/ts_10289402v010201p.pdf // DE_TimestampITS @@ -36,62 +20,57 @@ TEST(etsi_its_denm_msgs, test_set_get_denm) { // 94694401000 milliseconds, which includes one leap second insertion // since 2004-01-01T00:00:00.000Z. uint64_t t_2007 = ((uint64_t)1167609600)*1e9; - TimestampIts t_its; - etsi_its_denm_msgs::access::setTimestampITS(t_its, t_2007, 1); + denm_msgs::TimestampIts t_its; + denm_access::setTimestampITS(t_its, t_2007, 1); EXPECT_EQ(94694401000, t_its.value); - setReferenceTime(denm, t_2007, 1); - EXPECT_EQ(94694401000, getReferenceTimeValue(denm)); + denm_access::setReferenceTime(denm, t_2007, 1); + EXPECT_EQ(94694401000, denm_access::getReferenceTimeValue(denm)); uint64_t t_2007_off = t_2007 + 5*1e9; - EXPECT_EQ(t_2007, getUnixNanosecondsFromReferenceTime(getReferenceTime(denm), 1)); + EXPECT_EQ(t_2007, denm_access::getUnixNanosecondsFromReferenceTime(denm_access::getReferenceTime(denm), 1)); - int stationType_val = randomInt(StationType::MIN, StationType::MAX); - setStationType(denm, stationType_val); - EXPECT_EQ(stationType_val, getStationType(denm)); + int stationType_val = randomInt(denm_msgs::StationType::MIN, denm_msgs::StationType::MAX); + denm_access::setStationType(denm, stationType_val); + EXPECT_EQ(stationType_val, denm_access::getStationType(denm)); double latitude = randomDouble(-90.0, 90.0); double longitude = randomDouble(-180.0, 180.0); - setReferencePosition(denm, latitude, longitude); - EXPECT_NEAR(latitude, getLatitude(denm), 1e-7); - EXPECT_NEAR(longitude, getLongitude(denm), 1e-7); + denm_access::setReferencePosition(denm, latitude, longitude); + EXPECT_NEAR(latitude, denm_access::getLatitude(denm), 1e-7); + EXPECT_NEAR(longitude, denm_access::getLongitude(denm), 1e-7); latitude = randomDouble(-90.0, 90.0); longitude = randomDouble(-180.0, 180.0); double altitude = randomDouble(-1000.0, 8000.0); - setReferencePosition(denm, latitude, longitude, altitude); - EXPECT_NEAR(latitude, getLatitude(denm), 1e-7); - EXPECT_NEAR(longitude, getLongitude(denm), 1e-7); - EXPECT_NEAR(altitude, getAltitude(denm), 1e-2); + denm_access::setReferencePosition(denm, latitude, longitude, altitude); + EXPECT_NEAR(latitude, denm_access::getLatitude(denm), 1e-7); + EXPECT_NEAR(longitude, denm_access::getLongitude(denm), 1e-7); + EXPECT_NEAR(altitude, denm_access::getAltitude(denm), 1e-2); // Set specific position to test utm projection latitude = 50.787467; longitude = 6.046498; altitude = 209.0; - setReferencePosition(denm, latitude, longitude, altitude); + denm_access::setReferencePosition(denm, latitude, longitude, altitude); int zone; bool northp; - gm::PointStamped utm = getUTMPosition(denm, zone, northp); + gm::PointStamped utm = denm_access::getUTMPosition(denm, zone, northp); EXPECT_NEAR(291827.02, utm.point.x, 1e-1); EXPECT_NEAR(5630349.72, utm.point.y, 1e-1); EXPECT_EQ(altitude, utm.point.z); EXPECT_EQ(32, zone); EXPECT_EQ(true, northp); - setFromUTMPosition(denm, utm, zone, northp); - EXPECT_NEAR(latitude, getLatitude(denm), 1e-7); - EXPECT_NEAR(longitude, getLongitude(denm), 1e-7); - EXPECT_NEAR(altitude, getAltitude(denm), 1e-2); + denm_access::setFromUTMPosition(denm, utm, zone, northp); + EXPECT_NEAR(latitude, denm_access::getLatitude(denm), 1e-7); + EXPECT_NEAR(longitude, denm_access::getLongitude(denm), 1e-7); + EXPECT_NEAR(altitude, denm_access::getAltitude(denm), 1e-2); double heading_val = randomDouble(0.0, 360.0); denm.denm.location_is_present = true; - setHeading(denm, heading_val); - EXPECT_NEAR(heading_val, getHeading(denm), 1e-1); + denm_access::setHeading(denm, heading_val); + EXPECT_NEAR(heading_val, denm_access::getHeading(denm), 1e-1); double speed_val = randomDouble(0.0, 163.82); denm.denm.location_is_present = true; - setSpeed(denm, speed_val); - EXPECT_NEAR(speed_val, getSpeed(denm), 1e-2); -} - -int main(int argc, char *argv[]) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + denm_access::setSpeed(denm, speed_val); + EXPECT_NEAR(speed_val, denm_access::getSpeed(denm), 1e-2); } \ No newline at end of file diff --git a/etsi_its_msgs_utils/test/test_access.cpp b/etsi_its_msgs_utils/test/test_access.cpp new file mode 100644 index 000000000..0f1e93987 --- /dev/null +++ b/etsi_its_msgs_utils/test/test_access.cpp @@ -0,0 +1,46 @@ + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +std::default_random_engine random_engine; +double randomDouble(double min, double max) { + std::uniform_real_distribution uniform_distribution_double(min, max); + return uniform_distribution_double(random_engine); +} +int randomInt(int min, int max) { + std::uniform_int_distribution uniform_distribution_int(min, max); + return uniform_distribution_int(random_engine); +} + +namespace gm = geometry_msgs; + +namespace cam_msgs = etsi_its_cam_msgs; +#include + +namespace cam_ts_msgs = etsi_its_cam_ts_msgs; +#include + +namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs; +#include + +namespace denm_msgs = etsi_its_denm_msgs; +#include + + +int main(int argc, char *argv[]) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/etsi_its_msgs_utils/test/test_access.ros2.cpp b/etsi_its_msgs_utils/test/test_access.ros2.cpp new file mode 100644 index 000000000..880c27aec --- /dev/null +++ b/etsi_its_msgs_utils/test/test_access.ros2.cpp @@ -0,0 +1,46 @@ + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +std::default_random_engine random_engine; +double randomDouble(double min, double max) { + std::uniform_real_distribution uniform_distribution_double(min, max); + return uniform_distribution_double(random_engine); +} +int randomInt(int min, int max) { + std::uniform_int_distribution uniform_distribution_int(min, max); + return uniform_distribution_int(random_engine); +} + +namespace gm = geometry_msgs::msg; + +namespace cam_msgs = etsi_its_cam_msgs::msg; +#include + +namespace cam_ts_msgs = etsi_its_cam_ts_msgs::msg; +#include + +namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs::msg; +#include + +namespace denm_msgs = etsi_its_denm_msgs::msg; +#include + + +int main(int argc, char *argv[]) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/etsi_its_msgs_utils/test/test_cam_access.cpp b/etsi_its_msgs_utils/test/test_cam_access.cpp deleted file mode 100644 index 47a0d784c..000000000 --- a/etsi_its_msgs_utils/test/test_cam_access.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include - -#include - -using namespace etsi_its_cam_msgs; -using namespace etsi_its_cam_msgs::access; -namespace gm = geometry_msgs; - -#include diff --git a/etsi_its_msgs_utils/test/test_cam_access.ros2.cpp b/etsi_its_msgs_utils/test/test_cam_access.ros2.cpp deleted file mode 100644 index 255701401..000000000 --- a/etsi_its_msgs_utils/test/test_cam_access.ros2.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include - -#include - -using namespace etsi_its_cam_msgs::msg; -using namespace etsi_its_cam_msgs::access; -namespace gm = geometry_msgs::msg; - -#include diff --git a/etsi_its_msgs_utils/test/test_cam_ts_access.ros2.cpp b/etsi_its_msgs_utils/test/test_cam_ts_access.ros2.cpp deleted file mode 100644 index ab45d10bc..000000000 --- a/etsi_its_msgs_utils/test/test_cam_ts_access.ros2.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include - -#include - -using namespace etsi_its_cam_ts_msgs::msg; -using namespace etsi_its_cam_ts_msgs::access; -namespace gm = geometry_msgs::msg; - -#include diff --git a/etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp b/etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp deleted file mode 100644 index ce54047d4..000000000 --- a/etsi_its_msgs_utils/test/test_cpm_ts_access.ros2.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include - -#include - -using namespace etsi_its_cpm_ts_msgs::msg; -using namespace etsi_its_cpm_ts_msgs::access; -namespace gm = geometry_msgs::msg; - -#include diff --git a/etsi_its_msgs_utils/test/test_denm_access.cpp b/etsi_its_msgs_utils/test/test_denm_access.cpp deleted file mode 100644 index 2a8d206c0..000000000 --- a/etsi_its_msgs_utils/test/test_denm_access.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include - -#include - -using namespace etsi_its_denm_msgs; -using namespace etsi_its_denm_msgs::access; -namespace gm = geometry_msgs; - -#include diff --git a/etsi_its_msgs_utils/test/test_denm_access.ros2.cpp b/etsi_its_msgs_utils/test/test_denm_access.ros2.cpp deleted file mode 100644 index ed2fb7ae4..000000000 --- a/etsi_its_msgs_utils/test/test_denm_access.ros2.cpp +++ /dev/null @@ -1,10 +0,0 @@ - -#include - -#include - -using namespace etsi_its_denm_msgs::msg; -using namespace etsi_its_denm_msgs::access; -namespace gm = geometry_msgs::msg; - -#include \ No newline at end of file From d5dd7d98a4ce292bd8c33607a73b34f3a2ce4716 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Mon, 2 Sep 2024 11:56:38 +0000 Subject: [PATCH 17/52] add ROS 1 access function header --- .../etsi_its_msgs_utils/cpm_ts_access.h | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h new file mode 100644 index 000000000..10a87ed5f --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h @@ -0,0 +1,43 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= +*/ + +/** + * @file cpm_ts_access.h + * @brief Main CPM access header to include in ROS 1 projects + */ + +#pragma once + +// Messages +#include +#include + +namespace etsi_its_cpm_ts_msgs { + namespace gm = geometry_msgs; +} + +// Implementation +#include \ No newline at end of file From a7e44f6b4c87d2b146f54c34134bacc41b2a47c6 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Tue, 3 Sep 2024 11:02:57 +0200 Subject: [PATCH 18/52] fix path for ROS 1 access --- etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h index 10a87ed5f..00a9b6b80 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h @@ -32,7 +32,7 @@ SOFTWARE. #pragma once // Messages -#include +#include #include namespace etsi_its_cpm_ts_msgs { From f08d86f63c245c0d7237d5fe5ccf7caf291b6251 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Tue, 3 Sep 2024 15:27:35 +0200 Subject: [PATCH 19/52] implement setters for CPM perceived object container --- .../etsi_its_msgs_utils/cpm_ts_access.h | 1 + .../etsi_its_msgs_utils/cpm_ts_access.hpp | 1 + .../impl/cpm/cpm_ts_setters.h | 174 ++++++++++++++++++ 3 files changed, 176 insertions(+) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h index 00a9b6b80..5c8fb5724 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h @@ -34,6 +34,7 @@ SOFTWARE. // Messages #include #include +#include namespace etsi_its_cpm_ts_msgs { namespace gm = geometry_msgs; diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp index 60b66dcd7..9c76c8ada 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp @@ -34,6 +34,7 @@ SOFTWARE. // Messages #include #include +#include namespace etsi_its_cpm_ts_msgs { using namespace msg; 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 0dfb122fd..199b4bd1b 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 @@ -93,4 +93,178 @@ namespace etsi_its_cpm_ts_msgs::access { 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; + } + + 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; + } + } + + 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; + } + } + + inline void setVelocityComponent(VelocityComponent& velocity, const int16_t value, const uint8_t confidence = SpeedConfidence::UNAVAILABLE){ + // limit value range + int16_t limited_value = std::min(VelocityComponentValue::NEGATIVE_OUT_OF_RANGE, std::max(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 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; + } + + inline void setAccelerationComponent(AccelerationComponent& acceleration, const int16_t value, const uint8_t confidence = AccelerationConfidence::UNAVAILABLE){ + // limit value range + int16_t limited_value = std::min(AccelerationValue::NEGATIVE_OUT_OF_RANGE, std::max(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 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; + } + + 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; + while (yaw_in_degrees > 360.0) yaw_in_degrees -= 360.0; + while (yaw_in_degrees < 360.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 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::min(CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE, std::max(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 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; + } + } + + 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; + } + + 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; + } + + 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; + } + + 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 initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0){ + setPositionOfPerceivedObject(object, point); + setMeasurementDeltaTimeOfPerceivedObject(object, delta_time); + } + + 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; + } + + 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"); + } + } + } // namespace etsi_its_cpm_ts_msgs::access From 911c1e2d2db86242c8018fd63320eab9350836a4 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Tue, 3 Sep 2024 16:38:33 +0200 Subject: [PATCH 20/52] improve getting unix timestamp for CPM and DENM --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h | 7 ++++--- .../include/etsi_its_msgs_utils/impl/denm/denm_utils.h | 7 ++++--- etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp | 2 +- etsi_its_msgs_utils/test/impl/test_denm_access.cpp | 2 +- .../include/displays/DENM/denm_render_object.hpp | 2 +- etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp | 2 +- .../src/displays/DENM/denm_render_object.cpp | 4 ++-- 7 files changed, 14 insertions(+), 12 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h index 7b6567d66..c576f1e08 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h @@ -41,7 +41,8 @@ SOFTWARE. * @param n_leap_seconds number of leap-seconds since 2004. (Default: etsi_its_msgs::N_LEAP_SECONDS) * @return uint64_t the corresponding Unix-Nanoseconds */ -inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) -{ - return reference_time.value*1e6+etsi_its_msgs::UNIX_SECONDS_2004*1e9-n_leap_seconds*1e9; +inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time){ + double unix_time_with_leap_seconds = reference_time.value*1e-3+etsi_its_msgs::UNIX_SECONDS_2004; + uint16_t n_leap_seconds = etsi_its_msgs::getLeapSecondInsertionsSince2004(static_cast(unix_time_with_leap_seconds)); + return (unix_time_with_leap_seconds - n_leap_seconds) * 1e9; } \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h index cfcf9a8a8..4a17bea49 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h @@ -41,7 +41,8 @@ SOFTWARE. * @param n_leap_seconds number of leap-seconds since 2004. (Default: etsi_its_msgs::N_LEAP_SECONDS) * @return uint64_t the corresponding Unix-Nanoseconds */ -inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) -{ - return reference_time.value*1e6+etsi_its_msgs::UNIX_SECONDS_2004*1e9-n_leap_seconds*1e9; +inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time){ + double unix_time_with_leap_seconds = reference_time.value*1e-3+etsi_its_msgs::UNIX_SECONDS_2004; + uint16_t n_leap_seconds = etsi_its_msgs::getLeapSecondInsertionsSince2004(static_cast(unix_time_with_leap_seconds)); + return (unix_time_with_leap_seconds - n_leap_seconds) * 1e9; } \ No newline at end of file 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 91f2cb4b7..9db66c151 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 @@ -27,7 +27,7 @@ TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { EXPECT_EQ(94694401000, t_its.value); cpm_ts_access::setReferenceTime(cpm, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); EXPECT_EQ(94694401000, cpm_ts_access::getReferenceTimeValue(cpm)); - EXPECT_EQ(t_2007, cpm_ts_access::getUnixNanosecondsFromReferenceTime(cpm_ts_access::getReferenceTime(cpm), 1)); + EXPECT_EQ(t_2007, cpm_ts_access::getUnixNanosecondsFromReferenceTime(cpm_ts_access::getReferenceTime(cpm))); double latitude = randomDouble(-90.0, 90.0); double longitude = randomDouble(-180.0, 180.0); diff --git a/etsi_its_msgs_utils/test/impl/test_denm_access.cpp b/etsi_its_msgs_utils/test/impl/test_denm_access.cpp index d5bdbe266..35353a014 100644 --- a/etsi_its_msgs_utils/test/impl/test_denm_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_denm_access.cpp @@ -27,7 +27,7 @@ TEST(etsi_its_denm_msgs, test_set_get_denm) { denm_access::setReferenceTime(denm, t_2007, 1); EXPECT_EQ(94694401000, denm_access::getReferenceTimeValue(denm)); uint64_t t_2007_off = t_2007 + 5*1e9; - EXPECT_EQ(t_2007, denm_access::getUnixNanosecondsFromReferenceTime(denm_access::getReferenceTime(denm), 1)); + EXPECT_EQ(t_2007, denm_access::getUnixNanosecondsFromReferenceTime(denm_access::getReferenceTime(denm))); int stationType_val = randomInt(denm_msgs::StationType::MIN, denm_msgs::StationType::MAX); denm_access::setStationType(denm, stationType_val); diff --git a/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp b/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp index 804470b0c..c31ee7455 100644 --- a/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp +++ b/etsi_its_rviz_plugins/include/displays/DENM/denm_render_object.hpp @@ -22,7 +22,7 @@ namespace displays class DENMRenderObject { public: - DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm, uint16_t n_leap_seconds=etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second); + DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm); /** * @brief This function validates all float variables that are part of a DENMRenderObject diff --git a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp index c7012a063..03474f15e 100644 --- a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp @@ -72,7 +72,7 @@ void DENMDisplay::processMessage(etsi_its_denm_msgs::msg::DENM::ConstSharedPtr m { // Generate DENM render object from message rclcpp::Time now = rviz_node_->now(); - DENMRenderObject denm(*msg, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); // 5 leap seconds in 2023 + DENMRenderObject denm(*msg); // 5 leap seconds in 2023 if (!denm.validateFloats()) { setStatus( rviz_common::properties::StatusProperty::Error, "Topic", diff --git a/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp b/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp index efe979099..f82d34287 100644 --- a/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp +++ b/etsi_its_rviz_plugins/src/displays/DENM/denm_render_object.cpp @@ -5,14 +5,14 @@ namespace etsi_its_msgs namespace displays { - DENMRenderObject::DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm, uint16_t n_leap_seconds) { + DENMRenderObject::DENMRenderObject(etsi_its_denm_msgs::msg::DENM denm) { int zone; bool northp; geometry_msgs::msg::PointStamped p = etsi_its_denm_msgs::access::getUTMPosition(denm, zone, northp); header.frame_id = p.header.frame_id; - uint64_t nanosecs = etsi_its_denm_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_denm_msgs::access::getReferenceTime(denm), n_leap_seconds); + uint64_t nanosecs = etsi_its_denm_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_denm_msgs::access::getReferenceTime(denm)); header.stamp = rclcpp::Time(nanosecs); station_id = etsi_its_denm_msgs::access::getStationID(denm); From 2f5cddbd69bb720e5ad3c5be2f6cdf2994c19634 Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Wed, 4 Sep 2024 15:21:32 +0200 Subject: [PATCH 21/52] added some getters --- .../impl/cpm/cpm_ts_getters.h | 372 ++++++++++++++++-- 1 file changed, 342 insertions(+), 30 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 4e28cfab5..14ed4053c 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 @@ -35,7 +35,7 @@ namespace etsi_its_cpm_ts_msgs::access { #include - /** +/** * @brief Retrieves the station ID from the given CPM. * * This function extracts the station ID from the header of the provided CPM. @@ -43,61 +43,57 @@ 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 */ - inline TimestampIts getReferenceTime(const CollectivePerceptionMessage& cpm){ - return cpm.payload.management_container.reference_time; - } +inline TimestampIts getReferenceTime(const CollectivePerceptionMessage &cpm) { + return cpm.payload.management_container.reference_time; +} - /** +/** * @brief Get the ReferenceTime-Value * * @param cpm CPM to get the ReferenceTime-Value from * @return uint64_t the ReferenceTime-Value */ - inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage& cpm){ - return getReferenceTime(cpm).value; - } +inline uint64_t getReferenceTimeValue(const CollectivePerceptionMessage &cpm) { return getReferenceTime(cpm).value; } - /** +/** * @brief Get the Latitude value of CPM * * @param cpm CPM to get the Latitude value from * @return Latitude value in degree as decimal number */ - inline double getLatitude(const CollectivePerceptionMessage& cpm){ - return getLatitude(cpm.payload.management_container.reference_position.latitude); - } +inline double getLatitude(const CollectivePerceptionMessage &cpm) { + return getLatitude(cpm.payload.management_container.reference_position.latitude); +} - /** +/** * @brief Get the Longitude value of CPM * * @param cpm CPM to get the Longitude value from * @return Longitude value in degree as decimal number */ - inline double getLongitude(const CollectivePerceptionMessage& cpm){ - return getLongitude(cpm.payload.management_container.reference_position.longitude); - } +inline double getLongitude(const CollectivePerceptionMessage &cpm) { + return getLongitude(cpm.payload.management_container.reference_position.longitude); +} - /** +/** * @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 */ - inline double getAltitude(const CollectivePerceptionMessage& cpm){ - return getAltitude(cpm.payload.management_container.reference_position.altitude); - } +inline double getAltitude(const CollectivePerceptionMessage &cpm) { + return getAltitude(cpm.payload.management_container.reference_position.altitude); +} - /** +/** * @brief Get the UTM Position defined within the BasicContainer of the CPM * * The position is transformed into UTM by using GeographicLib::UTMUPS @@ -108,8 +104,324 @@ namespace etsi_its_cpm_ts_msgs::access { * @param[out] northp hemisphere (true means north, false means south) * @return gm::PointStamped geometry_msgs::PointStamped of the given position */ - inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage& cpm, int& zone, bool& northp){ - return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); - } - -} // namespace etsi_its_cpm_ts_msgs::access +inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) { + return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); +} + +/** + * @brief Get the number of perceived objects in the CPM + * + * @param cpm CPM to get the number of perceived objects from + * @return int the number of perceived objects + */ +inline int getNumberOfPerceivedObjects(const etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage &cpm) { + int number = cpm.payload.cpm_containers.value.array[0] + .container_data.perceived_object_container.number_of_perceived_objects.value; + return number; +} + +// Getters for the PerceivedObject + +/** + * @brief Get the PerceivedObject from the CPM + * + * @param cpm CPM to get the PerceivedObject from + * @param number_of_object the number of the object to get + * @return PerceivedObject the PerceivedObject + */ +inline etsi_its_cpm_ts_msgs::PerceivedObject getPerceivedObject(const CollectivePerceptionMessage &cpm, + int number_of_object) { + return cpm.payload.cpm_containers.value.array[0] + .container_data.perceived_object_container.perceived_objects.array[number_of_object]; +} + +//inline gm::PoseWithCovariance getPoseWithCovarianceOfPerceivedObject(const PerceivedObject &object) {} + +/** + * @brief Get the pose of the PerceivedObject + * + * @param object PerceivedObject to get the pose from + * @return gm::pose the pose of the PerceivedObject + */ +inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) { + geometry_msgs::msg::Pose pose; + pose.position = getPositionOfPerceivedObject(object); + pose.orientation = getOrientationOfPerceivedObject(object); + return pose; +} + +/** + * @brief Get the position of the PerceivedObject + * + * @param object PerceivedObject to get the position from + * @return gm::Point the position of the PerceivedObject + */ +inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) { + geometry_msgs::msg::Point point; + point.x = getXPositionOfPerceivedObject(object); + point.y = getYPositionOfPerceivedObject(object); + point.z = getZPositionOfPerceivedObject(object); + return point; +} + +/** + * @brief Get the x-coordinate of the PerceivedObject + * + * @param object PerceivedObject to get the x-coordinate from + * @return double the x-coordinate of the PerceivedObject + */ +inline double getXPositionOfPerceivedObject(const PerceivedObject &object) { + double altitude = object.position.z_coordinate.value.value / 100.0; + return altitude; +} + +/** + * @brief Get the y-coordinate of the PerceivedObject + * + * @param object PerceivedObject to get the y-coordinate from + * @return double the y-coordinate of the PerceivedObject + */ +inline double getYPositionOfPerceivedObject(const PerceivedObject &object) { + double longitude = object.position.y_coordinate.value.value / 100.0; + return longitude; +} + +/** + * @brief Get the z-coordinate of the PerceivedObject + * + * @param object PerceivedObject to get the z-coordinate from + * @return double the z-coordinate of the PerceivedObject + */ +inline double getZPositionOfPerceivedObject(const PerceivedObject &object) { + double latitude = object.position.x_coordinate.value.value / 100.0; + return latitude; +} + +/** + * @brief Get the orientation of the PerceivedObject + * + * @param object PerceivedObject to get the orientation from + * @return gm::Quaternion the orientation of the PerceivedObject + */ +inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) { + geometry_msgs::msg::Quaternion q; + + double yaw = getYawOfObject(object); + double pitch = getPitchOfObject(object); + double roll = getRollOfObject(object); + + yaw = yaw * M_PI / 180; + pitch = pitch * M_PI / 180; + roll = roll * M_PI / 180; + + // Abbreviations for the various angular functions + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + q.w = cy * cp * cr + sy * sp * sr; + q.x = cy * cp * sr - sy * sp * cr; + q.y = sy * cp * sr + cy * sp * cr; + q.z = sy * cp * cr - cy * sp * sr; + return q; +} + +/** + * @brief Get the roll of the PerceivedObject + * + * @param object PerceivedObject to get the roll from + * @return double the roll of the PerceivedObject + */ +inline double getRollOfPerceivedObject(const PerceivedObject &object) { + double roll = object.angles.x_angle.value.value / 100.0; + return roll; +} + +/** + * @brief Get the pitch of the PerceivedObject + * + * @param object PerceivedObject to get the pitch from + * @return double the pitch of the PerceivedObject + */ +inline double getPitchOfPerceivedObject(const PerceivedObject &object) { + double pitch = object.angles.y_angle.value.value / 100.0; + return pitch; +} + +/** + * @brief Get the yaw of the PerceivedObject + * + * @param object PerceivedObject to get the yaw from + * @return double the yaw of the PerceivedObject + */ +inline double getYawOfPerceivedObject(const PerceivedObject &object) { + double yaw = object.angles.z_angle.value.value / 100.0; + return yaw; +} + +/** + * @brief Get the yaw rate of the PerceivedObject + * + * @param object PerceivedObject to get the yaw rate from + * @return double the yaw rate of the PerceivedObject + */ +inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {} + + +/** + * @brief Get the velocity of the PerceivedObject + * + * @param object PerceivedObject to get the velocity from + * @return gm::Vector3 the velocity of the PerceivedObject + */ +inline gm::Vector3 getVelocityOfPerceivedObject(const PerceivedObject &object) { + geometry_msgs::msg::Vector3 velocity; + velocity.x = getXVelocityOfPerceivedObject; + velocity.y = getYVelocityOfPerceivedObject; + velocity.z = getZVelocityOfPerceivedObject; + return velocity; +} + +/** + * @brief Get the x-velocity of the PerceivedObject + * + * @param object PerceivedObject to get the x-velocity from + * @return double the x-velocity of the PerceivedObject + */ +inline double getXVelocityOfPerceivedObject(const PerceivedObject &object) { + + double velocity_x = object.velocity.cartesian_velocity.x_velocity.value.value; + return velocity_x; +} + +/** + * @brief Get the y-velocity of the PerceivedObject + * + * @param object PerceivedObject to get the y-velocity from + * @return double the y-velocity of the PerceivedObject + */ +inline double getYVelocityOfPerceivedObject(const PerceivedObject &object) { + double velocity_y = object.velocity.cartesian_velocity.y_velocity.value.value; + return velocity_y; +} + +/** + * @brief Get the z-velocity of the PerceivedObject + * + * @param object PerceivedObject to get the z-velocity from + * @return double the z-velocity of the PerceivedObject + */ +inline double getZVelocityOfPerceivedObject(const PerceivedObject &object) { + double velocity_z = object.velocity.cartesian_velocity.z_velocity.value.value; + return velocity_z; +} + +/** + * @brief Get the acceleration of the PerceivedObject + * + * @param object PerceivedObject to get the acceleration from + * @return gm::Vector3 the acceleration of the PerceivedObject + */ +inline gm::Vector3 getAccelerationOfPerceivedObject(const PerceivedObject &object) { + geometry_msgs::msg::Vector3 acceleration; + acceleration.x = getXAccelerationOfPerceivedObject; + acceleration.y = getYAccelerationOfPerceivedObject; + acceleration.z = getZAccelerationOfPerceivedObject; + return acceleration; +} + +/** + * @brief Get the x-acceleration of the PerceivedObject + * + * @param object PerceivedObject to get the x-acceleration from + * @return double the x-acceleration of the PerceivedObject + */ +inline double getXAccelerationOfPerceivedObject(const PerceivedObject &object) { + double acceleration_x = object.acceleration.cartesian_acceleration.x_acceleration.value.value; + return acceleration_x; +} + +/** + * @brief Get the y-acceleration of the PerceivedObject + * + * @param object PerceivedObject to get the y-acceleration from + * @return double the y-acceleration of the PerceivedObject + */ +inline double getYAccelerationOfPerceivedObject(const PerceivedObject &object) { + double acceleration_y = object.acceleration.cartesian_acceleration.y_acceleration.value.value; + return acceleration_y; +} + +/** + * @brief Get the z-acceleration of the PerceivedObject + * + * @param object PerceivedObject to get the z-acceleration from + * @return double the z-acceleration of the PerceivedObject + */ +inline double getZAccelerationOfPerceivedObject(const PerceivedObject &object) { + double acceleration_z = object.acceleration.cartesian_acceleration.z_acceleration.value.value; + return acceleration_z; +} + +/** + * @brief Get the dimensions of the PerceivedObject + * + * @param object PerceivedObject to get the dimensions from + * @return gm::Vector3 the dimensions of the PerceivedObject + */ +inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) { + geometry_msgs::msg::Vector3 dimensions; + dimensions.x = getXDimensionOfPerceivedObject(object); + dimensions.y = getYDimensionOfPerceivedObject(object); + dimensions.z = getZDimensionOfPerceivedObject(object); + return dimensions; +} + +/** + * @brief Get the x-dimension of the PerceivedObject + * + * @param object PerceivedObject to get the x-dimension from + * @return double the x-dimension of the PerceivedObject + */ +inline double getXDimensionOfPerceivedObject(const PerceivedObject &object) { + double x_dimension = object.object_dimension_x.value.value / 10.0; + return x_dimension; +} + +/** + * @brief Get the y-dimension of the PerceivedObject + * + * @param object PerceivedObject to get the y-dimension from + * @return double the y-dimension of the PerceivedObject + */ +inline double getYDimensionOfPerceivedObject(const PerceivedObject &object) { + double y_dimension = object.object_dimension_y.value.value / 10.0; + return y_dimension; +} + +/** + * @brief Get the z-dimension of the PerceivedObject + * + * @param object PerceivedObject to get the z-dimension from + * @return double the z-dimension of the PerceivedObject + */ +inline double getZDimensionOfPerceivedObject(const PerceivedObject &object) { + double z_dimension = object.object_dimension_z.value.value / 10.0; + return z_dimension; +} + +/** + * @brief Get the id of the PerceivedObject + * + * @param object PerceivedObject to get the id from + * @return uint16_t the id of the PerceivedObject + */ +inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { + int id = object.object_id.value; + return id; +} + +} // namespace etsi_its_cpm_ts_msgs::access From 2d8a680c2ac9f63986c772699d46ffeeccffdd76 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Wed, 4 Sep 2024 18:48:39 +0200 Subject: [PATCH 22/52] improve CPM getter functions --- etsi_its_msgs_utils/CMakeLists.txt | 7 +- .../etsi_its_msgs_utils/cpm_ts_access.h | 4 + .../etsi_its_msgs_utils/cpm_ts_access.hpp | 4 + .../impl/cpm/cpm_ts_getters.h | 387 ++++++------------ etsi_its_msgs_utils/package.xml | 1 + 5 files changed, 136 insertions(+), 267 deletions(-) diff --git a/etsi_its_msgs_utils/CMakeLists.txt b/etsi_its_msgs_utils/CMakeLists.txt index 235f10320..579bca57b 100644 --- a/etsi_its_msgs_utils/CMakeLists.txt +++ b/etsi_its_msgs_utils/CMakeLists.txt @@ -13,6 +13,7 @@ if(${ROS_VERSION} EQUAL 2) find_package(etsi_its_msgs REQUIRED) find_package(GeographicLib REQUIRED) find_package(geometry_msgs REQUIRED) + find_package(tf2_geometry_msgs REQUIRED) add_library(${PROJECT_NAME} INTERFACE) @@ -25,10 +26,11 @@ if(${ROS_VERSION} EQUAL 2) ${etsi_its_msgs_TARGETS} ${GeographicLib_LIBRARIES} ${geometry_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} ) ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) - ament_export_dependencies(etsi_its_msgs geometry_msgs) + ament_export_dependencies(etsi_its_msgs geometry_msgs, tf2_geometry_msgs) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} @@ -53,6 +55,7 @@ if(${ROS_VERSION} EQUAL 2) etsi_its_msgs GeographicLib geometry_msgs + tf2_geometry_msgs ) endif() @@ -66,6 +69,7 @@ elseif(${ROS_VERSION} EQUAL 1) find_package(catkin REQUIRED COMPONENTS etsi_its_msgs geometry_msgs + tf2_geometry_msgs ) find_package(GeographicLib REQUIRED) @@ -76,6 +80,7 @@ elseif(${ROS_VERSION} EQUAL 1) CATKIN_DEPENDS etsi_its_msgs geometry_msgs + tf2_geometry_msgs ) include_directories( diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h index 5c8fb5724..b009be08b 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h @@ -34,7 +34,11 @@ SOFTWARE. // Messages #include #include +#include +#include +#include #include +#include namespace etsi_its_cpm_ts_msgs { namespace gm = geometry_msgs; diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp index 9c76c8ada..639fecfd8 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp @@ -34,7 +34,11 @@ SOFTWARE. // Messages #include #include +#include +#include +#include #include +#include namespace etsi_its_cpm_ts_msgs { using namespace msg; 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 14ed4053c..05c9bfedc 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,7 +43,9 @@ 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 @@ -94,7 +96,7 @@ inline double getAltitude(const CollectivePerceptionMessage &cpm) { } /** - * @brief Get the UTM Position defined within the BasicContainer of the CPM + * @brief Get the UTM Position defined within the ManagementContainer of the CPM * * The position is transformed into UTM by using GeographicLib::UTMUPS * The altitude value is directly used as z-Coordinate @@ -108,270 +110,167 @@ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, i return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); } -/** - * @brief Get the number of perceived objects in the CPM - * - * @param cpm CPM to get the number of perceived objects from - * @return int the number of perceived objects - */ -inline int getNumberOfPerceivedObjects(const etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage &cpm) { - int number = cpm.payload.cpm_containers.value.array[0] - .container_data.perceived_object_container.number_of_perceived_objects.value; - return number; +inline std::vector getCpmContainerIds(const CollectivePerceptionMessage &cpm) { + std::vector 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; } -// Getters for the PerceivedObject +inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cpm, const CpmContainerId 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"); +} -/** - * @brief Get the PerceivedObject from the CPM - * - * @param cpm CPM to get the PerceivedObject from - * @param number_of_object the number of the object to get - * @return PerceivedObject the PerceivedObject - */ -inline etsi_its_cpm_ts_msgs::PerceivedObject getPerceivedObject(const CollectivePerceptionMessage &cpm, - int number_of_object) { - return cpm.payload.cpm_containers.value.array[0] - .container_data.perceived_object_container.perceived_objects.array[number_of_object]; +inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm){ + getCpmContainer(cpm, CpmContainerId::PERCEIVED_OBJECT_CONTAINER); } -//inline gm::PoseWithCovariance getPoseWithCovarianceOfPerceivedObject(const PerceivedObject &object) {} +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"); + } + uint8_t number = container.container_data.perceived_object_container.number_of_perceived_objects.value; + return number; +} -/** - * @brief Get the pose of the PerceivedObject - * - * @param object PerceivedObject to get the pose from - * @return gm::pose the pose of the PerceivedObject - */ -inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) { - geometry_msgs::msg::Pose pose; - pose.position = getPositionOfPerceivedObject(object); - pose.orientation = getOrientationOfPerceivedObject(object); - return pose; +inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) { + return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm)); } -/** - * @brief Get the position of the PerceivedObject - * - * @param object PerceivedObject to get the position from - * @return gm::Point the position of the PerceivedObject - */ -inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) { - geometry_msgs::msg::Point point; - point.x = getXPositionOfPerceivedObject(object); - point.y = getYPositionOfPerceivedObject(object); - point.z = getZPositionOfPerceivedObject(object); - return point; +// getters for the PerceivedObject + +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]; } -/** - * @brief Get the x-coordinate of the PerceivedObject - * - * @param object PerceivedObject to get the x-coordinate from - * @return double the x-coordinate of the PerceivedObject - */ -inline double getXPositionOfPerceivedObject(const PerceivedObject &object) { - double altitude = object.position.z_coordinate.value.value / 100.0; - return altitude; +inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { + return object.object_id.value; } -/** - * @brief Get the y-coordinate of the PerceivedObject - * - * @param object PerceivedObject to get the y-coordinate from - * @return double the y-coordinate of the PerceivedObject - */ -inline double getYPositionOfPerceivedObject(const PerceivedObject &object) { - double longitude = object.position.y_coordinate.value.value / 100.0; - return longitude; +inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) { + return coordinate.value.value / 100; } -/** - * @brief Get the z-coordinate of the PerceivedObject - * - * @param object PerceivedObject to get the z-coordinate from - * @return double the z-coordinate of the PerceivedObject - */ -inline double getZPositionOfPerceivedObject(const PerceivedObject &object) { - double latitude = object.position.x_coordinate.value.value / 100.0; - return latitude; +inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) { + return coordinate.confidence.value / 100; } -/** - * @brief Get the orientation of the PerceivedObject - * - * @param object PerceivedObject to get the orientation from - * @return gm::Quaternion the orientation of the PerceivedObject - */ -inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &object) { - geometry_msgs::msg::Quaternion q; - - double yaw = getYawOfObject(object); - double pitch = getPitchOfObject(object); - double roll = getRollOfObject(object); - - yaw = yaw * M_PI / 180; - pitch = pitch * M_PI / 180; - roll = roll * M_PI / 180; - - // Abbreviations for the various angular functions - double cy = cos(yaw * 0.5); - double sy = sin(yaw * 0.5); - double cp = cos(pitch * 0.5); - double sp = sin(pitch * 0.5); - double cr = cos(roll * 0.5); - double sr = sin(roll * 0.5); - - q.w = cy * cp * cr + sy * sp * sr; - q.x = cy * cp * sr - sy * sp * cr; - q.y = sy * cp * sr + cy * sp * cr; - q.z = sy * cp * cr - cy * sp * sr; - return q; +inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) { + gm::Point point; + point.x = getCartesianCoordinate(object.position.x_coordinate); + point.y = getCartesianCoordinate(object.position.y_coordinate); + if (object.position.z_coordinate_is_present) { + point.z = getCartesianCoordinate(object.position.z_coordinate); + } + return point; } -/** - * @brief Get the roll of the PerceivedObject - * - * @param object PerceivedObject to get the roll from - * @return double the roll of the PerceivedObject - */ -inline double getRollOfPerceivedObject(const PerceivedObject &object) { - double roll = object.angles.x_angle.value.value / 100.0; - return roll; +inline uint16_t getCartesianAngle(const CartesianAngle &angle) { + return angle.value.value / 10; } -/** - * @brief Get the pitch of the PerceivedObject - * - * @param object PerceivedObject to get the pitch from - * @return double the pitch of the PerceivedObject - */ -inline double getPitchOfPerceivedObject(const PerceivedObject &object) { - double pitch = object.angles.y_angle.value.value / 100.0; - return pitch; +inline uint8_t getCartesianAngleConfidence(const CartesianAngle &angle) { + return angle.confidence.value / 10; } -/** - * @brief Get the yaw of the PerceivedObject - * - * @param object PerceivedObject to get the yaw from - * @return double the yaw of the PerceivedObject - */ -inline double getYawOfPerceivedObject(const PerceivedObject &object) { - double yaw = object.angles.z_angle.value.value / 100.0; - return yaw; +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 = getCartesianAngle(object.angles.x_angle); + } + if (object.angles.y_angle_is_present) { + pitch = getCartesianAngle(object.angles.y_angle); + } + yaw = getCartesianAngle(object.angles.z_angle); + q.setRPY(roll, pitch, yaw); + + return tf2::toMsg(q); } -/** - * @brief Get the yaw rate of the PerceivedObject - * - * @param object PerceivedObject to get the yaw rate from - * @return double the yaw rate of the PerceivedObject - */ -inline double getYawRateOfPerceivedObject(const PerceivedObject &object) {} +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) {} +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; +} -/** - * @brief Get the velocity of the PerceivedObject - * - * @param object PerceivedObject to get the velocity from - * @return gm::Vector3 the velocity of the PerceivedObject - */ -inline gm::Vector3 getVelocityOfPerceivedObject(const PerceivedObject &object) { - geometry_msgs::msg::Vector3 velocity; - velocity.x = getXVelocityOfPerceivedObject; - velocity.y = getYVelocityOfPerceivedObject; - velocity.z = getZVelocityOfPerceivedObject; - return velocity; +inline int16_t getVelocityComponent(const VelocityComponent &velocity) { + return velocity.value.value / 100; } -/** - * @brief Get the x-velocity of the PerceivedObject - * - * @param object PerceivedObject to get the x-velocity from - * @return double the x-velocity of the PerceivedObject - */ -inline double getXVelocityOfPerceivedObject(const PerceivedObject &object) { +inline uint8_t getVelocityComponentConfidence(const VelocityComponent &velocity) { + return velocity.confidence.value / 100; +} - double velocity_x = object.velocity.cartesian_velocity.x_velocity.value.value; - return velocity_x; +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; } -/** - * @brief Get the y-velocity of the PerceivedObject - * - * @param object PerceivedObject to get the y-velocity from - * @return double the y-velocity of the PerceivedObject - */ -inline double getYVelocityOfPerceivedObject(const PerceivedObject &object) { - double velocity_y = object.velocity.cartesian_velocity.y_velocity.value.value; - return velocity_y; +inline int16_t getAccelerationComponent(const AccelerationComponent &acceleration) { + return acceleration.value.value / 10; } -/** - * @brief Get the z-velocity of the PerceivedObject - * - * @param object PerceivedObject to get the z-velocity from - * @return double the z-velocity of the PerceivedObject - */ -inline double getZVelocityOfPerceivedObject(const PerceivedObject &object) { - double velocity_z = object.velocity.cartesian_velocity.z_velocity.value.value; - return velocity_z; +inline uint8_t getAccelerationComponentConfidence(const AccelerationComponent &acceleration) { + return acceleration.confidence.value / 10; } -/** - * @brief Get the acceleration of the PerceivedObject - * - * @param object PerceivedObject to get the acceleration from - * @return gm::Vector3 the acceleration of the PerceivedObject - */ -inline gm::Vector3 getAccelerationOfPerceivedObject(const PerceivedObject &object) { - geometry_msgs::msg::Vector3 acceleration; - acceleration.x = getXAccelerationOfPerceivedObject; - acceleration.y = getYAccelerationOfPerceivedObject; - acceleration.z = getZAccelerationOfPerceivedObject; +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; } -/** - * @brief Get the x-acceleration of the PerceivedObject - * - * @param object PerceivedObject to get the x-acceleration from - * @return double the x-acceleration of the PerceivedObject - */ -inline double getXAccelerationOfPerceivedObject(const PerceivedObject &object) { - double acceleration_x = object.acceleration.cartesian_acceleration.x_acceleration.value.value; - return acceleration_x; +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; } -/** - * @brief Get the y-acceleration of the PerceivedObject - * - * @param object PerceivedObject to get the y-acceleration from - * @return double the y-acceleration of the PerceivedObject - */ -inline double getYAccelerationOfPerceivedObject(const PerceivedObject &object) { - double acceleration_y = object.acceleration.cartesian_acceleration.y_acceleration.value.value; - return acceleration_y; +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; } -/** - * @brief Get the z-acceleration of the PerceivedObject - * - * @param object PerceivedObject to get the z-acceleration from - * @return double the z-acceleration of the PerceivedObject - */ -inline double getZAccelerationOfPerceivedObject(const PerceivedObject &object) { - double acceleration_z = object.acceleration.cartesian_acceleration.z_acceleration.value.value; - return acceleration_z; +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; } -/** - * @brief Get the dimensions of the PerceivedObject - * - * @param object PerceivedObject to get the dimensions from - * @return gm::Vector3 the dimensions of the PerceivedObject - */ inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) { geometry_msgs::msg::Vector3 dimensions; dimensions.x = getXDimensionOfPerceivedObject(object); @@ -380,48 +279,4 @@ inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) return dimensions; } -/** - * @brief Get the x-dimension of the PerceivedObject - * - * @param object PerceivedObject to get the x-dimension from - * @return double the x-dimension of the PerceivedObject - */ -inline double getXDimensionOfPerceivedObject(const PerceivedObject &object) { - double x_dimension = object.object_dimension_x.value.value / 10.0; - return x_dimension; -} - -/** - * @brief Get the y-dimension of the PerceivedObject - * - * @param object PerceivedObject to get the y-dimension from - * @return double the y-dimension of the PerceivedObject - */ -inline double getYDimensionOfPerceivedObject(const PerceivedObject &object) { - double y_dimension = object.object_dimension_y.value.value / 10.0; - return y_dimension; -} - -/** - * @brief Get the z-dimension of the PerceivedObject - * - * @param object PerceivedObject to get the z-dimension from - * @return double the z-dimension of the PerceivedObject - */ -inline double getZDimensionOfPerceivedObject(const PerceivedObject &object) { - double z_dimension = object.object_dimension_z.value.value / 10.0; - return z_dimension; -} - -/** - * @brief Get the id of the PerceivedObject - * - * @param object PerceivedObject to get the id from - * @return uint16_t the id of the PerceivedObject - */ -inline uint16_t getIdOfPerceivedObject(const PerceivedObject &object) { - int id = object.object_id.value; - return id; -} - } // namespace etsi_its_cpm_ts_msgs::access diff --git a/etsi_its_msgs_utils/package.xml b/etsi_its_msgs_utils/package.xml index 6b7a79ece..4bb63447d 100644 --- a/etsi_its_msgs_utils/package.xml +++ b/etsi_its_msgs_utils/package.xml @@ -19,6 +19,7 @@ geographiclib geometry_msgs ros_environment + tf2_geometry_msgs catkin ament_cmake From 66d74d28e8bbf07f53beb18815e8a232a403c55d Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Wed, 4 Sep 2024 19:45:59 +0200 Subject: [PATCH 23/52] added getter for Object UTM position --- .../impl/cpm/cpm_ts_getters.h | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) 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 05c9bfedc..799daec56 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 @@ -279,4 +279,28 @@ inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) return dimensions; } +inline gm::Position getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object, int &zone, bool &is_northp) { + double ref_latitude = getLatitude(cpm); //addition needs to happen in wgs84 + double ref_longitude = getLongitude(cpm); + double ref_altitude = getAltitude(cpm); + + gm::PointStamped utm_point; + gm::Position relative_position = getPositionOfPerceivedObject(object); + double absolute_longitude = ref_longitude + relative_position.x; + double absolute_latitude = ref_latitude + relative_position.y; + utm_point.z = ref_altitude + relative_position.z; + + try { //cant use getUTMPosition here because its designed for the reference position + GeographicLib::UTMUPS::Forward(absolute_latitude, absolute_longitude, zone, is_northp, utm_point.point.x, utm_point.point.y); + std::string hemisphere; + if(northp) hemisphere="N"; + else hemisphere="S"; + utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere; + } catch (GeographicLib::GeographicErr& e) { + throw std::invalid_argument(e.what()); + } + return utm_point; + +} + } // namespace etsi_its_cpm_ts_msgs::access From c8e8717bd1d5ce9fa2d70903441f59dd63899198 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 5 Sep 2024 10:44:34 +0000 Subject: [PATCH 24/52] fix build of ws --- etsi_its_msgs_utils/CMakeLists.txt | 2 +- .../etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/etsi_its_msgs_utils/CMakeLists.txt b/etsi_its_msgs_utils/CMakeLists.txt index 579bca57b..9e377f8b2 100644 --- a/etsi_its_msgs_utils/CMakeLists.txt +++ b/etsi_its_msgs_utils/CMakeLists.txt @@ -30,7 +30,7 @@ if(${ROS_VERSION} EQUAL 2) ) ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) - ament_export_dependencies(etsi_its_msgs geometry_msgs, tf2_geometry_msgs) + ament_export_dependencies(etsi_its_msgs geometry_msgs tf2_geometry_msgs) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} 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 799daec56..c6abcd845 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 @@ -110,15 +110,15 @@ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, i return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); } -inline std::vector getCpmContainerIds(const CollectivePerceptionMessage &cpm) { - std::vector container_ids; +inline std::vector getCpmContainerIds(const CollectivePerceptionMessage &cpm) { + std::vector 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 CpmContainerId container_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]; @@ -279,21 +279,21 @@ inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) return dimensions; } -inline gm::Position getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object, int &zone, bool &is_northp) { +inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object, int &zone, bool &is_northp) { double ref_latitude = getLatitude(cpm); //addition needs to happen in wgs84 double ref_longitude = getLongitude(cpm); double ref_altitude = getAltitude(cpm); gm::PointStamped utm_point; - gm::Position relative_position = getPositionOfPerceivedObject(object); + gm::Point relative_position = getPositionOfPerceivedObject(object); double absolute_longitude = ref_longitude + relative_position.x; double absolute_latitude = ref_latitude + relative_position.y; - utm_point.z = ref_altitude + relative_position.z; + utm_point.point.z = ref_altitude + relative_position.z; try { //cant use getUTMPosition here because its designed for the reference position GeographicLib::UTMUPS::Forward(absolute_latitude, absolute_longitude, zone, is_northp, utm_point.point.x, utm_point.point.y); std::string hemisphere; - if(northp) hemisphere="N"; + if(is_northp) hemisphere="N"; else hemisphere="S"; utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere; } catch (GeographicLib::GeographicErr& e) { From 3dcd0bc67529d639605e64ba2f1687ee4655f25b Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 5 Sep 2024 11:05:37 +0000 Subject: [PATCH 25/52] fix UTM getter and setter for perceived objects --- .../impl/cpm/cpm_ts_getters.h | 50 ++++++++++--------- .../impl/cpm/cpm_ts_setters.h | 13 +++++ 2 files changed, 40 insertions(+), 23 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 c6abcd845..39fa723ad 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 @@ -104,12 +104,27 @@ inline double getAltitude(const CollectivePerceptionMessage &cpm) { * @param[in] cpm CPM to get the UTM Position from * @param[out] zone the UTM zone (zero means UPS) * @param[out] northp hemisphere (true means north, false means south) - * @return gm::PointStamped geometry_msgs::PointStamped of the given position + * @return geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id */ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) { return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); } +/** + * @brief Get the UTM Position defined within the ManagementContainer of the CPM + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param cpm CPM from which to extract the UTM position. + * @return geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id + */ +inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) { + int zone; + bool northp; + return getUTMPosition(cpm, zone, northp); +} + inline std::vector getCpmContainerIds(const CollectivePerceptionMessage &cpm) { std::vector container_ids; for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) { @@ -128,7 +143,7 @@ inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cp } inline WrappedCpmContainer getPerceivedObjectContainer(const CollectivePerceptionMessage &cpm){ - getCpmContainer(cpm, CpmContainerId::PERCEIVED_OBJECT_CONTAINER); + return getCpmContainer(cpm, CpmContainerId::PERCEIVED_OBJECT_CONTAINER); } inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) { @@ -272,35 +287,24 @@ inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) { } inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) { - geometry_msgs::msg::Vector3 dimensions; + gm::Vector3 dimensions; dimensions.x = getXDimensionOfPerceivedObject(object); dimensions.y = getYDimensionOfPerceivedObject(object); dimensions.z = getZDimensionOfPerceivedObject(object); return dimensions; } -inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object, int &zone, bool &is_northp) { - double ref_latitude = getLatitude(cpm); //addition needs to happen in wgs84 - double ref_longitude = getLongitude(cpm); - double ref_altitude = getAltitude(cpm); - - gm::PointStamped utm_point; +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); - double absolute_longitude = ref_longitude + relative_position.x; - double absolute_latitude = ref_latitude + relative_position.y; - utm_point.point.z = ref_altitude + relative_position.z; - - try { //cant use getUTMPosition here because its designed for the reference position - GeographicLib::UTMUPS::Forward(absolute_latitude, absolute_longitude, zone, is_northp, utm_point.point.x, utm_point.point.y); - std::string hemisphere; - if(is_northp) hemisphere="N"; - else hemisphere="S"; - utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere; - } catch (GeographicLib::GeographicErr& e) { - throw std::invalid_argument(e.what()); - } - return utm_point; + 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; } } // namespace etsi_its_cpm_ts_msgs::access 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 199b4bd1b..329c37f5f 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 @@ -128,6 +128,19 @@ namespace etsi_its_cpm_ts_msgs::access { } } + 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; + } + } + inline void setVelocityComponent(VelocityComponent& velocity, const int16_t value, const uint8_t confidence = SpeedConfidence::UNAVAILABLE){ // limit value range int16_t limited_value = std::min(VelocityComponentValue::NEGATIVE_OUT_OF_RANGE, std::max(VelocityComponentValue::POSITIVE_OUT_OF_RANGE, value)); From d7fd302b29f06ae73f1d344fa7aa77d3ece69e03 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 5 Sep 2024 14:17:10 +0200 Subject: [PATCH 26/52] add UTM getter without zone for CAM und DENM --- .../impl/cam/cam_getters_common.h | 15 +++++++++++++++ .../etsi_its_msgs_utils/impl/denm/denm_getters.h | 12 ++++++++++++ 2 files changed, 27 insertions(+) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h index 073b51523..645be472a 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h @@ -208,6 +208,21 @@ inline gm::PointStamped getUTMPosition(const CAM& cam, int& zone, bool& northp){ return getUTMPosition(cam.cam.cam_parameters.basic_container.reference_position, zone, northp); } +/** + * @brief Get the UTM Position defined within the BasicContainer of the CAM + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param[in] cam CAM to get the UTM Position from + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ +inline gm::PointStamped getUTMPosition(const CAM& cam){ + int zone; + bool northp; + return getUTMPosition(cam.cam.cam_parameters.basic_container.reference_position, zone, northp); +} + /** * @brief Get the Exterior Lights in form of bool vector * diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h index 50de03ff1..b4ba05735 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h @@ -199,6 +199,18 @@ namespace etsi_its_denm_msgs::access { return getUTMPosition(denm.denm.management.event_position, zone, northp); } + /** + * @brief + * + * @param denm DENM to get the UTM Position from + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ + inline gm::PointStamped getUTMPosition(const DENM& denm){ + int zone; + bool northp; + return getUTMPosition(denm.denm.management.event_position, zone, northp); + } + /** * @brief Get the Cause Code object * From 4b2cc6f6ed5729e31518e38dd46ad1462500242c Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 5 Sep 2024 17:34:27 +0200 Subject: [PATCH 27/52] add init from utm function for perceived objects --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 5 +++++ 1 file changed, 5 insertions(+) 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 329c37f5f..80f4eaf41 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 @@ -256,6 +256,11 @@ namespace etsi_its_cpm_ts_msgs::access { setMeasurementDeltaTimeOfPerceivedObject(object, delta_time); } + 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); + } + 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; From 0d20c4421a9756b7132754557bdf6b90f03ba318 Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Thu, 5 Sep 2024 18:30:37 +0200 Subject: [PATCH 28/52] fixed quaternion input for orientation in getters --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 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 39fa723ad..75182f9d0 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 @@ -203,12 +203,13 @@ inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &obj double roll{0}, pitch{0}, yaw{0}; if (object.angles.x_angle_is_present) { - roll = getCartesianAngle(object.angles.x_angle); + + roll = (getCartesianAngle(object.angles.x_angle) - 180) / 180 * M_PI; } if (object.angles.y_angle_is_present) { - pitch = getCartesianAngle(object.angles.y_angle); + pitch = (getCartesianAngle(object.angles.y_angle) - 180) / 180 * M_PI; } - yaw = getCartesianAngle(object.angles.z_angle); + yaw = (getCartesianAngle(object.angles.z_angle) - 180) / 180 * M_PI; q.setRPY(roll, pitch, yaw); return tf2::toMsg(q); From e713fa405debf9f10a53de97bb1fb7d3516bba4e Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Fri, 6 Sep 2024 16:36:55 +0200 Subject: [PATCH 29/52] fixed integer handling in orientation --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h | 6 +++--- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 4 ++-- 2 files changed, 5 insertions(+), 5 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 75182f9d0..be359b7fe 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 @@ -204,12 +204,12 @@ inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &obj if (object.angles.x_angle_is_present) { - roll = (getCartesianAngle(object.angles.x_angle) - 180) / 180 * M_PI; + roll = ((static_cast(getCartesianAngle(object.angles.x_angle)) - 180) / 180) * M_PI; } if (object.angles.y_angle_is_present) { - pitch = (getCartesianAngle(object.angles.y_angle) - 180) / 180 * M_PI; + pitch = ((static_cast(getCartesianAngle(object.angles.y_angle)) - 180) / 180) * M_PI; } - yaw = (getCartesianAngle(object.angles.z_angle) - 180) / 180 * M_PI; + yaw = ((static_cast(getCartesianAngle(object.angles.z_angle)) - 180) / 180) * M_PI; q.setRPY(roll, pitch, yaw); return tf2::toMsg(q); 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 80f4eaf41..383cbde90 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 @@ -191,9 +191,9 @@ namespace etsi_its_cpm_ts_msgs::access { 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; + double yaw_in_degrees = yaw * 180 / M_PI + 180; while (yaw_in_degrees > 360.0) yaw_in_degrees -= 360.0; - 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){ From 1fbc9095bee5567f1484f9e4a338b319d83f6e1a Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Fri, 6 Sep 2024 18:06:56 +0200 Subject: [PATCH 30/52] added getter for yaw, fixed min/max mixups --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h | 8 ++++++++ .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 6 +++--- 2 files changed, 11 insertions(+), 3 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 be359b7fe..f360aa766 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 @@ -215,6 +215,14 @@ inline gm::Quaternion getOrientationOfPerceivedObject(const PerceivedObject &obj 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 gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) { gm::Pose pose; pose.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 383cbde90..bcd70b678 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 @@ -143,7 +143,7 @@ namespace etsi_its_cpm_ts_msgs::access { inline void setVelocityComponent(VelocityComponent& velocity, const int16_t value, const uint8_t confidence = SpeedConfidence::UNAVAILABLE){ // limit value range - int16_t limited_value = std::min(VelocityComponentValue::NEGATIVE_OUT_OF_RANGE, std::max(VelocityComponentValue::POSITIVE_OUT_OF_RANGE, value)); + 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 @@ -167,7 +167,7 @@ namespace etsi_its_cpm_ts_msgs::access { inline void setAccelerationComponent(AccelerationComponent& acceleration, const int16_t value, const uint8_t confidence = AccelerationConfidence::UNAVAILABLE){ // limit value range - int16_t limited_value = std::min(AccelerationValue::NEGATIVE_OUT_OF_RANGE, std::max(AccelerationValue::POSITIVE_OUT_OF_RANGE, value)); + 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 @@ -207,7 +207,7 @@ namespace etsi_its_cpm_ts_msgs::access { 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::min(CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE, std::max(CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE, yaw_rate_in_degrees)); + 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; From 09c7529895e24150e57a6d18f7472746977ed5ee Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Wed, 11 Sep 2024 14:45:59 +0200 Subject: [PATCH 31/52] added structure for cpm rviz plugin --- etsi_its_rviz_plugins/CMakeLists.txt | 4 + .../include/displays/CPM/cpm_display.hpp | 92 +++++++ .../displays/CPM/cpm_render_object.hpp | 118 ++++++++ etsi_its_rviz_plugins/plugins_description.xml | 11 + .../src/displays/CPM/cpm_display.cpp | 252 ++++++++++++++++++ .../src/displays/CPM/cpm_render_object.cpp | 97 +++++++ 6 files changed, 574 insertions(+) create mode 100644 etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp create mode 100644 etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp create mode 100644 etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp create mode 100644 etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp diff --git a/etsi_its_rviz_plugins/CMakeLists.txt b/etsi_its_rviz_plugins/CMakeLists.txt index 07c3ed0d6..6a329d7f9 100644 --- a/etsi_its_rviz_plugins/CMakeLists.txt +++ b/etsi_its_rviz_plugins/CMakeLists.txt @@ -29,6 +29,8 @@ if(${ROS_VERSION} EQUAL 2) include/displays/CAM/cam_render_object.hpp include/displays/DENM/denm_display.hpp include/displays/DENM/denm_render_object.hpp + include/displays/CPM/cpm_display.hpp + include/displays/CPM/cpm_render_object.hpp ) foreach(header "${display_headers_to_moc}") @@ -40,6 +42,8 @@ if(${ROS_VERSION} EQUAL 2) src/displays/CAM/cam_render_object.cpp src/displays/DENM/denm_display.cpp src/displays/DENM/denm_render_object.cpp + src/displays/CPM/cpm_display.cpp + src/displays/CPM/cpm_render_object.cpp ) add_library(${PROJECT_NAME} SHARED diff --git a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp new file mode 100644 index 000000000..45fc60faa --- /dev/null +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -0,0 +1,92 @@ +/** ============================================================================ +MIT License + +Copyright (c) 2023-2024 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 "etsi_its_cpm_ts_msgs/msg/collective_perception_message.hpp" + +#include "displays/CPM/cpm_render_object.hpp" + +#include "rviz_common/ros_topic_display.hpp" +#include "rviz_rendering/objects/movable_text.hpp" +#include "rviz_rendering/objects/shape.hpp" + +#include + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz_common +{ +namespace properties +{ + class ColorProperty; + class FloatProperty; +} // namespace properties +} // namespace rviz_common + +namespace etsi_its_msgs +{ +namespace displays +{ + +/** + * @class CPMDisplay + * @brief Displays an etsi_its_cpm_msgs::CollectivePerceptionMessage + */ +class CPMDisplay : public + rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + CPMDisplay(); + ~CPMDisplay() override; + + void onInitialize() override; + + void reset() override; + +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_; + + rclcpp::Node::SharedPtr rviz_node_; + + // Properties + rviz_common::properties::BoolProperty *show_meta_, *show_station_id_, *show_speed_; + rviz_common::properties::FloatProperty *buffer_timeout_, *bb_scale_, *char_height_; + rviz_common::properties::ColorProperty *color_property_, *text_color_property_; + + std::unordered_map cpms_; + std::vector> bboxs_; + std::vector> texts_; +}; + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file 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 new file mode 100644 index 000000000..80f280592 --- /dev/null +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp @@ -0,0 +1,118 @@ +/** ============================================================================ +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= */ + +#include "etsi_its_cpm_ts_msgs/msg/collective_perception_message.hpp" +#include +#include +#include +#include + +#include +#include + +#include "rviz_common/validate_floats.hpp" + +namespace etsi_its_msgs +{ +namespace displays +{ + +/** + * @class CPMRenderObject + * @brief + */ +class CPMRenderObject +{ + public: + 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); + + /** + * @brief This function validates all float variables that are part of a CPMRenderObject + * + */ + bool validateFloats(); + + /** + * @brief Get age of CPM-object + * + * @param now reference point in time to calculate the age with + * @return age in seconds as double value + */ + double getAge(rclcpp::Time now); + + /** + * @brief Get header of CPM-object + * + * @return std_msgs::msg::Header + */ + std_msgs::msg::Header getHeader(); + + /** + * @brief Get the StationID of CPM-object + * + * @return int + */ + uint32_t getStationID(); + + /** + * @brief Get the StationType of CPM-object + * + * @return int + */ + int getStationType(); + + /** + * @brief Get pose of CPM-object + * + * @return geometry_msgs::msg::Pose + */ + geometry_msgs::msg::Pose getPose(); + + /** + * @brief Get dimensions of CPM-Object + * + * @return geometry_msgs::msg::Vector3 (x equals length, y equals width, z equals height) + */ + geometry_msgs::msg::Vector3 getDimensions(); + + /** + * @brief Get speed of CPM-object + * + * @return double + */ + double getSpeed(); + + private: + // member variables + std_msgs::msg::Header header; + uint32_t station_id; + int station_type; + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Vector3 dimensions; + double speed; + +}; + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file diff --git a/etsi_its_rviz_plugins/plugins_description.xml b/etsi_its_rviz_plugins/plugins_description.xml index 9517bced4..6ce44ad60 100644 --- a/etsi_its_rviz_plugins/plugins_description.xml +++ b/etsi_its_rviz_plugins/plugins_description.xml @@ -22,4 +22,15 @@ etsi_its_denm_msgs/msg/DENM + + + + Displays data from a etsi_its_cpm_ts_msgs::CollectivePerceptionMessage. + + etsi_its_cpm_ts_msgs/msg/CollectivePerceptionMessage + \ No newline at end of file diff --git a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp new file mode 100644 index 000000000..6fc8c0d5e --- /dev/null +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -0,0 +1,252 @@ +/** ============================================================================ +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= */ + +#include "displays/CPM/cpm_display.hpp" + +#include +#include +#include +#include +#include +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" +#include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/bool_property.hpp" + +#include "rviz_common/properties/parse_color.hpp" + +namespace etsi_its_msgs +{ +namespace displays +{ + +CPMDisplay::CPMDisplay() +{ + // General Properties + buffer_timeout_ = new rviz_common::properties::FloatProperty( + "Timeout", 0.1f, + "Time (in s) until objects disappear", this); + buffer_timeout_->setMin(0); + bb_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 1.0f, + "Scale of objects", this); + bb_scale_->setMin(0.01); + color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(25, 0, 255), + "Object color", this); + show_meta_ = new rviz_common::properties::BoolProperty("Metadata", true, + "Show metadata as text next to objects", this); + text_color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(25, 0, 255), + "Text color", show_meta_); + char_height_ = new rviz_common::properties::FloatProperty("Scale", 4.0, "Scale of text", show_meta_); + show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, + "Show StationID", show_meta_); + show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, + "Show speed", show_meta_); + +} + +CPMDisplay::~CPMDisplay() +{ + if (initialized() ) { + scene_manager_->destroyManualObject(manual_object_); + } +} + +void CPMDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + auto nodeAbstraction = context_->getRosNodeAbstraction().lock(); + rviz_node_ = nodeAbstraction->get_raw_node(); + + manual_object_ = scene_manager_->createManualObject(); + manual_object_->setDynamic(true); + scene_node_->attachObject(manual_object_); +} + +void CPMDisplay::reset() +{ + RTDClass::reset(); + manual_object_->clear(); +} + +void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) +{ + // Generate CPM render object from message + rclcpp::Time now = rviz_node_->now(); + uint64_t nanosecs = now.nanoseconds(); + if (nanosecs == 0) { + setStatus( + rviz_common::properties::StatusProperty::Warn, "Topic", + "Message received before clock got a valid time"); + return; + } + + CPMRenderObject cpm(*msg, now, getLeapSecondInsertionsSince2004(static_cast(now.seconds()))); + if (!cpm.validateFloats()) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + // Check if Station ID is already present in list + auto it = cpms_.find(cpm.getStationID()); + if (it != cpms_.end()) it->second = cpm; // Key exists, update the value + else cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); + + return; +} + +void CPMDisplay::update(float, float) +{ + // Check for outdated CPMs + for (auto it = cpms_.begin(); it != cpms_.end(); ) { + if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) { + it = cpms_.erase(it); + } + else { + ++it; + } + } + + // Render all valid cpms + bboxs_.clear(); + texts_.clear(); + for(auto it = cpms_.begin(); it != cpms_.end(); ++it) { + + CPMRenderObject cpm = it->second; + Ogre::Vector3 sn_position; + Ogre::Quaternion sn_orientation; + if (!context_->getFrameManager()->getTransform(cpm.getHeader(), sn_position, sn_orientation)) { + // Check if transform exists + setMissingTransformToFixedFrame(cpm.getHeader().frame_id); + return; + } + // We don't want to use the transform in sn_position and sn_orientation though, because they are only in float precision. + // So we get the transfrom manually from tf2: + std::string fixed_frame = fixed_frame_.toStdString(); + geometry_msgs::msg::PoseStamped pose_origin; + pose_origin.header = cpm.getHeader(); + pose_origin.pose.position.x = 0; + pose_origin.pose.position.y = 0; + pose_origin.pose.position.z = 0; + pose_origin.pose.orientation.w = 1; + pose_origin.pose.orientation.x = 0; + pose_origin.pose.orientation.y = 0; + pose_origin.pose.orientation.z = 0; + geometry_msgs::msg::PoseStamped pose_fixed_frame = context_->getTransformationManager()->getCurrentTransformer()->transform(pose_origin, fixed_frame); + geometry_msgs::msg::TransformStamped transform_to_fixed_frame; + transform_to_fixed_frame.header = pose_fixed_frame.header; + transform_to_fixed_frame.transform.translation.x = pose_fixed_frame.pose.position.x; + transform_to_fixed_frame.transform.translation.y = pose_fixed_frame.pose.position.y; + transform_to_fixed_frame.transform.translation.z = pose_fixed_frame.pose.position.z; + transform_to_fixed_frame.transform.rotation = pose_fixed_frame.pose.orientation; + + setTransformOk(); + + // set pose of scene node to origin (=fixed frame)! + scene_node_->setPosition(Ogre::Vector3{0.0f, 0.0f, 0.0f}); + scene_node_->setOrientation(Ogre::Quaternion{1.0f, 0.0f, 0.0f, 0.0f}); + + auto child_scene_node = scene_node_->createChildSceneNode(); + // Set position of scene node to the position relative to the fixed frame + geometry_msgs::msg::Pose pose = cpm.getPose(); + geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(); + tf2::doTransform(pose, pose, transform_to_fixed_frame); + Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); + Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + + if(3 <= cpm.getStationType() && cpm.getStationType() <= 11) + { + // If the station type of the originating ITS-S is set to one out of the values 3 to 11 + // the reference point shall be the ground position of the centre of the front side of + // the bounding box of the vehicle. + // https://www.etsi.org/deliver/etsi_en/302600_302699/30263702/01.03.01_30/en_30263702v010301v.pdf + tf2::Quaternion q; + tf2::fromMsg(pose.orientation, q); + tf2::Matrix3x3 m(q); + tf2::Vector3 v(-dimensions.x/2.0, 0.0, dimensions.z/2.0); + v = m*v; + position.x += v.x(); + position.y += v.y(); + position.z += v.z(); + } + + // set pose of child scene node of bounding-box + child_scene_node->setPosition(position); + child_scene_node->setOrientation(orientation); + + // create boundind-box object + std::shared_ptr bbox = std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); + + // set the dimensions of bounding box + Ogre::Vector3 dims; + double scale = bb_scale_->getFloat(); + dims.x = dimensions.x*scale; + dims.y = dimensions.y*scale; + dims.z = dimensions.z*scale; + bbox->setScale(dims); + // set the color of bounding box + Ogre::ColourValue bb_color = rviz_common::properties::qtToOgre(color_property_->getColor()); + bbox->setColor(bb_color); + bboxs_.push_back(bbox); + + // Visualize meta-information as text + if(show_meta_->getBool()) { + std::string text; + if(show_station_id_->getBool()) { + text+="StationID: " + std::to_string(cpm.getStationID()); + text+="\n"; + } + if(show_speed_->getBool()) { + text+="Speed: " + std::to_string((int)(cpm.getSpeed()*3.6)) + " km/h"; + } + if(!text.size()) return; + std::shared_ptr text_render = std::make_shared(text, "Liberation Sans", char_height_->getFloat()); + double height = dims.z; + height+=text_render->getBoundingRadius(); + Ogre::Vector3 offs(0.0, 0.0, height); + // There is a bug in rviz_rendering::MovableText::setGlobalTranslation https://github.com/ros2/rviz/issues/974 + text_render->setGlobalTranslation(offs); + Ogre::ColourValue text_color = rviz_common::properties::qtToOgre(text_color_property_->getColor()); + text_render->setColor(text_color); + child_scene_node->attachObject(text_render.get()); + texts_.push_back(text_render); + } + } +} + +} // namespace displays +} // namespace etsi_its_msgs + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(etsi_its_msgs::displays::CPMDisplay, rviz_common::Display) \ No newline at end of file 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 new file mode 100644 index 000000000..116172a55 --- /dev/null +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp @@ -0,0 +1,97 @@ +/** ============================================================================ +MIT License + +Copyright (c) 2023-2024 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. +============================================================================= */ + +#include "displays/CPM/cpm_render_object.hpp" + +namespace etsi_its_msgs +{ +namespace displays +{ + + CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, uint16_t n_leap_seconds) { + + // int zone; + // bool northp; + // geometry_msgs::msg::PointStamped p = etsi_its_cpm_ts_msgs::access::getUTMPosition(cpm, zone, northp); + // header.frame_id = p.header.frame_id; + + // uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromGenerationDeltaTime(etsi_its_cpm_msgs::access::getGenerationDeltaTime(cpm), receive_time.nanoseconds(), n_leap_seconds); + // header.stamp = rclcpp::Time(nanosecs); + + // station_id = etsi_its_cpm_msgs::access::getStationID(cpm); + // station_type = etsi_its_cpm_msgs::access::getStationType(cpm); + + // double heading = (90-etsi_its_cpm_msgs::access::getHeading(cpm))*M_PI/180.0; // 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + // while(heading<0) heading+=2*M_PI; + // pose.position = p.point; + // tf2::Quaternion orientation; + // orientation.setRPY(0.0, 0.0, heading); + // pose.orientation = tf2::toMsg(orientation); + + // dimensions.x = etsi_its_cpm_msgs::access::getVehicleLength(cpm); + // dimensions.y = etsi_its_cpm_msgs::access::getVehicleWidth(cpm); + // dimensions.z = 1.6; + + // speed = etsi_its_cpm_msgs::access::getSpeed(cpm); + } + + bool CPMRenderObject::validateFloats() { + bool valid = true; + valid = valid && rviz_common::validateFloats(pose); + valid = valid && rviz_common::validateFloats(dimensions); + valid = valid && rviz_common::validateFloats(speed); + return valid; + } + + double CPMRenderObject::getAge(rclcpp::Time now) { + return (now-header.stamp).seconds(); + } + + std_msgs::msg::Header CPMRenderObject::getHeader() { + return header; + } + + uint32_t CPMRenderObject::getStationID() { + return station_id; + } + + int CPMRenderObject::getStationType() { + return station_type; + } + + geometry_msgs::msg::Pose CPMRenderObject::getPose() { + return pose; + } + + geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions() { + + return dimensions; + } + + double CPMRenderObject::getSpeed() { + return speed; + } + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file From 833cd2252edb63c805f33bda3e969921879aa859 Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Wed, 11 Sep 2024 15:36:40 +0200 Subject: [PATCH 32/52] implementing cpm rendering --- .../displays/CPM/cpm_render_object.hpp | 3 +- .../src/displays/CPM/cpm_render_object.cpp | 53 ++++++++++--------- 2 files changed, 31 insertions(+), 25 deletions(-) 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 80f280592..253f2f70c 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 @@ -110,7 +110,8 @@ class CPMRenderObject int station_type; geometry_msgs::msg::Pose pose; geometry_msgs::msg::Vector3 dimensions; - double speed; + geometry_msgs::msg::Vector3 velocity; + geometry_msgs::msg::Quaternion orientation; }; 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 116172a55..51359f889 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 @@ -31,36 +31,41 @@ namespace displays CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, uint16_t n_leap_seconds) { - // int zone; - // bool northp; - // geometry_msgs::msg::PointStamped p = etsi_its_cpm_ts_msgs::access::getUTMPosition(cpm, zone, northp); - // header.frame_id = p.header.frame_id; + int number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(cpm); - // uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromGenerationDeltaTime(etsi_its_cpm_msgs::access::getGenerationDeltaTime(cpm), receive_time.nanoseconds(), n_leap_seconds); - // header.stamp = rclcpp::Time(nanosecs); + for(int i=0; i Date: Wed, 11 Sep 2024 15:42:58 +0200 Subject: [PATCH 33/52] small changes --- .../include/displays/CPM/cpm_render_object.hpp | 2 +- etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp | 2 +- etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) 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 253f2f70c..9d3a1916b 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 @@ -101,7 +101,7 @@ class CPMRenderObject * * @return double */ - double getSpeed(); + geometry_msgs::msg::Vector3 getSpeed(); private: // member variables 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 6fc8c0d5e..2a2478970 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -228,7 +228,7 @@ void CPMDisplay::update(float, float) text+="\n"; } if(show_speed_->getBool()) { - text+="Speed: " + std::to_string((int)(cpm.getSpeed()*3.6)) + " km/h"; + // text+="Speed: " + std::to_string((int)(cpm.getSpeed()*3.6)) + " km/h"; } if(!text.size()) return; std::shared_ptr text_render = std::make_shared(text, "Liberation Sans", char_height_->getFloat()); 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 51359f889..05df8bd92 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 @@ -35,11 +35,11 @@ namespace displays for(int i=0; i Date: Fri, 13 Sep 2024 18:58:13 +0200 Subject: [PATCH 34/52] structural changes --- .../include/displays/CPM/cpm_display.hpp | 2 + .../displays/CPM/cpm_render_object.hpp | 21 ++++---- .../src/displays/CPM/cpm_display.cpp | 49 +++++++------------ .../src/displays/CPM/cpm_render_object.cpp | 34 ++++++++----- 4 files changed, 54 insertions(+), 52 deletions(-) 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 45fc60faa..0b2acda39 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -69,6 +69,7 @@ class CPMDisplay : public void onInitialize() override; void reset() override; + CPMRenderObject cpm; protected: void processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) override; @@ -84,6 +85,7 @@ class CPMDisplay : public rviz_common::properties::ColorProperty *color_property_, *text_color_property_; std::unordered_map cpms_; + 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 9d3a1916b..4af99a131 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,6 +45,7 @@ namespace displays class CPMRenderObject { public: + CPMRenderObject() = default; // Default constructor 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); /** @@ -75,39 +76,39 @@ class CPMRenderObject */ uint32_t getStationID(); - /** - * @brief Get the StationType of CPM-object - * - * @return int - */ - int getStationType(); - /** * @brief Get pose of CPM-object * * @return geometry_msgs::msg::Pose */ - geometry_msgs::msg::Pose getPose(); + geometry_msgs::msg::Pose getPose(int i); /** * @brief Get dimensions of CPM-Object * * @return geometry_msgs::msg::Vector3 (x equals length, y equals width, z equals height) */ - geometry_msgs::msg::Vector3 getDimensions(); + geometry_msgs::msg::Vector3 getDimensions(int i); /** * @brief Get speed of CPM-object * * @return double */ - geometry_msgs::msg::Vector3 getSpeed(); + geometry_msgs::msg::Vector3 getSpeed(int i); + + int getNumberOfObjects(); private: // member variables std_msgs::msg::Header header; + uint8_t number_of_objects; uint32_t station_id; int station_type; + //declare an array of poses, dimensions and velocities + std::vector poses; + std::vector dimensionss; + std::vector velocities; 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 2a2478970..66d23a6c8 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -128,27 +128,25 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM void CPMDisplay::update(float, float) { - // Check for outdated CPMs - for (auto it = cpms_.begin(); it != cpms_.end(); ) { - if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) { - it = cpms_.erase(it); - } - else { - ++it; - } - } - - // Render all valid cpms + bboxs_.clear(); texts_.clear(); - for(auto it = cpms_.begin(); it != cpms_.end(); ++it) { - CPMRenderObject cpm = it->second; + + //info logger for the number of perceived objects + u_int8_t number_of_objects = cpm.getNumberOfObjects(); + //info logger for the number of perceived objects + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Number of perceived objects: %d", number_of_objects); + + //begin loop over all perceived objects + for(int i=0; i<5; i++) { + Ogre::Vector3 sn_position; Ogre::Quaternion sn_orientation; if (!context_->getFrameManager()->getTransform(cpm.getHeader(), sn_position, sn_orientation)) { // Check if transform exists - setMissingTransformToFixedFrame(cpm.getHeader().frame_id); + // setMissingTransformToFixedFrame(cpm.getHeader().frame_id); + setMissingTransformToFixedFrame("map"); return; } // We don't want to use the transform in sn_position and sn_orientation though, because they are only in float precision. @@ -179,27 +177,15 @@ void CPMDisplay::update(float, float) auto child_scene_node = scene_node_->createChildSceneNode(); // Set position of scene node to the position relative to the fixed frame - geometry_msgs::msg::Pose pose = cpm.getPose(); - geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(); + geometry_msgs::msg::Pose pose = cpm.getPose(i); + + geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(i); tf2::doTransform(pose, pose, transform_to_fixed_frame); Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - if(3 <= cpm.getStationType() && cpm.getStationType() <= 11) - { - // If the station type of the originating ITS-S is set to one out of the values 3 to 11 - // the reference point shall be the ground position of the centre of the front side of - // the bounding box of the vehicle. - // https://www.etsi.org/deliver/etsi_en/302600_302699/30263702/01.03.01_30/en_30263702v010301v.pdf - tf2::Quaternion q; - tf2::fromMsg(pose.orientation, q); - tf2::Matrix3x3 m(q); - tf2::Vector3 v(-dimensions.x/2.0, 0.0, dimensions.z/2.0); - v = m*v; - position.x += v.x(); - position.y += v.y(); - position.z += v.z(); - } + //info logger for the position of the object + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Position of the object: x: %f, y: %f, z: %f", position.x, position.y, position.z); // set pose of child scene node of bounding-box child_scene_node->setPosition(position); @@ -242,6 +228,7 @@ void CPMDisplay::update(float, float) child_scene_node->attachObject(text_render.get()); texts_.push_back(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 05df8bd92..505970b3a 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 @@ -31,33 +31,41 @@ namespace displays CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, uint16_t n_leap_seconds) { - int number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(cpm); + uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(cpm); + + + geometry_msgs::msg::Pose poses[number_of_objects]; + geometry_msgs::msg::Vector3 dimensionss[number_of_objects]; + geometry_msgs::msg::Vector3 velocities[number_of_objects]; for(int i=0; i Date: Mon, 16 Sep 2024 09:41:47 +0200 Subject: [PATCH 35/52] formating / bug fixes --- .../include/displays/CPM/cpm_display.hpp | 1 - .../displays/CPM/cpm_render_object.hpp | 2 +- .../src/displays/CPM/cpm_display.cpp | 273 +++++++++--------- .../src/displays/CPM/cpm_render_object.cpp | 72 ++--- 4 files changed, 158 insertions(+), 190 deletions(-) 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 0b2acda39..9628fba15 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -69,7 +69,6 @@ class CPMDisplay : public void onInitialize() override; void reset() override; - CPMRenderObject cpm; protected: void processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) override; 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 4af99a131..689cf68eb 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,7 +45,7 @@ namespace displays class CPMRenderObject { public: - CPMRenderObject() = default; // Default constructor + //CPMRenderObject() = default; // Default constructor 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); /** 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 66d23a6c8..d9227bd30 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -24,64 +24,50 @@ SOFTWARE. #include "displays/CPM/cpm_display.hpp" -#include -#include -#include #include +#include #include +#include +#include #include #include "rviz_common/display_context.hpp" -#include "rviz_common/transformation/transformation_manager.hpp" #include "rviz_common/frame_manager_iface.hpp" #include "rviz_common/logging.hpp" +#include "rviz_common/properties/bool_property.hpp" #include "rviz_common/properties/color_property.hpp" #include "rviz_common/properties/float_property.hpp" -#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/transformation/transformation_manager.hpp" #include "rviz_common/properties/parse_color.hpp" -namespace etsi_its_msgs -{ -namespace displays -{ +namespace etsi_its_msgs { +namespace displays { -CPMDisplay::CPMDisplay() -{ +CPMDisplay::CPMDisplay() { // General Properties - buffer_timeout_ = new rviz_common::properties::FloatProperty( - "Timeout", 0.1f, - "Time (in s) until objects disappear", this); + buffer_timeout_ = + new rviz_common::properties::FloatProperty("Timeout", 0.1f, "Time (in s) until objects disappear", this); buffer_timeout_->setMin(0); - bb_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 1.0f, - "Scale of objects", this); + bb_scale_ = new rviz_common::properties::FloatProperty("Scale", 1.0f, "Scale of objects", this); bb_scale_->setMin(0.01); - color_property_ = new rviz_common::properties::ColorProperty( - "Color", QColor(25, 0, 255), - "Object color", this); - show_meta_ = new rviz_common::properties::BoolProperty("Metadata", true, - "Show metadata as text next to objects", this); - text_color_property_ = new rviz_common::properties::ColorProperty( - "Color", QColor(25, 0, 255), - "Text color", show_meta_); + color_property_ = new rviz_common::properties::ColorProperty("Color", QColor(25, 0, 255), "Object color", this); + show_meta_ = + new rviz_common::properties::BoolProperty("Metadata", true, "Show metadata as text next to objects", this); + text_color_property_ = + new rviz_common::properties::ColorProperty("Color", QColor(25, 0, 255), "Text color", show_meta_); char_height_ = new rviz_common::properties::FloatProperty("Scale", 4.0, "Scale of text", show_meta_); - show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, - "Show StationID", show_meta_); - show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, - "Show speed", show_meta_); - + show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, "Show StationID", show_meta_); + show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, "Show speed", show_meta_); } -CPMDisplay::~CPMDisplay() -{ - if (initialized() ) { +CPMDisplay::~CPMDisplay() { + if (initialized()) { scene_manager_->destroyManualObject(manual_object_); } } -void CPMDisplay::onInitialize() -{ +void CPMDisplay::onInitialize() { RTDClass::onInitialize(); auto nodeAbstraction = context_->getRosNodeAbstraction().lock(); @@ -92,143 +78,144 @@ void CPMDisplay::onInitialize() scene_node_->attachObject(manual_object_); } -void CPMDisplay::reset() -{ +void CPMDisplay::reset() { RTDClass::reset(); manual_object_->clear(); } -void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) -{ +void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) { // Generate CPM render object from message rclcpp::Time now = rviz_node_->now(); uint64_t nanosecs = now.nanoseconds(); if (nanosecs == 0) { - setStatus( - rviz_common::properties::StatusProperty::Warn, "Topic", - "Message received before clock got a valid time"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Topic", "Message received before clock got a valid time"); return; } - CPMRenderObject cpm(*msg, now, getLeapSecondInsertionsSince2004(static_cast(now.seconds()))); + uint16_t n_leap_seconds = getLeapSecondInsertionsSince2004(static_cast(now.seconds())); + + CPMRenderObject cpm(*msg, now, n_leap_seconds); if (!cpm.validateFloats()) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); return; } // Check if Station ID is already present in list auto it = cpms_.find(cpm.getStationID()); - if (it != cpms_.end()) it->second = cpm; // Key exists, update the value - else cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); + if (it != cpms_.end()) + it->second = cpm; // Key exists, update the value + else + cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); return; } -void CPMDisplay::update(float, float) -{ - +void CPMDisplay::update(float, float) { bboxs_.clear(); texts_.clear(); + for (auto it = cpms_.begin(); it != cpms_.end(); ++it) { + CPMRenderObject cpm = it->second; - - //info logger for the number of perceived objects - u_int8_t number_of_objects = cpm.getNumberOfObjects(); - //info logger for the number of perceived objects - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Number of perceived objects: %d", number_of_objects); + //info logger for the number of perceived objects + u_int8_t number_of_objects = cpm.getNumberOfObjects(); + //info logger for the number of perceived objects + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Number of perceived objects: %d", number_of_objects); //begin loop over all perceived objects - for(int i=0; i<5; i++) { - - Ogre::Vector3 sn_position; - Ogre::Quaternion sn_orientation; - if (!context_->getFrameManager()->getTransform(cpm.getHeader(), sn_position, sn_orientation)) { - // Check if transform exists - // setMissingTransformToFixedFrame(cpm.getHeader().frame_id); - setMissingTransformToFixedFrame("map"); - return; - } - // We don't want to use the transform in sn_position and sn_orientation though, because they are only in float precision. - // So we get the transfrom manually from tf2: - std::string fixed_frame = fixed_frame_.toStdString(); - geometry_msgs::msg::PoseStamped pose_origin; - pose_origin.header = cpm.getHeader(); - pose_origin.pose.position.x = 0; - pose_origin.pose.position.y = 0; - pose_origin.pose.position.z = 0; - pose_origin.pose.orientation.w = 1; - pose_origin.pose.orientation.x = 0; - pose_origin.pose.orientation.y = 0; - pose_origin.pose.orientation.z = 0; - geometry_msgs::msg::PoseStamped pose_fixed_frame = context_->getTransformationManager()->getCurrentTransformer()->transform(pose_origin, fixed_frame); - geometry_msgs::msg::TransformStamped transform_to_fixed_frame; - transform_to_fixed_frame.header = pose_fixed_frame.header; - transform_to_fixed_frame.transform.translation.x = pose_fixed_frame.pose.position.x; - transform_to_fixed_frame.transform.translation.y = pose_fixed_frame.pose.position.y; - transform_to_fixed_frame.transform.translation.z = pose_fixed_frame.pose.position.z; - transform_to_fixed_frame.transform.rotation = pose_fixed_frame.pose.orientation; - - setTransformOk(); - - // set pose of scene node to origin (=fixed frame)! - scene_node_->setPosition(Ogre::Vector3{0.0f, 0.0f, 0.0f}); - scene_node_->setOrientation(Ogre::Quaternion{1.0f, 0.0f, 0.0f, 0.0f}); - - auto child_scene_node = scene_node_->createChildSceneNode(); - // Set position of scene node to the position relative to the fixed frame - geometry_msgs::msg::Pose pose = cpm.getPose(i); - - geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(i); - tf2::doTransform(pose, pose, transform_to_fixed_frame); - Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); - Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - - //info logger for the position of the object - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Position of the object: x: %f, y: %f, z: %f", position.x, position.y, position.z); - - // set pose of child scene node of bounding-box - child_scene_node->setPosition(position); - child_scene_node->setOrientation(orientation); - - // create boundind-box object - std::shared_ptr bbox = std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); - - // set the dimensions of bounding box - Ogre::Vector3 dims; - double scale = bb_scale_->getFloat(); - dims.x = dimensions.x*scale; - dims.y = dimensions.y*scale; - dims.z = dimensions.z*scale; - bbox->setScale(dims); - // set the color of bounding box - Ogre::ColourValue bb_color = rviz_common::properties::qtToOgre(color_property_->getColor()); - bbox->setColor(bb_color); - bboxs_.push_back(bbox); - - // Visualize meta-information as text - if(show_meta_->getBool()) { - std::string text; - if(show_station_id_->getBool()) { - text+="StationID: " + std::to_string(cpm.getStationID()); - text+="\n"; + for (int i = 0; i < number_of_objects; i++) { + Ogre::Vector3 sn_position; + Ogre::Quaternion sn_orientation; + if (!context_->getFrameManager()->getTransform(cpm.getHeader(), sn_position, sn_orientation)) { + // Check if transform exists + // setMissingTransformToFixedFrame(cpm.getHeader().frame_id); + setMissingTransformToFixedFrame("map"); + return; } - if(show_speed_->getBool()) { - // text+="Speed: " + std::to_string((int)(cpm.getSpeed()*3.6)) + " km/h"; + // We don't want to use the transform in sn_position and sn_orientation though, because they are only in float precision. + // So we get the transfrom manually from tf2: + std::string fixed_frame = fixed_frame_.toStdString(); + geometry_msgs::msg::PoseStamped pose_origin; + pose_origin.header = cpm.getHeader(); + pose_origin.pose.position.x = 0; + pose_origin.pose.position.y = 0; + pose_origin.pose.position.z = 0; + pose_origin.pose.orientation.w = 1; + pose_origin.pose.orientation.x = 0; + pose_origin.pose.orientation.y = 0; + pose_origin.pose.orientation.z = 0; + geometry_msgs::msg::PoseStamped pose_fixed_frame = + context_->getTransformationManager()->getCurrentTransformer()->transform(pose_origin, fixed_frame); + geometry_msgs::msg::TransformStamped transform_to_fixed_frame; + transform_to_fixed_frame.header = pose_fixed_frame.header; + transform_to_fixed_frame.transform.translation.x = pose_fixed_frame.pose.position.x; + transform_to_fixed_frame.transform.translation.y = pose_fixed_frame.pose.position.y; + transform_to_fixed_frame.transform.translation.z = pose_fixed_frame.pose.position.z; + transform_to_fixed_frame.transform.rotation = pose_fixed_frame.pose.orientation; + + setTransformOk(); + + // set pose of scene node to origin (=fixed frame)! + scene_node_->setPosition(Ogre::Vector3{0.0f, 0.0f, 0.0f}); + scene_node_->setOrientation(Ogre::Quaternion{1.0f, 0.0f, 0.0f, 0.0f}); + + auto child_scene_node = scene_node_->createChildSceneNode(); + // Set position of scene node to the position relative to the fixed frame + geometry_msgs::msg::Pose pose = cpm.getPose(i); + + geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(i); + tf2::doTransform(pose, pose, transform_to_fixed_frame); + Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); + Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + + //info logger for the position of the object + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Position of the object: x: %f, y: %f, z: %f", position.x, + position.y, position.z); + + // set pose of child scene node of bounding-box + child_scene_node->setPosition(position); + child_scene_node->setOrientation(orientation); + + // create boundind-box object + std::shared_ptr bbox = + std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); + + // set the dimensions of bounding box + Ogre::Vector3 dims; + double scale = bb_scale_->getFloat(); + dims.x = dimensions.x * scale; + dims.y = dimensions.y * scale; + dims.z = dimensions.z * scale; + bbox->setScale(dims); + // set the color of bounding box + Ogre::ColourValue bb_color = rviz_common::properties::qtToOgre(color_property_->getColor()); + bbox->setColor(bb_color); + bboxs_.push_back(bbox); + + // Visualize meta-information as text + if (show_meta_->getBool()) { + std::string text; + if (show_station_id_->getBool()) { + text += "StationID: " + std::to_string(cpm.getStationID()); + text += "\n"; + } + if (show_speed_->getBool()) { + // text+="Speed: " + std::to_string((int)(cpm.getSpeed()*3.6)) + " km/h"; + } + if (!text.size()) return; + std::shared_ptr text_render = + std::make_shared(text, "Liberation Sans", char_height_->getFloat()); + double height = dims.z; + height += text_render->getBoundingRadius(); + Ogre::Vector3 offs(0.0, 0.0, height); + // There is a bug in rviz_rendering::MovableText::setGlobalTranslation https://github.com/ros2/rviz/issues/974 + text_render->setGlobalTranslation(offs); + Ogre::ColourValue text_color = rviz_common::properties::qtToOgre(text_color_property_->getColor()); + text_render->setColor(text_color); + child_scene_node->attachObject(text_render.get()); + texts_.push_back(text_render); } - if(!text.size()) return; - std::shared_ptr text_render = std::make_shared(text, "Liberation Sans", char_height_->getFloat()); - double height = dims.z; - height+=text_render->getBoundingRadius(); - Ogre::Vector3 offs(0.0, 0.0, height); - // There is a bug in rviz_rendering::MovableText::setGlobalTranslation https://github.com/ros2/rviz/issues/974 - text_render->setGlobalTranslation(offs); - Ogre::ColourValue text_color = rviz_common::properties::qtToOgre(text_color_property_->getColor()); - text_render->setColor(text_color); - child_scene_node->attachObject(text_render.get()); - texts_.push_back(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 505970b3a..33203b895 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 @@ -24,23 +24,20 @@ SOFTWARE. #include "displays/CPM/cpm_render_object.hpp" -namespace etsi_its_msgs -{ -namespace displays -{ +namespace etsi_its_msgs { +namespace displays { - CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, uint16_t n_leap_seconds) { +CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, + uint16_t n_leap_seconds) { + uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(cpm); - uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(cpm); + geometry_msgs::msg::Pose poses[number_of_objects]; + geometry_msgs::msg::Vector3 dimensionss[number_of_objects]; + geometry_msgs::msg::Vector3 velocities[number_of_objects]; - - geometry_msgs::msg::Pose poses[number_of_objects]; - geometry_msgs::msg::Vector3 dimensionss[number_of_objects]; - geometry_msgs::msg::Vector3 velocities[number_of_objects]; - - for(int i=0; i Date: Mon, 16 Sep 2024 13:25:30 +0200 Subject: [PATCH 36/52] structural changes --- .../include/displays/CPM/cpm_display.hpp | 3 + .../displays/CPM/cpm_render_object.hpp | 17 ++--- .../src/displays/CPM/cpm_display.cpp | 71 ++++++++++++++----- .../src/displays/CPM/cpm_render_object.cpp | 38 ++++------ 4 files changed, 78 insertions(+), 51 deletions(-) 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 9628fba15..5761174d2 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -84,6 +84,9 @@ class CPMDisplay : public rviz_common::properties::ColorProperty *color_property_, *text_color_property_; std::unordered_map cpms_; + + // Rendering objects + std::vector> cpm_render_objects_; 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 689cf68eb..d5994db07 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,8 +45,7 @@ namespace displays class CPMRenderObject { public: - //CPMRenderObject() = default; // Default constructor - 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); + 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); /** * @brief This function validates all float variables that are part of a CPMRenderObject @@ -81,34 +80,30 @@ class CPMRenderObject * * @return geometry_msgs::msg::Pose */ - geometry_msgs::msg::Pose getPose(int i); + geometry_msgs::msg::Pose getPose(); /** * @brief Get dimensions of CPM-Object * * @return geometry_msgs::msg::Vector3 (x equals length, y equals width, z equals height) */ - geometry_msgs::msg::Vector3 getDimensions(int i); + geometry_msgs::msg::Vector3 getDimensions(); /** * @brief Get speed of CPM-object * * @return double */ - geometry_msgs::msg::Vector3 getSpeed(int i); + geometry_msgs::msg::Vector3 getVelocity(); - int getNumberOfObjects(); + uint8_t getNumberOfObjects(); private: // member variables std_msgs::msg::Header header; - uint8_t number_of_objects; + uint8_t number_of_objects_; uint32_t station_id; int station_type; - //declare an array of poses, dimensions and velocities - std::vector poses; - std::vector dimensionss; - std::vector velocities; 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 d9227bd30..5564fdcb5 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -92,38 +92,69 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM return; } + uint16_t n_leap_seconds = getLeapSecondInsertionsSince2004(static_cast(now.seconds())); + uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects( + etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(*msg)); + + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "6"); + - CPMRenderObject cpm(*msg, now, n_leap_seconds); + cpm_render_objects_.clear(); + for(int i=0; i(cpm)); + + } + + // // Check if Station ID is already present in list + // auto it = cpms_.find(cpm.getStationID()); + // if (it != cpms_.end()) + // it->second = cpm; // Key exists, update the value + // else + // cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); + + // return; - // Check if Station ID is already present in list - auto it = cpms_.find(cpm.getStationID()); - if (it != cpms_.end()) - it->second = cpm; // Key exists, update the value - else - cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); - return; + } void CPMDisplay::update(float, float) { + + + // Check for outdated CPMs + for (auto it = cpms_.begin(); it != cpms_.end(); ) { + if (it->second.getAge(rviz_node_->now())/ 10e9 > buffer_timeout_->getFloat()) { + it = cpms_.erase(it); + } + else { + ++it; + } + } bboxs_.clear(); texts_.clear(); + + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "1"); + for (auto it = cpms_.begin(); it != cpms_.end(); ++it) { CPMRenderObject cpm = it->second; - //info logger for the number of perceived objects - u_int8_t number_of_objects = cpm.getNumberOfObjects(); - //info logger for the number of perceived objects - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Number of perceived objects: %d", number_of_objects); + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "2"); + + for(int i=0; i cpm_ptr = cpm_render_objects_[i]; + etsi_its_msgs::displays::CPMRenderObject cpm = *cpm_ptr; // Dereference the shared_ptr to get the object + + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "3"); - //begin loop over all perceived objects - for (int i = 0; i < number_of_objects; i++) { Ogre::Vector3 sn_position; Ogre::Quaternion sn_orientation; if (!context_->getFrameManager()->getTransform(cpm.getHeader(), sn_position, sn_orientation)) { @@ -161,9 +192,9 @@ void CPMDisplay::update(float, float) { auto child_scene_node = scene_node_->createChildSceneNode(); // Set position of scene node to the position relative to the fixed frame - geometry_msgs::msg::Pose pose = cpm.getPose(i); + geometry_msgs::msg::Pose pose = cpm.getPose(); - geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(i); + geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(); tf2::doTransform(pose, pose, transform_to_fixed_frame); Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); @@ -215,7 +246,13 @@ void CPMDisplay::update(float, float) { child_scene_node->attachObject(text_render.get()); texts_.push_back(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 33203b895..84d8f7079 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 @@ -28,16 +28,10 @@ namespace etsi_its_msgs { namespace displays { CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, - uint16_t n_leap_seconds) { - uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(cpm); - - geometry_msgs::msg::Pose poses[number_of_objects]; - geometry_msgs::msg::Vector3 dimensionss[number_of_objects]; - geometry_msgs::msg::Vector3 velocities[number_of_objects]; - - for (int i = 0; i < number_of_objects; i++) { + uint16_t n_leap_seconds, uint8_t number_of_object) { + etsi_its_cpm_ts_msgs::msg::PerceivedObject object = etsi_its_cpm_ts_msgs::access::getPerceivedObject( - etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(cpm), i); + etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(cpm), number_of_object); geometry_msgs::msg::Point position = etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject(object); @@ -47,23 +41,23 @@ CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerception pose.position = position; pose.orientation = orientation; - poses[i] = pose; geometry_msgs::msg::Vector3 dimensions = etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject(object); dimensions.z = 1.6; - dimensionss[i] = dimensions; //header.frame_id = position.header.frame_id; header.frame_id = "map"; - // uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromGenerationDeltaTime(etsi_its_cpm_ts_msgs::access::getGenerationDeltaTime(cpm), receive_time.nanoseconds(), n_leap_seconds); - // header.stamp = rclcpp::Time(nanosecs); + 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; geometry_msgs::msg::Vector3 velocity = etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject(object); - velocities[i] = velocity; - } + } bool CPMRenderObject::validateFloats() { @@ -80,17 +74,15 @@ std_msgs::msg::Header CPMRenderObject::getHeader() { return header; } uint32_t CPMRenderObject::getStationID() { return station_id; } -// int CPMRenderObject::getStationType() { -// return station_type; -// } - -geometry_msgs::msg::Pose CPMRenderObject::getPose(int i) { return poses[i]; } +geometry_msgs::msg::Pose CPMRenderObject::getPose() { return pose; } -int CPMRenderObject::getNumberOfObjects() { return number_of_objects; } +uint8_t CPMRenderObject::getNumberOfObjects() { + return number_of_objects_; +} -geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions(int i) { return dimensionss[i]; } +geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions() { return dimensions; } -geometry_msgs::msg::Vector3 CPMRenderObject::getSpeed(int i) { return velocities[i]; } +geometry_msgs::msg::Vector3 CPMRenderObject::getVelocity() { return velocity; } } // namespace displays } // namespace etsi_its_msgs \ No newline at end of file From debe1204ea1c150464de0e04ca49cea6381d788b Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Tue, 17 Sep 2024 16:04:59 +0200 Subject: [PATCH 37/52] rendering works now --- .../include/displays/CPM/cpm_display.hpp | 3 + .../displays/CPM/cpm_render_object.hpp | 3 + .../src/displays/CPM/cpm_display.cpp | 83 +++++++------------ .../src/displays/CPM/cpm_render_object.cpp | 19 +---- 4 files changed, 40 insertions(+), 68 deletions(-) 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 5761174d2..f3be9220b 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -87,6 +87,9 @@ 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 d5994db07..34935e453 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,6 +45,9 @@ namespace displays class CPMRenderObject { public: + // CPMRenderObject() { + // // Initialize members with default values if necessary + // } 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); /** 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 5564fdcb5..621e9bc37 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -84,7 +84,6 @@ void CPMDisplay::reset() { } void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage::ConstSharedPtr msg) { - // Generate CPM render object from message rclcpp::Time now = rviz_node_->now(); uint64_t nanosecs = now.nanoseconds(); if (nanosecs == 0) { @@ -92,68 +91,54 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM return; } - uint16_t n_leap_seconds = getLeapSecondInsertionsSince2004(static_cast(now.seconds())); uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects( etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(*msg)); - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "6"); - + CPMRenderObject cpm(*msg, now, n_leap_seconds, 0); cpm_render_objects_.clear(); - for(int i=0; i(cpm)); - + if (!cpm.validateFloats()) { + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + cpm_render_objects_.push_back(std::make_shared(cpm)); } - // // Check if Station ID is already present in list - // auto it = cpms_.find(cpm.getStationID()); - // if (it != cpms_.end()) - // it->second = cpm; // Key exists, update the value - // else - // cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); + // Check if Station ID is already present in list + auto it = cpms_.find(cpm.getStationID()); + if (it != cpms_.end()) + it->second = cpm; // Key exists, update the value + else + cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); - // return; - - - + return; } void CPMDisplay::update(float, float) { - - - // Check for outdated CPMs - for (auto it = cpms_.begin(); it != cpms_.end(); ) { - if (it->second.getAge(rviz_node_->now())/ 10e9 > buffer_timeout_->getFloat()) { + // Check for outdated CPMs + for (auto it = cpms_.begin(); it != cpms_.end();) { + if (it->second.getAge(rviz_node_->now()) / 10e9 > buffer_timeout_->getFloat()) { it = cpms_.erase(it); - } - else { + } else { ++it; } } bboxs_.clear(); texts_.clear(); - - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "1"); - for (auto it = cpms_.begin(); it != cpms_.end(); ++it) { - CPMRenderObject cpm = it->second; - - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "2"); + //info logger for cpms size + RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Size of cpms: %d", cpms_.size()); - for(int i=0; i cpm_ptr = cpm_render_objects_[i]; - etsi_its_msgs::displays::CPMRenderObject cpm = *cpm_ptr; // Dereference the shared_ptr to get the object - - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "3"); + //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; Ogre::Vector3 sn_position; Ogre::Quaternion sn_orientation; @@ -168,6 +153,8 @@ 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; @@ -199,10 +186,6 @@ void CPMDisplay::update(float, float) { Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - //info logger for the position of the object - RCLCPP_INFO(rclcpp::get_logger("cpm_display"), "Position of the object: x: %f, y: %f, z: %f", position.x, - position.y, position.z); - // set pose of child scene node of bounding-box child_scene_node->setPosition(position); child_scene_node->setOrientation(orientation); @@ -246,13 +229,7 @@ void CPMDisplay::update(float, float) { child_scene_node->attachObject(text_render.get()); texts_.push_back(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 84d8f7079..59840ae3a 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,18 +33,10 @@ 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); - geometry_msgs::msg::Point position = etsi_its_cpm_ts_msgs::access::getPositionOfPerceivedObject(object); - - geometry_msgs::msg::Quaternion orientation = etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject(object); - - geometry_msgs::msg::Pose pose; - - pose.position = position; - pose.orientation = orientation; - - geometry_msgs::msg::Vector3 dimensions = etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject(object); - dimensions.z = 1.6; - + 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"; @@ -55,9 +47,6 @@ CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerception uint32_t station_id = etsi_its_cpm_ts_msgs::access::getStationID(cpm); //hardcoded station_id to 10 for testing station_id = 10; - - geometry_msgs::msg::Vector3 velocity = etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject(object); - } bool CPMRenderObject::validateFloats() { From 5c557b992925d7f710184a76b31616ff92293736 Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Thu, 19 Sep 2024 15:54:45 +0200 Subject: [PATCH 38/52] 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 f360aa766..4592fbf20 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 bcd70b678..51003cea6 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 9db66c151..b09bbfe21 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 f3be9220b..479a39b59 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 34935e453..360b01461 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 621e9bc37..b32ab9c46 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 59840ae3a..a1807ced1 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 From fefb8a4b1e5b1d8baefcae86b0eb94fda56281d6 Mon Sep 17 00:00:00 2001 From: Oliver Kurzaj Date: Thu, 19 Sep 2024 16:25:55 +0200 Subject: [PATCH 39/52] fix namespace in tests; fix datatype in cpm setter --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 4 ++-- etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 51003cea6..64c7fa7e0 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 @@ -136,10 +136,10 @@ inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, co * @param value Value to set in cm * @param confidence Confidence to set cm (optional) */ -inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const int32_t value, +inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const int16_t value, const uint16_t confidence = CoordinateConfidence::UNAVAILABLE) { // limit value range - int32_t limited_value = std::max(CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE, + int16_t limited_value = std::max(CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE, std::min(CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE, value)); coordinate.value.value = limited_value; 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 b09bbfe21..d6b42417f 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,7 +60,7 @@ 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); - etsi_its_cpm_ts_msgs::msg::PerceivedObject object; + cpm_ts_msgs::PerceivedObject object; gm::Vector3 dimensions; dimensions.x = randomDouble(0.1, 25.6); dimensions.y = randomDouble(0.1, 25.6); From bc8110952f70fe1074be1d74892b605c2524401e Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Wed, 9 Oct 2024 11:48:50 +0000 Subject: [PATCH 40/52] format cam utils --- .../etsi_its_msgs_utils/impl/cam/cam_access.h | 2 +- .../impl/cam/cam_getters.h | 3 +- .../impl/cam/cam_getters_common.h | 90 +++++++++---------- .../impl/cam/cam_setters.h | 26 +++--- .../impl/cam/cam_setters_common.h | 83 ++++++++++------- .../impl/cam/cam_ts_access.h | 2 +- .../impl/cam/cam_ts_getters.h | 3 +- .../impl/cam/cam_ts_setters.h | 24 ++--- .../etsi_its_msgs_utils/impl/cam/cam_utils.h | 20 +++-- 9 files changed, 132 insertions(+), 121 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h index 194972d8d..844171a51 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h @@ -53,4 +53,4 @@ SOFTWARE. namespace etsi_its_cam_msgs::access { #include -} // namespace etsi_its_cam_msgs::access \ No newline at end of file +} // namespace etsi_its_cam_msgs::access \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters.h index 6e6c30b32..7b6973c86 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters.h @@ -33,5 +33,6 @@ SOFTWARE. namespace etsi_its_cam_msgs::access { #include + #include -} // namespace etsi_its_cam_msgs::access \ No newline at end of file +} // namespace etsi_its_cam_msgs::access \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h index 645be472a..277f1b6c7 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_getters_common.h @@ -38,9 +38,7 @@ SOFTWARE. * @param cam CAM to get the StationID value from * @return stationID value */ -inline uint32_t getStationID(const CAM& cam){ - return getStationID(cam.header); -} +inline uint32_t getStationID(const CAM& cam) { return getStationID(cam.header); } /** * @brief Get the GenerationDeltaTime @@ -48,9 +46,7 @@ inline uint32_t getStationID(const CAM& cam){ * @param cam CAM to get the GenerationDeltaTime from * @return GenerationDeltaTime the GenerationDeltaTime */ -inline GenerationDeltaTime getGenerationDeltaTime(const CAM& cam){ - return cam.cam.generation_delta_time; -} +inline GenerationDeltaTime getGenerationDeltaTime(const CAM& cam) { return cam.cam.generation_delta_time; } /** * @brief Get the GenerationDeltaTime-Value @@ -58,9 +54,7 @@ inline GenerationDeltaTime getGenerationDeltaTime(const CAM& cam){ * @param cam CAM to get the GenerationDeltaTime-Value from * @return uint16_t the GenerationDeltaTime-Value */ -inline uint16_t getGenerationDeltaTimeValue(const CAM& cam){ - return getGenerationDeltaTime(cam).value; -} +inline uint16_t getGenerationDeltaTimeValue(const CAM& cam) { return getGenerationDeltaTime(cam).value; } /** * @brief Get the stationType object @@ -68,9 +62,7 @@ inline uint16_t getGenerationDeltaTimeValue(const CAM& cam){ * @param cam CAM to get the stationType value from * @return stationType value */ -inline uint8_t getStationType(const CAM& cam){ - return cam.cam.cam_parameters.basic_container.station_type.value; -} +inline uint8_t getStationType(const CAM& cam) { return cam.cam.cam_parameters.basic_container.station_type.value; } /** * @brief Get the Latitude value of CAM @@ -78,7 +70,7 @@ inline uint8_t getStationType(const CAM& cam){ * @param cam CAM to get the Latitude value from * @return Latitude value in degree as decimal number */ -inline double getLatitude(const CAM& cam){ +inline double getLatitude(const CAM& cam) { return getLatitude(cam.cam.cam_parameters.basic_container.reference_position.latitude); } @@ -88,7 +80,7 @@ inline double getLatitude(const CAM& cam){ * @param cam CAM to get the Longitude value from * @return Longitude value in degree as decimal number */ -inline double getLongitude(const CAM& cam){ +inline double getLongitude(const CAM& cam) { return getLongitude(cam.cam.cam_parameters.basic_container.reference_position.longitude); } @@ -98,7 +90,7 @@ inline double getLongitude(const CAM& cam){ * @param cam CAM to get the Altitude value from * @return Altitude value (above the reference ellipsoid surface) in meter as decimal number */ -inline double getAltitude(const CAM& cam){ +inline double getAltitude(const CAM& cam) { return getAltitude(cam.cam.cam_parameters.basic_container.reference_position.altitude); } @@ -110,9 +102,7 @@ inline double getAltitude(const CAM& cam){ * @param heading to get the Heading value from * @return Heading value in degree as decimal number */ -inline double getHeading(const Heading& heading){ - return ((double)heading.heading_value.value)*1e-1; -} +inline double getHeading(const Heading& heading) { return ((double)heading.heading_value.value) * 1e-1; } /** * @brief Get the Heading value of CAM @@ -122,7 +112,7 @@ inline double getHeading(const Heading& heading){ * @param cam CAM to get the Heading value from * @return Heading value in degree as decimal number */ -inline double getHeading(const CAM& cam){ +inline double getHeading(const CAM& cam) { return getHeading(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.heading); } @@ -132,8 +122,8 @@ inline double getHeading(const CAM& cam){ * @param vehicleLength to get the vehicle length value from * @return vehicle length value in meter as decimal number */ -inline double getVehicleLength(const VehicleLength& vehicle_length){ - return ((double)vehicle_length.vehicle_length_value.value)*1e-1; +inline double getVehicleLength(const VehicleLength& vehicle_length) { + return ((double)vehicle_length.vehicle_length_value.value) * 1e-1; } /** @@ -142,8 +132,9 @@ inline double getVehicleLength(const VehicleLength& vehicle_length){ * @param cam CAM to get the vehicle length value from * @return vehicle length value in meter as decimal number */ -inline double getVehicleLength(const CAM& cam){ - return getVehicleLength(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_length); +inline double getVehicleLength(const CAM& cam) { + return getVehicleLength( + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_length); } /** @@ -152,8 +143,9 @@ inline double getVehicleLength(const CAM& cam){ * @param cam CAM to get the vehicle width value from * @return vehicle width value in meter as decimal number */ -inline double getVehicleWidth(const CAM& cam){ - return getVehicleWidth(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_width); +inline double getVehicleWidth(const CAM& cam) { + return getVehicleWidth( + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_width); } /** @@ -162,7 +154,7 @@ inline double getVehicleWidth(const CAM& cam){ * @param cam CAM to get the speed value from * @return speed value in m/s as decimal number */ -inline double getSpeed(const CAM& cam){ +inline double getSpeed(const CAM& cam) { return getSpeed(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.speed); } @@ -172,8 +164,9 @@ inline double getSpeed(const CAM& cam){ * @param cam CAM to get the lateral acceleration from * @return lateral acceleration in m/s^2 as decimal number (left is positive) */ -inline double getLongitudinalAcceleration(const CAM& cam){ - return getLongitudinalAcceleration(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.longitudinal_acceleration); +inline double getLongitudinalAcceleration(const CAM& cam) { + return getLongitudinalAcceleration( + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.longitudinal_acceleration); } /** @@ -182,13 +175,12 @@ inline double getLongitudinalAcceleration(const CAM& cam){ * @param cam CAM to get the lateral acceleration from * @return lateral acceleration in m/s^2 as decimal number (left is positive) */ -inline double getLateralAcceleration(const CAM& cam){ - if(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.lateral_acceleration_is_present) - { - return getLateralAcceleration(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.lateral_acceleration); - } - else - { +inline double getLateralAcceleration(const CAM& cam) { + if (cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency + .lateral_acceleration_is_present) { + return getLateralAcceleration( + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.lateral_acceleration); + } else { throw std::invalid_argument("LateralAcceleration is not present!"); } } @@ -204,7 +196,7 @@ inline double getLateralAcceleration(const CAM& cam){ * @param[out] northp hemisphere (true means north, false means south) * @return gm::PointStamped geometry_msgs::PointStamped of the given position */ -inline gm::PointStamped getUTMPosition(const CAM& cam, int& zone, bool& northp){ +inline gm::PointStamped getUTMPosition(const CAM& cam, int& zone, bool& northp) { return getUTMPosition(cam.cam.cam_parameters.basic_container.reference_position, zone, northp); } @@ -217,7 +209,7 @@ inline gm::PointStamped getUTMPosition(const CAM& cam, int& zone, bool& northp){ * @param[in] cam CAM to get the UTM Position from * @return gm::PointStamped geometry_msgs::PointStamped of the given position */ -inline gm::PointStamped getUTMPosition(const CAM& cam){ +inline gm::PointStamped getUTMPosition(const CAM& cam) { int zone; bool northp; return getUTMPosition(cam.cam.cam_parameters.basic_container.reference_position, zone, northp); @@ -229,7 +221,7 @@ inline gm::PointStamped getUTMPosition(const CAM& cam){ * @param exterior_lights * @return std::vector */ -inline std::vector getExteriorLights(const ExteriorLights& exterior_lights){ +inline std::vector getExteriorLights(const ExteriorLights& exterior_lights) { return getBitString(exterior_lights.value, exterior_lights.bits_unused); } @@ -239,16 +231,16 @@ inline std::vector getExteriorLights(const ExteriorLights& exterior_lights * @param cam CAM to get the ExteriorLights values from * @return std::vector */ -inline std::vector getExteriorLights(const CAM& cam){ - if(cam.cam.cam_parameters.low_frequency_container_is_present) { - if(cam.cam.cam_parameters.low_frequency_container.choice == LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY) { - return getExteriorLights(cam.cam.cam_parameters.low_frequency_container.basic_vehicle_container_low_frequency.exterior_lights); - } - else { +inline std::vector getExteriorLights(const CAM& cam) { + if (cam.cam.cam_parameters.low_frequency_container_is_present) { + if (cam.cam.cam_parameters.low_frequency_container.choice == + LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY) { + return getExteriorLights( + cam.cam.cam_parameters.low_frequency_container.basic_vehicle_container_low_frequency.exterior_lights); + } else { throw std::invalid_argument("LowFrequencyContainer is not BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY!"); } - } - else { + } else { throw std::invalid_argument("LowFrequencyContainer is not present!"); } } @@ -259,7 +251,7 @@ inline std::vector getExteriorLights(const CAM& cam){ * @param acceleration_control * @return std::vector */ -inline std::vector getAccelerationControl(const AccelerationControl& acceleration_control){ +inline std::vector getAccelerationControl(const AccelerationControl& acceleration_control) { return getBitString(acceleration_control.value, acceleration_control.bits_unused); } @@ -269,7 +261,7 @@ inline std::vector getAccelerationControl(const AccelerationControl& accel * @param driving_lane_status * @return std::vector */ -inline std::vector getDrivingLaneStatus(const DrivingLaneStatus& driving_lane_status){ +inline std::vector getDrivingLaneStatus(const DrivingLaneStatus& driving_lane_status) { return getBitString(driving_lane_status.value, driving_lane_status.bits_unused); } @@ -303,4 +295,4 @@ inline std::vector getEmergencyPriority(const EmergencyPriority& emergency return getBitString(emergency_priority.value, emergency_priority.bits_unused); } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters.h index dbdf0d564..c18e4e5e5 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters.h @@ -36,18 +36,18 @@ SOFTWARE. namespace etsi_its_cam_msgs::access { #include + #include - /** - * @brief Set the ItsPduHeader-object for a CAM - * - * @param cam CAM-Message to set the ItsPduHeader - * @param station_id - * @param protocol_version - */ - inline void setItsPduHeader(CAM &cam, const uint32_t station_id, const uint8_t protocol_version = 0) - { - setItsPduHeader(cam.header, ItsPduHeader::MESSAGE_ID_CAM, station_id, protocol_version); - } - -} // namespace etsi_its_cam_msgs::access +/** + * @brief Set the ItsPduHeader-object for a CAM + * + * @param cam CAM-Message to set the ItsPduHeader + * @param station_id + * @param protocol_version + */ +inline void setItsPduHeader(CAM &cam, const uint32_t station_id, const uint8_t protocol_version = 0) { + setItsPduHeader(cam.header, ItsPduHeader::MESSAGE_ID_CAM, station_id, protocol_version); +} + +} // namespace etsi_its_cam_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h index e12869942..52c92b1e6 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_setters_common.h @@ -29,8 +29,8 @@ SOFTWARE. * @brief Common setter functions for the ETSI ITS CAM (EN and TS) */ -# ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H -# define ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H #include @@ -41,12 +41,14 @@ SOFTWARE. * @param unix_nanosecs Timestamp in unix-nanoseconds to set the GenerationDeltaTime-Value from * @param n_leap_seconds Number of leap seconds since 2004 for the given timestamp (Default: etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) */ -inline void setGenerationDeltaTime(GenerationDeltaTime& generation_delta_time, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) { +inline void setGenerationDeltaTime( + GenerationDeltaTime& generation_delta_time, 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); - uint16_t gdt_value = t_its.value%65536; + uint16_t gdt_value = t_its.value % 65536; throwIfOutOfRange(gdt_value, GenerationDeltaTime::MIN, GenerationDeltaTime::MAX, "GenerationDeltaTime"); - generation_delta_time.value=gdt_value; + generation_delta_time.value = gdt_value; } /** @@ -56,7 +58,9 @@ inline void setGenerationDeltaTime(GenerationDeltaTime& generation_delta_time, c * @param unix_nanosecs Timestamp in unix-nanoseconds to set the GenerationDeltaTime-Value from * @param n_leap_seconds Number of leap seconds since 2004 for the given timestamp (Default: etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) */ -inline void setGenerationDeltaTime(CAM& cam, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) { +inline void setGenerationDeltaTime( + CAM& cam, const uint64_t unix_nanosecs, + const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) { setGenerationDeltaTime(cam.cam.generation_delta_time, unix_nanosecs, n_leap_seconds); } @@ -66,7 +70,7 @@ inline void setGenerationDeltaTime(CAM& cam, const uint64_t unix_nanosecs, const * @param cam CAM-Message to set the station_type value * @param value station_type value to set */ -inline void setStationType(CAM& cam, const uint8_t value){ +inline void setStationType(CAM& cam, const uint8_t value) { setStationType(cam.cam.cam_parameters.basic_container.station_type, value); } @@ -79,7 +83,7 @@ inline void setStationType(CAM& cam, const uint8_t value){ * @param value Heading value in degree as decimal number */ inline void setHeadingValue(HeadingValue& heading, const double value) { - int64_t deg = (int64_t)std::round(value*1e1); + int64_t deg = (int64_t)std::round(value * 1e1); throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); heading.value = deg; } @@ -107,8 +111,9 @@ inline void setHeading(Heading& heading, const double value) { * @param cam CAM to set the ReferencePosition * @param value Heading value in degree as decimal number */ -inline void setHeading(CAM& cam, const double heading_val){ - setHeading(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.heading, heading_val); +inline void setHeading(CAM& cam, const double heading_val) { + setHeading(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.heading, + heading_val); } /** @@ -118,7 +123,7 @@ inline void setHeading(CAM& cam, const double heading_val){ * @param value VehicleLengthValue in meter as decimal number */ inline void setVehicleLengthValue(VehicleLengthValue& vehicle_length, const double value) { - int64_t length = (int64_t)std::round(value*1e1); + int64_t length = (int64_t)std::round(value * 1e1); throwIfOutOfRange(length, VehicleLengthValue::MIN, VehicleLengthValue::MAX, "VehicleLengthValue"); vehicle_length.value = length; } @@ -143,9 +148,12 @@ inline void setVehicleLength(VehicleLength& vehicle_length, const double value) * @param vehicle_length vehicle length in meter as decimal number * @param vehicle_width vehicle width in meter as decimal number */ -inline void setVehicleDimensions(CAM& cam, const double vehicle_length, const double vehicle_width){ - setVehicleLength(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_length, vehicle_length); - setVehicleWidth(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_width, vehicle_width); +inline void setVehicleDimensions(CAM& cam, const double vehicle_length, const double vehicle_width) { + setVehicleLength( + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_length, + vehicle_length); + setVehicleWidth(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.vehicle_width, + vehicle_width); } /** @@ -154,7 +162,7 @@ inline void setVehicleDimensions(CAM& cam, const double vehicle_length, const do * @param cam CAM to set the speed value * @param speed_val speed value to set in m/s as decimal number */ -inline void setSpeed(CAM& cam, const double speed_val){ +inline void setSpeed(CAM& cam, const double speed_val) { setSpeed(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.speed, speed_val); } @@ -164,8 +172,10 @@ inline void setSpeed(CAM& cam, const double speed_val){ * @param cam CAM to set the acceleration value s * @param lon_accel longitudinal acceleration to set in m/s^2 as decimal number (braking is negative), if not available use 16.1 m/s^2 */ -inline void setLongitudinalAcceleration(CAM& cam, const double lon_accel){ - setLongitudinalAcceleration(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.longitudinal_acceleration, lon_accel); +inline void setLongitudinalAcceleration(CAM& cam, const double lon_accel) { + setLongitudinalAcceleration( + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.longitudinal_acceleration, + lon_accel); } /** @@ -174,9 +184,12 @@ inline void setLongitudinalAcceleration(CAM& cam, const double lon_accel){ * @param cam CAM to set the acceleration value s * @param lat_accel lateral acceleration to set in m/s^2 as decimal number (left is positiv), if not available use 16.1 m/s^2 */ -inline void setLateralAcceleration(CAM& cam, const double lat_accel){ - setLateralAcceleration(cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.lateral_acceleration, lat_accel); - cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.lateral_acceleration_is_present = true; +inline void setLateralAcceleration(CAM& cam, const double lat_accel) { + setLateralAcceleration( + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency.lateral_acceleration, + lat_accel); + cam.cam.cam_parameters.high_frequency_container.basic_vehicle_container_high_frequency + .lateral_acceleration_is_present = true; } /** @@ -190,7 +203,8 @@ inline void setLateralAcceleration(CAM& cam, const double lat_accel){ * @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(CAM& cam, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE) { +inline void setReferencePosition(CAM& cam, const double latitude, const double longitude, + const double altitude = AltitudeValue::UNAVAILABLE) { setReferencePosition(cam.cam.cam_parameters.basic_container.reference_position, latitude, longitude, altitude); } @@ -206,8 +220,7 @@ inline void setReferencePosition(CAM& cam, const double latitude, const double l * @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(CAM& cam, const gm::PointStamped& utm_position, const int& zone, const bool& northp) -{ +inline void setFromUTMPosition(CAM& cam, const gm::PointStamped& utm_position, const int& zone, const bool& northp) { setFromUTMPosition(cam.cam.cam_parameters.basic_container.reference_position, utm_position, zone, northp); } @@ -227,19 +240,21 @@ inline void setExteriorLights(ExteriorLights& exterior_lights, const std::vector * @param cam CAM to set the exterior lights * @param exterior_lights vector of bools to set the exterior lights */ -inline void setExteriorLights(CAM& cam, const std::vector& exterior_lights){ - if(ExteriorLights::SIZE_BITS != exterior_lights.size()) { - throw std::invalid_argument("Vector has wrong size. (" + std::to_string(exterior_lights.size()) + " != " + std::to_string(ExteriorLights::SIZE_BITS) + ")"); +inline void setExteriorLights(CAM& cam, const std::vector& exterior_lights) { + if (ExteriorLights::SIZE_BITS != exterior_lights.size()) { + throw std::invalid_argument("Vector has wrong size. (" + std::to_string(exterior_lights.size()) + + " != " + std::to_string(ExteriorLights::SIZE_BITS) + ")"); } - if(cam.cam.cam_parameters.low_frequency_container_is_present) { - if(cam.cam.cam_parameters.low_frequency_container.choice == LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY) { - setExteriorLights(cam.cam.cam_parameters.low_frequency_container.basic_vehicle_container_low_frequency.exterior_lights, exterior_lights); - } - else { + if (cam.cam.cam_parameters.low_frequency_container_is_present) { + if (cam.cam.cam_parameters.low_frequency_container.choice == + LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY) { + setExteriorLights( + cam.cam.cam_parameters.low_frequency_container.basic_vehicle_container_low_frequency.exterior_lights, + exterior_lights); + } else { throw std::invalid_argument("LowFrequencyContainer is not BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY!"); } - } - else { + } else { throw std::invalid_argument("LowFrequencyContainer is not present!"); } } @@ -294,4 +309,4 @@ inline void setEmergencyPriority(EmergencyPriority& emergency_priority, const st setBitString(emergency_priority, bits); } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h index 0c11afd65..52a3f15c4 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h @@ -53,4 +53,4 @@ SOFTWARE. namespace etsi_its_cam_ts_msgs::access { #include -} // namespace etsi_its_cam_ts_msgs::access \ No newline at end of file +} // namespace etsi_its_cam_ts_msgs::access \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_getters.h index 1c83b2c12..baecb4a56 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_getters.h @@ -33,5 +33,6 @@ SOFTWARE. namespace etsi_its_cam_ts_msgs::access { #include + #include -} // namespace etsi_its_cam_ts_msgs::access +} // namespace etsi_its_cam_ts_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h index 84d4cd539..a5ef2a5a0 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h @@ -36,18 +36,18 @@ SOFTWARE. namespace etsi_its_cam_ts_msgs::access { #include -#include - /** - * @brief Set the ItsPduHeader-object for a CAM - * - * @param cam CAM-Message to set the ItsPduHeader - * @param station_id - * @param protocol_version - */ - inline void setItsPduHeader(CAM& cam, const uint32_t station_id, const uint8_t protocol_version = 0){ - setItsPduHeader(cam.header, MessageId::CAM, station_id, protocol_version); - } +#include +/** + * @brief Set the ItsPduHeader-object for a CAM + * + * @param cam CAM-Message to set the ItsPduHeader + * @param station_id + * @param protocol_version + */ +inline void setItsPduHeader(CAM& cam, const uint32_t station_id, const uint8_t protocol_version = 0) { + setItsPduHeader(cam.header, MessageId::CAM, station_id, protocol_version); +} -} // namespace etsi_its_cam_ts_msgs::access +} // namespace etsi_its_cam_ts_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h index 8de6e1158..f2f8a8d10 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_utils.h @@ -42,10 +42,10 @@ SOFTWARE. * @param timestamp_estimate estimated time to calculate the corresponding generation from * @return TimestampIts the corresponding TimestampITS object */ -inline TimestampIts getTimestampITSFromGenerationDeltaTime(const GenerationDeltaTime& generation_delta_time, const TimestampIts& timestamp_estimate) -{ +inline TimestampIts getTimestampITSFromGenerationDeltaTime(const GenerationDeltaTime& generation_delta_time, + const TimestampIts& timestamp_estimate) { TimestampIts t_its; - t_its.value = std::floor(timestamp_estimate.value/65536)*65536+generation_delta_time.value; + t_its.value = std::floor(timestamp_estimate.value / 65536) * 65536 + generation_delta_time.value; throwIfOutOfRange(t_its.value, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts"); return t_its; } @@ -58,10 +58,11 @@ inline TimestampIts getTimestampITSFromGenerationDeltaTime(const GenerationDelta * @param n_leap_seconds number of leap-seconds since 2004. (Default: etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) * @return uint64_t the corresponding Unix-Nanoseconds */ -inline uint64_t getUnixNanosecondsFromGenerationDeltaTime(const GenerationDeltaTime& generation_delta_time, const TimestampIts& timestamp_estimate, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) -{ +inline uint64_t getUnixNanosecondsFromGenerationDeltaTime( + const GenerationDeltaTime& generation_delta_time, const TimestampIts& timestamp_estimate, + const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) { TimestampIts t_its = getTimestampITSFromGenerationDeltaTime(generation_delta_time, timestamp_estimate); - return t_its.value*1e6+etsi_its_msgs::UNIX_SECONDS_2004*1e9-n_leap_seconds*1e9; + return t_its.value * 1e6 + etsi_its_msgs::UNIX_SECONDS_2004 * 1e9 - n_leap_seconds * 1e9; } /** @@ -71,11 +72,12 @@ inline uint64_t getUnixNanosecondsFromGenerationDeltaTime(const GenerationDeltaT * @param unix_timestamp_estimate estimated unix-time (in Nanoseconds) to calculate the corresponding generation from * @return uint64_t the corresponding Unix-Nanoseconds */ -inline uint64_t getUnixNanosecondsFromGenerationDeltaTime(const GenerationDeltaTime& generation_delta_time, const uint64_t unix_timestamp_estimate, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) -{ +inline uint64_t getUnixNanosecondsFromGenerationDeltaTime( + const GenerationDeltaTime& generation_delta_time, const uint64_t unix_timestamp_estimate, + const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) { TimestampIts t_its; setTimestampITS(t_its, unix_timestamp_estimate, n_leap_seconds); return getUnixNanosecondsFromGenerationDeltaTime(generation_delta_time, t_its, n_leap_seconds); } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H \ No newline at end of file From e85ee044b0aea0e2daca970f91f734e133f676cd Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Wed, 9 Oct 2024 12:01:08 +0000 Subject: [PATCH 41/52] format cdd utils --- .../etsi_its_msgs_utils/impl/cdd/cdd_checks.h | 7 +-- .../impl/cdd/cdd_getters_common.h | 44 ++++++---------- .../impl/cdd/cdd_setters_common.h | 47 +++++++++-------- .../impl/cdd/cdd_v1-3-1_getters.h | 12 ++--- .../impl/cdd/cdd_v1-3-1_setters.h | 50 +++++++++---------- .../impl/cdd/cdd_v2-1-1_getters.h | 11 ++-- .../impl/cdd/cdd_v2-1-1_setters.h | 34 ++++++++----- 7 files changed, 101 insertions(+), 104 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h index fee0b9ea0..b643b8f01 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h @@ -32,10 +32,11 @@ SOFTWARE. #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H - template void throwIfOutOfRange(const T1& val, const T2& min, const T2& max, const std::string val_desc) { - if (val < min || val > max) throw std::invalid_argument(val_desc+" value is out of range ("+std::to_string(min)+"..."+std::to_string(max)+")!"); + if (val < min || val > max) + throw std::invalid_argument(val_desc + " value is out of range (" + std::to_string(min) + "..." + + std::to_string(max) + ")!"); } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h index c6c447d2f..517a438ad 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h @@ -29,21 +29,18 @@ SOFTWARE. * @brief Common getter functions for the ETSI ITS Common Data Dictionary (CDD) v1.3.1 and v2.1.1 */ -# ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H -# define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H #include - /** * @brief Get the StationID of ItsPduHeader * * @param header ItsPduHeader to get the StationID value from * @return stationID value */ -inline uint32_t getStationID(const ItsPduHeader& header){ - return header.station_id.value; -} +inline uint32_t getStationID(const ItsPduHeader& header) { return header.station_id.value; } /** * @brief Get the Latitude value @@ -51,9 +48,7 @@ inline uint32_t getStationID(const ItsPduHeader& header){ * @param latitude to get the Latitude value from * @return Latitude value in degree as decimal number */ -inline double getLatitude(const Latitude& latitude){ - return ((double)latitude.value)*1e-7; -} +inline double getLatitude(const Latitude& latitude) { return ((double)latitude.value) * 1e-7; } /** * @brief Get the Longitude value @@ -61,9 +56,7 @@ inline double getLatitude(const Latitude& latitude){ * @param longitude to get the Longitude value from * @return Longitude value in degree as decimal number */ -inline double getLongitude(const Longitude& longitude){ - return ((double)longitude.value)*1e-7; -} +inline double getLongitude(const Longitude& longitude) { return ((double)longitude.value) * 1e-7; } /** * @brief Get the Altitude value @@ -71,9 +64,7 @@ inline double getLongitude(const Longitude& longitude){ * @param altitude to get the Altitude value from * @return Altitude value (above the reference ellipsoid surface) in meter as decimal number */ -inline double getAltitude(const Altitude& altitude){ - return ((double)altitude.altitude_value.value)*1e-2; -} +inline double getAltitude(const Altitude& altitude) { return ((double)altitude.altitude_value.value) * 1e-2; } /** * @brief Get the Vehicle Width @@ -81,9 +72,7 @@ inline double getAltitude(const Altitude& altitude){ * @param vehicleWidth to get the vehicle width value from * @return vehicle width value in meter as decimal number */ -inline double getVehicleWidth(const VehicleWidth& vehicle_width){ - return ((double)vehicle_width.value)*1e-1; -} +inline double getVehicleWidth(const VehicleWidth& vehicle_width) { return ((double)vehicle_width.value) * 1e-1; } /** * @brief Get the vehicle speed @@ -91,9 +80,7 @@ inline double getVehicleWidth(const VehicleWidth& vehicle_width){ * @param speed to get the speed value from * @return speed value in m/s as decimal number */ -inline double getSpeed(const Speed& speed){ - return ((double)speed.speed_value.value)*1e-2; -} +inline double getSpeed(const Speed& speed) { return ((double)speed.speed_value.value) * 1e-2; } /** * @brief Get a Bit String in form of bool vector @@ -112,10 +99,8 @@ inline std::vector getBitString(const std::vector& buffer, const // loop over bytes in reverse order for (int byte_idx = n_bytes - 1; byte_idx >= 0; byte_idx--) { - // loop over bits in a byte for (int bit_idx_in_byte = 0; bit_idx_in_byte < bits_per_byte; bit_idx_in_byte++) { - // map bit index in byte to bit index in total bitstring int bit_idx = (n_bytes - byte_idx - 1) * bits_per_byte + bit_idx_in_byte; if (byte_idx == 0 && bit_idx >= n_bits - bits_unused) break; @@ -140,7 +125,7 @@ inline std::vector getBitString(const std::vector& buffer, const * @return gm::PointStamped geometry_msgs::PointStamped of the given position */ template -inline gm::PointStamped getUTMPosition(const T& reference_position, int& zone, bool& northp){ +inline gm::PointStamped getUTMPosition(const T& reference_position, int& zone, bool& northp) { gm::PointStamped utm_point; double latitude = getLatitude(reference_position.latitude); double longitude = getLongitude(reference_position.longitude); @@ -148,13 +133,16 @@ inline gm::PointStamped getUTMPosition(const T& reference_position, int& zone, b try { GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y); std::string hemisphere; - if(northp) hemisphere="N"; - else hemisphere="S"; - utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere; + if (northp) { + hemisphere = "N"; + } else { + hemisphere = "S"; + } + utm_point.header.frame_id = "utm_" + std::to_string(zone) + hemisphere; } catch (GeographicLib::GeographicErr& e) { throw std::invalid_argument(e.what()); } return utm_point; } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h index 6b83d81ff..18b5cbdf3 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h @@ -32,12 +32,10 @@ SOFTWARE. #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H -#include #include #include #include - - +#include /** * @brief Set the TimestampITS object @@ -47,8 +45,10 @@ SOFTWARE. * @param[in] n_leap_seconds Number of leap-seconds since 2004. (Default: etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) * @param[in] epoch_offset Unix-Timestamp in seconds for the 01.01.2004 at 00:00:00 */ -inline void setTimestampITS(TimestampIts& timestamp_its, const uint64_t unix_nanosecs, const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) { - uint64_t t_its = unix_nanosecs*1e-6 + (uint64_t)(n_leap_seconds*1e3) - etsi_its_msgs::UNIX_SECONDS_2004*1e3; +inline void setTimestampITS( + TimestampIts& timestamp_its, const uint64_t unix_nanosecs, + const uint16_t n_leap_seconds = etsi_its_msgs::LEAP_SECOND_INSERTIONS_SINCE_2004.end()->second) { + uint64_t t_its = unix_nanosecs * 1e-6 + (uint64_t)(n_leap_seconds * 1e3) - etsi_its_msgs::UNIX_SECONDS_2004 * 1e3; throwIfOutOfRange(t_its, TimestampIts::MIN, TimestampIts::MAX, "TimestampIts"); timestamp_its.value = t_its; } @@ -60,7 +60,7 @@ inline void setTimestampITS(TimestampIts& timestamp_its, const uint64_t unix_nan * @param deg Latitude value in degree as decimal number */ inline void setLatitude(Latitude& latitude, const double deg) { - int64_t angle_in_10_micro_degree = (int64_t)std::round(deg*1e7); + int64_t angle_in_10_micro_degree = (int64_t)std::round(deg * 1e7); throwIfOutOfRange(angle_in_10_micro_degree, Latitude::MIN, Latitude::MAX, "Latitude"); latitude.value = angle_in_10_micro_degree; } @@ -72,7 +72,7 @@ inline void setLatitude(Latitude& latitude, const double deg) { * @param deg Longitude value in degree as decimal number */ inline void setLongitude(Longitude& longitude, const double deg) { - int64_t angle_in_10_micro_degree = (int64_t)std::round(deg*1e7); + int64_t angle_in_10_micro_degree = (int64_t)std::round(deg * 1e7); throwIfOutOfRange(angle_in_10_micro_degree, Longitude::MIN, Longitude::MAX, "Longitude"); longitude.value = angle_in_10_micro_degree; } @@ -84,10 +84,14 @@ inline void setLongitude(Longitude& longitude, const double deg) { * @param value AltitudeValue value (above the reference ellipsoid surface) in meter as decimal number */ inline void setAltitudeValue(AltitudeValue& altitude, const double value) { - int64_t alt_in_cm = (int64_t)std::round(value*1e2); - if(alt_in_cm>=AltitudeValue::MIN && alt_in_cm<=AltitudeValue::MAX) altitude.value = alt_in_cm; - else if(alt_in_cmAltitudeValue::MAX) altitude.value = AltitudeValue::MAX; + int64_t alt_in_cm = (int64_t)std::round(value * 1e2); + if (alt_in_cm >= AltitudeValue::MIN && alt_in_cm <= AltitudeValue::MAX) { + altitude.value = alt_in_cm; + } else if (alt_in_cm < AltitudeValue::MIN) { + altitude.value = AltitudeValue::MIN; + } else if (alt_in_cm > AltitudeValue::MAX) { + altitude.value = AltitudeValue::MAX; + } } /** @@ -110,7 +114,7 @@ inline void setAltitude(Altitude& altitude, const double value) { * @param value VehicleWidth in meter as decimal number */ inline void setVehicleWidth(VehicleWidth& vehicle_width, const double value) { - int64_t width = (int64_t)std::round(value*1e1); + int64_t width = (int64_t)std::round(value * 1e1); throwIfOutOfRange(width, VehicleWidth::MIN, VehicleWidth::MAX, "VehicleWidthValue"); vehicle_width.value = width; } @@ -122,7 +126,7 @@ inline void setVehicleWidth(VehicleWidth& vehicle_width, const double value) { * @param value SpeedValue in m/s as decimal number */ inline void setSpeedValue(SpeedValue& speed, const double value) { - int64_t speed_val = (int64_t)std::round(value*1e2); + int64_t speed_val = (int64_t)std::round(value * 1e2); throwIfOutOfRange(speed_val, SpeedValue::MIN, SpeedValue::MAX, "SpeedValue"); speed.value = speed_val; } @@ -152,7 +156,8 @@ inline void setSpeed(Speed& speed, const double value) { * @param altitude The altitude value (above the reference ellipsoid surface) in meter as decimal number (optional). */ template -inline void setReferencePosition(T& ref_position, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE) { +inline void setReferencePosition(T& ref_position, const double latitude, const double longitude, + const double altitude = AltitudeValue::UNAVAILABLE) { setLatitude(ref_position.latitude, latitude); setLongitude(ref_position.longitude, longitude); if (altitude != AltitudeValue::UNAVAILABLE) { @@ -177,12 +182,12 @@ inline void setReferencePosition(T& ref_position, const double latitude, const d * @param[in] northp hemisphere (true means north, false means south) */ template -inline void setFromUTMPosition(T& reference_position, const gm::PointStamped& utm_position, const int zone, const bool northp) -{ +inline void setFromUTMPosition(T& reference_position, const gm::PointStamped& utm_position, const int zone, + const bool northp) { std::string required_frame_prefix = "utm_"; - if(utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) - { - throw std::invalid_argument("Frame-ID of UTM Position '"+utm_position.header.frame_id+"' does not start with required prefix '"+required_frame_prefix+"'!"); + if (utm_position.header.frame_id.rfind(required_frame_prefix, 0) != 0) { + throw std::invalid_argument("Frame-ID of UTM Position '" + utm_position.header.frame_id + + "' does not start with required prefix '" + required_frame_prefix + "'!"); } double latitude, longitude; try { @@ -213,10 +218,8 @@ inline void setBitString(T& bitstring, const std::vector& bits) { // loop over all bytes in reverse order for (int byte_idx = n_bytes - 1; byte_idx >= 0; byte_idx--) { - // loop over bits in a byte for (int bit_idx_in_byte = 0; bit_idx_in_byte < bits_per_byte; bit_idx_in_byte++) { - // map bit index in byte to bit index in total bitstring int bit_idx = (n_bytes - byte_idx - 1) * bits_per_byte + bit_idx_in_byte; if (byte_idx == 0 && bit_idx >= n_bits - bitstring.bits_unused) break; @@ -227,4 +230,4 @@ inline void setBitString(T& bitstring, const std::vector& bits) { } } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h index 20a21bb7d..18bc0fc51 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_getters.h @@ -33,8 +33,8 @@ SOFTWARE. #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H #include -#include +#include /** * @brief Get the lateral acceleration @@ -42,8 +42,8 @@ SOFTWARE. * @param longitudinalAcceleration to get the lateral acceleration from * @return lateral acceleration in m/s^2 as decimal number (left is positive) */ -inline double getLongitudinalAcceleration(const LongitudinalAcceleration& longitudinal_acceleration){ - return ((double)longitudinal_acceleration.longitudinal_acceleration_value.value)*1e-1; +inline double getLongitudinalAcceleration(const LongitudinalAcceleration& longitudinal_acceleration) { + return ((double)longitudinal_acceleration.longitudinal_acceleration_value.value) * 1e-1; } /** @@ -52,8 +52,8 @@ inline double getLongitudinalAcceleration(const LongitudinalAcceleration& longit * @param lateralAcceleration to get the lateral acceleration from * @return lateral acceleration in m/s^2 as decimal number (left is positive) */ -inline double getLateralAcceleration(const LateralAcceleration& lateral_acceleration){ - return ((double)lateral_acceleration.lateral_acceleration_value.value)*1e-1; +inline double getLateralAcceleration(const LateralAcceleration& lateral_acceleration) { + return ((double)lateral_acceleration.lateral_acceleration_value.value) * 1e-1; } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h index 10fce1816..13748547c 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h @@ -32,11 +32,10 @@ SOFTWARE. #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H -#include #include #include #include - +#include /** * @brief Set the Station Id object @@ -57,11 +56,13 @@ inline void setStationId(StationID& station_id, const uint32_t id_value) { * @param station_id * @param protocol_version */ -inline void setItsPduHeader(ItsPduHeader& header, const uint8_t message_id, const uint32_t station_id, const uint8_t protocol_version=0) { +inline void setItsPduHeader(ItsPduHeader& header, const uint8_t message_id, const uint32_t station_id, + const uint8_t protocol_version = 0) { setStationId(header.station_id, station_id); throwIfOutOfRange(message_id, ItsPduHeader::MESSAGE_ID_MIN, ItsPduHeader::MESSAGE_ID_MAX, "MessageID"); header.message_id = message_id; - throwIfOutOfRange(protocol_version, ItsPduHeader::PROTOCOL_VERSION_MIN, ItsPduHeader::PROTOCOL_VERSION_MAX, "ProtocolVersion"); + throwIfOutOfRange(protocol_version, ItsPduHeader::PROTOCOL_VERSION_MIN, ItsPduHeader::PROTOCOL_VERSION_MAX, + "ProtocolVersion"); header.protocol_version = protocol_version; } @@ -83,10 +84,14 @@ inline void setStationType(StationType& station_type, const uint8_t value) { * @param value LongitudinalAccelerationValue in m/s^2 as decimal number (braking is negative) */ inline void setLongitudinalAccelerationValue(LongitudinalAccelerationValue& accel, const double value) { - int64_t accel_val = (int64_t)std::round(value*1e1); - if(accel_val>=LongitudinalAccelerationValue::MIN && accel_val<=LongitudinalAccelerationValue::MAX) accel.value = accel_val; - else if(accel_valLongitudinalAccelerationValue::MAX) accel.value = LongitudinalAccelerationValue::MAX-1; + int64_t accel_val = (int64_t)std::round(value * 1e1); + if (accel_val >= LongitudinalAccelerationValue::MIN && accel_val <= LongitudinalAccelerationValue::MAX) { + accel.value = accel_val; + } else if (accel_val < LongitudinalAccelerationValue::MIN) { + accel.value = LongitudinalAccelerationValue::MIN; + } else if (accel_val > LongitudinalAccelerationValue::MAX) { + accel.value = LongitudinalAccelerationValue::MAX - 1; + } } /** @@ -102,17 +107,21 @@ inline void setLongitudinalAcceleration(LongitudinalAcceleration& accel, const d setLongitudinalAccelerationValue(accel.longitudinal_acceleration_value, value); } - /** +/** * @brief Set the LateralAccelerationValue object * * @param accel object to set * @param value LateralAccelerationValue in m/s^2 as decimal number (left is positive) */ inline void setLateralAccelerationValue(LateralAccelerationValue& accel, const double value) { - int64_t accel_val = (int64_t)std::round(value*1e1); - if(accel_val>=LateralAccelerationValue::MIN && accel_val<=LateralAccelerationValue::MAX) accel.value = accel_val; - else if(accel_valLateralAccelerationValue::MAX) accel.value = LateralAccelerationValue::MAX-1; + int64_t accel_val = (int64_t)std::round(value * 1e1); + if (accel_val >= LateralAccelerationValue::MIN && accel_val <= LateralAccelerationValue::MAX) { + accel.value = accel_val; + } else if (accel_val < LateralAccelerationValue::MIN) { + accel.value = LateralAccelerationValue::MIN; + } else if (accel_val > LateralAccelerationValue::MAX) { + accel.value = LateralAccelerationValue::MAX - 1; + } } /** @@ -128,17 +137,4 @@ inline void setLateralAcceleration(LateralAcceleration& accel, const double valu setLateralAccelerationValue(accel.lateral_acceleration_value, value); } -/** - * @brief Set the ReferencePosition from a given UTM-Position - * - * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS - * The z-Coordinate is directly used as altitude value - * The frame_id of the given utm_position must be set to 'utm_' - * - * @param[out] reference_position ReferencePosition to set - * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position - * @param[in] zone the UTM zone (zero means UPS) of the given position - * @param[in] northp hemisphere (true means north, false means south) - */ - -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h index 027f153e8..ade2fc874 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_getters.h @@ -33,6 +33,7 @@ SOFTWARE. #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H #include + #include /** @@ -41,8 +42,8 @@ SOFTWARE. * @param longitudinalAcceleration to get the lateral acceleration from * @return lateral acceleration in m/s^2 as decimal number (left is positive) */ -inline double getLongitudinalAcceleration(const AccelerationComponent& longitudinal_acceleration){ - return ((int16_t)longitudinal_acceleration.value.value)*1e-1; +inline double getLongitudinalAcceleration(const AccelerationComponent& longitudinal_acceleration) { + return ((int16_t)longitudinal_acceleration.value.value) * 1e-1; } /** @@ -51,8 +52,8 @@ inline double getLongitudinalAcceleration(const AccelerationComponent& longitudi * @param lateralAcceleration to get the lateral acceleration from * @return lateral acceleration in m/s^2 as decimal number (left is positive) */ -inline double getLateralAcceleration(const AccelerationComponent& lateral_acceleration){ - return ((int16_t)lateral_acceleration.value.value)*1e-1; +inline double getLateralAcceleration(const AccelerationComponent& lateral_acceleration) { + return ((int16_t)lateral_acceleration.value.value) * 1e-1; } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h index b44bdbf77..3978c9f90 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2-1-1_setters.h @@ -32,12 +32,11 @@ SOFTWARE. #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H -#include #include #include #include #include - +#include /** * @brief Set the Station Id object @@ -58,7 +57,8 @@ inline void setStationId(StationId& station_id, const uint32_t id_value) { * @param station_id * @param protocol_version */ -inline void setItsPduHeader(ItsPduHeader& header, const uint8_t message_id, const uint32_t station_id, const uint8_t protocol_version=0) { +inline void setItsPduHeader(ItsPduHeader& header, const uint8_t message_id, const uint32_t station_id, + const uint8_t protocol_version = 0) { setStationId(header.station_id, station_id); throwIfOutOfRange(message_id, MessageId::MIN, MessageId::MAX, "MessageID"); header.message_id.value = message_id; @@ -84,10 +84,14 @@ inline void setStationType(TrafficParticipantType& station_type, const uint8_t v * @param value LongitudinalAccelerationValue in m/s^2 as decimal number (braking is negative) */ inline void setLongitudinalAccelerationValue(AccelerationValue& accel, const double value) { - int64_t accel_val = (int64_t)std::round(value*1e1); - if(accel_val>=AccelerationValue::MIN && accel_val<=AccelerationValue::MAX) accel.value = accel_val; - else if(accel_valAccelerationValue::MAX) accel.value = AccelerationValue::MAX-1; + int64_t accel_val = (int64_t)std::round(value * 1e1); + if (accel_val >= AccelerationValue::MIN && accel_val <= AccelerationValue::MAX) { + accel.value = accel_val; + } else if (accel_val < AccelerationValue::MIN) { + accel.value = AccelerationValue::MIN; + } else if (accel_val > AccelerationValue::MAX) { + accel.value = AccelerationValue::MAX - 1; + } } /** @@ -103,17 +107,21 @@ inline void setLongitudinalAcceleration(AccelerationComponent& accel, const doub setLongitudinalAccelerationValue(accel.value, value); } - /** +/** * @brief Set the LateralAccelerationValue object * * @param accel object to set * @param value LateralAccelerationValue in m/s^2 as decimal number (left is positive) */ inline void setLateralAccelerationValue(AccelerationValue& accel, const double value) { - int64_t accel_val = (int64_t)std::round(value*1e1); - if(accel_val>=AccelerationValue::MIN && accel_val<=AccelerationValue::MAX) accel.value = accel_val; - else if(accel_valAccelerationValue::MAX) accel.value = AccelerationValue::MAX-1; + int64_t accel_val = (int64_t)std::round(value * 1e1); + if (accel_val >= AccelerationValue::MIN && accel_val <= AccelerationValue::MAX) { + accel.value = accel_val; + } else if (accel_val < AccelerationValue::MIN) { + accel.value = AccelerationValue::MIN; + } else if (accel_val > AccelerationValue::MAX) { + accel.value = AccelerationValue::MAX - 1; + } } /** @@ -129,4 +137,4 @@ inline void setLateralAcceleration(AccelerationComponent& accel, const double va setLateralAccelerationValue(accel.value, value); } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H \ No newline at end of file +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H \ No newline at end of file From c0955d673882530e765ebc6f4056228b88bb81d6 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Wed, 9 Oct 2024 12:07:50 +0000 Subject: [PATCH 42/52] format denm utils --- .../impl/denm/denm_access.h | 2 +- .../impl/denm/denm_getters.h | 860 ++++++++++-------- .../impl/denm/denm_setters.h | 324 ++++--- .../impl/denm/denm_utils.h | 9 +- 4 files changed, 630 insertions(+), 565 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h index 1d7778858..630db783f 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h @@ -50,4 +50,4 @@ SOFTWARE. namespace etsi_its_denm_msgs::access { #include -} // namespace etsi_its_denm_msgs::access \ No newline at end of file +} // namespace etsi_its_denm_msgs::access \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h index b4ba05735..7a125580c 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_getters.h @@ -35,428 +35,496 @@ namespace etsi_its_denm_msgs::access { #include - /** - * @brief Get the Station ID object - * - * @param denm DENM to get the StationID value from - * @return stationID value - */ - inline uint32_t getStationID(const DENM& denm){ - return getStationID(denm.header); - } +/** + * @brief Get the Station ID object + * + * @param denm DENM to get the StationID value from + * @return stationID value + */ +inline uint32_t getStationID(const DENM& denm) { return getStationID(denm.header); } - /** - * @brief Get the Reference Time object - * - * @param denm DENM to get the ReferenceTime-Value from - * @return TimestampIts - */ - inline TimestampIts getReferenceTime(const DENM& denm){ - return denm.denm.management.reference_time; - } +/** + * @brief Get the Reference Time object + * + * @param denm DENM to get the ReferenceTime-Value from + * @return TimestampIts + */ +inline TimestampIts getReferenceTime(const DENM& denm) { return denm.denm.management.reference_time; } - /** - * @brief Get the ReferenceTime-Value - * - * @param denm DENM to get the ReferenceTime-Value from - * @return uint64_t the ReferenceTime-Value - */ - inline uint64_t getReferenceTimeValue(const DENM& denm){ - return getReferenceTime(denm).value; - } +/** + * @brief Get the ReferenceTime-Value + * + * @param denm DENM to get the ReferenceTime-Value from + * @return uint64_t the ReferenceTime-Value + */ +inline uint64_t getReferenceTimeValue(const DENM& denm) { return getReferenceTime(denm).value; } - /** - * @brief Get the stationType object - * - * @param denm DENM to get the stationType value from - * @return stationType value - */ - inline uint8_t getStationType(const DENM& denm){ - return denm.denm.management.station_type.value; - } +/** + * @brief Get the stationType object + * + * @param denm DENM to get the stationType value from + * @return stationType value + */ +inline uint8_t getStationType(const DENM& denm) { return denm.denm.management.station_type.value; } - /** - * @brief Get the Latitude value of DENM - * - * @param denm DENM to get the Latitude value from - * @return Latitude value in degree as decimal number - */ - inline double getLatitude(const DENM& denm){ - return getLatitude(denm.denm.management.event_position.latitude); - } +/** + * @brief Get the Latitude value of DENM + * + * @param denm DENM to get the Latitude value from + * @return Latitude value in degree as decimal number + */ +inline double getLatitude(const DENM& denm) { return getLatitude(denm.denm.management.event_position.latitude); } - /** - * @brief Get the Longitude value of DENM - * - * @param denm DENM to get the Longitude value from - * @return Longitude value in degree as decimal number - */ - inline double getLongitude(const DENM& denm){ - return getLongitude(denm.denm.management.event_position.longitude); - } +/** + * @brief Get the Longitude value of DENM + * + * @param denm DENM to get the Longitude value from + * @return Longitude value in degree as decimal number + */ +inline double getLongitude(const DENM& denm) { return getLongitude(denm.denm.management.event_position.longitude); } - /** - * @brief Get the Altitude value of DENM - * - * @param denm DENM to get the Altitude value from - * @return Altitude value (above the reference ellipsoid surface) in meter as decimal number - */ - inline double getAltitude(const DENM& denm){ - return getAltitude(denm.denm.management.event_position.altitude); - } +/** + * @brief Get the Altitude value of DENM + * + * @param denm DENM to get the Altitude value from + * @return Altitude value (above the reference ellipsoid surface) in meter as decimal number + */ +inline double getAltitude(const DENM& denm) { return getAltitude(denm.denm.management.event_position.altitude); } - /** - * @brief Get the Heading value - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * - * @param heading to get the Heading value from - * @return Heading value in degree as decimal number - */ - inline double getHeading(const Heading& heading){ - return ((double)heading.heading_value.value)*1e-1; - } +/** + * @brief Get the Heading value + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * + * @param heading to get the Heading value from + * @return Heading value in degree as decimal number + */ +inline double getHeading(const Heading& heading) { return ((double)heading.heading_value.value) * 1e-1; } - /** - * @brief Get the Heading object - * - * @param denm DENM to get the Heading-Value from - * @return getHeading value - */ - inline double getHeading(const DENM& denm){ - if(denm.denm.location_is_present){ - if(denm.denm.location.event_position_heading_is_present){ - return getHeading(denm.denm.location.event_position_heading); - } - else{ - throw std::invalid_argument("Heading is not present!"); - } - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); - } - } - - /** - * @brief Get the IsHeadingPresent object - * - * @param denm DENM to get the IsHeadingPresent-Value from - * @return IsHeadingPresent-Value (true or false) - */ - inline bool getIsHeadingPresent(const DENM& denm){ - if(denm.denm.location_is_present){ - return denm.denm.location.event_position_heading_is_present; - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); +/** + * @brief Get the Heading object + * + * @param denm DENM to get the Heading-Value from + * @return getHeading value + */ +inline double getHeading(const DENM& denm) { + if (denm.denm.location_is_present) { + if (denm.denm.location.event_position_heading_is_present) { + return getHeading(denm.denm.location.event_position_heading); + } else { + throw std::invalid_argument("Heading is not present!"); } + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief Get the vehicle speed - * - * @param denm DENM to get the speed value from - * @return speed value in m/s as decimal number - */ - inline double getSpeed(const DENM& denm){ - if(denm.denm.location_is_present){ - if(denm.denm.location.event_speed_is_present){ - return getSpeed(denm.denm.location.event_speed); - } - else{ - throw std::invalid_argument("Speed is not present!"); - } - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); - } +/** + * @brief Get the IsHeadingPresent object + * + * @param denm DENM to get the IsHeadingPresent-Value from + * @return IsHeadingPresent-Value (true or false) + */ +inline bool getIsHeadingPresent(const DENM& denm) { + if (denm.denm.location_is_present) { + return denm.denm.location.event_position_heading_is_present; + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief Get the IsSpeedPresent object - * - * @param denm DENM to get the IsSpeedPresent-Value from - * @return IsSpeedPresent-Value (true or false) - */ - inline bool getIsSpeedPresent(const DENM& denm){ - if(denm.denm.location_is_present){ - return denm.denm.location.event_speed_is_present; - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); +/** + * @brief Get the vehicle speed + * + * @param denm DENM to get the speed value from + * @return speed value in m/s as decimal number + */ +inline double getSpeed(const DENM& denm) { + if (denm.denm.location_is_present) { + if (denm.denm.location.event_speed_is_present) { + return getSpeed(denm.denm.location.event_speed); + } else { + throw std::invalid_argument("Speed is not present!"); } + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief - * - * @param denm DENM to get the UTM Position from - * @param zone the UTM zone (zero means UPS) - * @param northp hemisphere (true means north, false means south) - * @return gm::PointStamped geometry_msgs::PointStamped of the given position - */ - inline gm::PointStamped getUTMPosition(const DENM& denm, int& zone, bool& northp){ - return getUTMPosition(denm.denm.management.event_position, zone, northp); +/** + * @brief Get the IsSpeedPresent object + * + * @param denm DENM to get the IsSpeedPresent-Value from + * @return IsSpeedPresent-Value (true or false) + */ +inline bool getIsSpeedPresent(const DENM& denm) { + if (denm.denm.location_is_present) { + return denm.denm.location.event_speed_is_present; + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief - * - * @param denm DENM to get the UTM Position from - * @return gm::PointStamped geometry_msgs::PointStamped of the given position - */ - inline gm::PointStamped getUTMPosition(const DENM& denm){ - int zone; - bool northp; - return getUTMPosition(denm.denm.management.event_position, zone, northp); - } +/** + * @brief + * + * @param denm DENM to get the UTM Position from + * @param zone the UTM zone (zero means UPS) + * @param northp hemisphere (true means north, false means south) + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ +inline gm::PointStamped getUTMPosition(const DENM& denm, int& zone, bool& northp) { + return getUTMPosition(denm.denm.management.event_position, zone, northp); +} - /** - * @brief Get the Cause Code object - * - * @param denm DENM to get the causeCode value from - * @return causeCode value - */ - inline uint8_t getCauseCode(const DENM& denm){ - return denm.denm.situation.event_type.cause_code.value; - } +/** + * @brief + * + * @param denm DENM to get the UTM Position from + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ +inline gm::PointStamped getUTMPosition(const DENM& denm) { + int zone; + bool northp; + return getUTMPosition(denm.denm.management.event_position, zone, northp); +} - /** - * @brief Get the Sub Cause Code object - * - * @param denm DENM to get the subCauseCode value from - * @return subCauseCode value - */ - inline uint8_t getSubCauseCode(const DENM& denm){ - return denm.denm.situation.event_type.sub_cause_code.value; - } +/** + * @brief Get the Cause Code object + * + * @param denm DENM to get the causeCode value from + * @return causeCode value + */ +inline uint8_t getCauseCode(const DENM& denm) { return denm.denm.situation.event_type.cause_code.value; } + +/** + * @brief Get the Sub Cause Code object + * + * @param denm DENM to get the subCauseCode value from + * @return subCauseCode value + */ +inline uint8_t getSubCauseCode(const DENM& denm) { return denm.denm.situation.event_type.sub_cause_code.value; } + +/** + * @brief Get the Cause Code Type object + * + * https://www.etsi.org/deliver/etsi_en/302600_302699/30263703/01.02.01_30/en_30263703v010201v.pdf + * + * @param denm DENM to get the causeCodeType value from + * @return causeCodeType value + */ +inline std::string getCauseCodeType(const DENM& denm) { + if (denm.denm.situation_is_present) { + int cause_code = getCauseCode(denm); + std::string cause_code_type = "undefined"; - /** - * @brief Get the Cause Code Type object - * - * https://www.etsi.org/deliver/etsi_en/302600_302699/30263703/01.02.01_30/en_30263703v010201v.pdf - * - * @param denm DENM to get the causeCodeType value from - * @return causeCodeType value - */ - inline std::string getCauseCodeType(const DENM& denm){ - if(denm.denm.situation_is_present){ - int cause_code = getCauseCode(denm); - std::string cause_code_type = "undefined"; - - if(cause_code == CauseCodeType().TRAFFIC_CONDITION) cause_code_type = "traffic condition"; - else if(cause_code == CauseCodeType().ACCIDENT) cause_code_type = "accident"; - else if(cause_code == CauseCodeType().ROADWORKS) cause_code_type = "roadworks"; - else if(cause_code == CauseCodeType().IMPASSABILITY) cause_code_type = "impassibility"; - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_ADHESION) cause_code_type = "adverse weather condition - adhesion"; - else if(cause_code == CauseCodeType().AQUAPLANNNING) cause_code_type = "aquaplanning"; - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_SURFACE_CONDITION) cause_code_type = "hazardous location - surface condition"; - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_OBSTACLE_ON_THE_ROAD) cause_code_type = "hazardous location - obstacle on the road"; - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_ANIMAL_ON_THE_ROAD) cause_code_type = "hazardous location - animal on the road"; - else if(cause_code == CauseCodeType().HUMAN_PRESENCE_ON_THE_ROAD) cause_code_type = "human presence on the road"; - else if(cause_code == CauseCodeType().WRONG_WAY_DRIVING) cause_code_type = "wrong way driving"; - else if(cause_code == CauseCodeType().RESCUE_AND_RECOVERY_WORK_IN_PROGRESS) cause_code_type = "rescue and recovery in progress"; - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_EXTREME_WEATHER_CONDITION) cause_code_type = "adverse weather condition - extreme weather condition"; - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_VISIBILITY) cause_code_type = "adverse weather condition - visibility"; - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_PRECIPITATION) cause_code_type = "adverse weather condition - precipitation"; - else if(cause_code == CauseCodeType().SLOW_VEHICLE) cause_code_type = "slow vehicle"; - else if(cause_code == CauseCodeType().DANGEROUS_END_OF_QUEUE) cause_code_type = "dangerous end of queue"; - else if(cause_code == CauseCodeType().VEHICLE_BREAKDOWN) cause_code_type = "vehicle breakdown"; - else if(cause_code == CauseCodeType().POST_CRASH) cause_code_type = "post crash"; - else if(cause_code == CauseCodeType().HUMAN_PROBLEM) cause_code_type = "human problem"; - else if(cause_code == CauseCodeType().STATIONARY_VEHICLE) cause_code_type = "stationary vehicle"; - else if(cause_code == CauseCodeType().EMERGENCY_VEHICLE_APPROACHING) cause_code_type = "emergency vehicle approaching"; - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_DANGEROUS_CURVE) cause_code_type = "hazardous location - dangerous curve"; - else if(cause_code == CauseCodeType().COLLISION_RISK) cause_code_type = "collision risk"; - else if(cause_code == CauseCodeType().SIGNAL_VIOLATION) cause_code_type = "signal violation"; - else if(cause_code == CauseCodeType().DANGEROUS_SITUATION) cause_code_type = "dangerous situation"; + if (cause_code == CauseCodeType().TRAFFIC_CONDITION) + cause_code_type = "traffic condition"; + else if (cause_code == CauseCodeType().ACCIDENT) + cause_code_type = "accident"; + else if (cause_code == CauseCodeType().ROADWORKS) + cause_code_type = "roadworks"; + else if (cause_code == CauseCodeType().IMPASSABILITY) + cause_code_type = "impassibility"; + else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_ADHESION) + cause_code_type = "adverse weather condition - adhesion"; + else if (cause_code == CauseCodeType().AQUAPLANNNING) + cause_code_type = "aquaplanning"; + else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_SURFACE_CONDITION) + cause_code_type = "hazardous location - surface condition"; + else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_OBSTACLE_ON_THE_ROAD) + cause_code_type = "hazardous location - obstacle on the road"; + else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_ANIMAL_ON_THE_ROAD) + cause_code_type = "hazardous location - animal on the road"; + else if (cause_code == CauseCodeType().HUMAN_PRESENCE_ON_THE_ROAD) + cause_code_type = "human presence on the road"; + else if (cause_code == CauseCodeType().WRONG_WAY_DRIVING) + cause_code_type = "wrong way driving"; + else if (cause_code == CauseCodeType().RESCUE_AND_RECOVERY_WORK_IN_PROGRESS) + cause_code_type = "rescue and recovery in progress"; + else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_EXTREME_WEATHER_CONDITION) + cause_code_type = "adverse weather condition - extreme weather condition"; + else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_VISIBILITY) + cause_code_type = "adverse weather condition - visibility"; + else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_PRECIPITATION) + cause_code_type = "adverse weather condition - precipitation"; + else if (cause_code == CauseCodeType().SLOW_VEHICLE) + cause_code_type = "slow vehicle"; + else if (cause_code == CauseCodeType().DANGEROUS_END_OF_QUEUE) + cause_code_type = "dangerous end of queue"; + else if (cause_code == CauseCodeType().VEHICLE_BREAKDOWN) + cause_code_type = "vehicle breakdown"; + else if (cause_code == CauseCodeType().POST_CRASH) + cause_code_type = "post crash"; + else if (cause_code == CauseCodeType().HUMAN_PROBLEM) + cause_code_type = "human problem"; + else if (cause_code == CauseCodeType().STATIONARY_VEHICLE) + cause_code_type = "stationary vehicle"; + else if (cause_code == CauseCodeType().EMERGENCY_VEHICLE_APPROACHING) + cause_code_type = "emergency vehicle approaching"; + else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_DANGEROUS_CURVE) + cause_code_type = "hazardous location - dangerous curve"; + else if (cause_code == CauseCodeType().COLLISION_RISK) + cause_code_type = "collision risk"; + else if (cause_code == CauseCodeType().SIGNAL_VIOLATION) + cause_code_type = "signal violation"; + else if (cause_code == CauseCodeType().DANGEROUS_SITUATION) + cause_code_type = "dangerous situation"; - return cause_code_type; - } - else{ - throw std::invalid_argument("SituationContainer is not present!"); - } + return cause_code_type; + } else { + throw std::invalid_argument("SituationContainer is not present!"); } +} - /** - * @brief Get the Sub Cause Code Type object - * - * https://www.etsi.org/deliver/etsi_en/302600_302699/30263703/01.02.01_30/en_30263703v010201v.pdf - * - * @param denm DENM to get the subCauseCodeType value from - * @return causeCodeType value - */ - inline std::string getSubCauseCodeType(const DENM& denm){ - if(denm.denm.situation_is_present){ - int cause_code = getCauseCode(denm); - int sub_cause_code = getSubCauseCode(denm); - std::string sub_cause_code_type = "undefined"; - if(cause_code == CauseCodeType().TRAFFIC_CONDITION) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "not defined"; - else if(sub_cause_code == 2) sub_cause_code_type = "traffic jam slowly increasing"; - else if(sub_cause_code == 3) sub_cause_code_type = "traffic jam increasing"; - else if(sub_cause_code == 4) sub_cause_code_type = "traffic jam strongly increasing"; - else if(sub_cause_code == 5) sub_cause_code_type = "traffic stationary"; - else if(sub_cause_code == 6) sub_cause_code_type = "traffic jam slightly decreasing"; - else if(sub_cause_code == 7) sub_cause_code_type = "traffic jam decreasing"; - else if(sub_cause_code == 8) sub_cause_code_type = "traffic jam strongly decreasing"; - } - else if(cause_code == CauseCodeType().ACCIDENT) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 7) sub_cause_code_type = "not defined"; - else if(sub_cause_code == 8) sub_cause_code_type = "assistance requested (e-Call)"; - } - else if(cause_code == CauseCodeType().ROADWORKS) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 3) sub_cause_code_type = "not defined"; - else if(sub_cause_code == 4) sub_cause_code_type = "short-term stationary roadworks"; - else if(sub_cause_code == 5) sub_cause_code_type = "street cleaning"; - else if(sub_cause_code == 6) sub_cause_code_type = "winter service"; - } - else if(cause_code == CauseCodeType().IMPASSABILITY) sub_cause_code_type = "not defined"; - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_ADHESION) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 10) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().AQUAPLANNNING) sub_cause_code_type = "not defined"; - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_SURFACE_CONDITION) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 9) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_OBSTACLE_ON_THE_ROAD) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 7) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_ANIMAL_ON_THE_ROAD) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 4) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().HUMAN_PRESENCE_ON_THE_ROAD) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 3) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().WRONG_WAY_DRIVING) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "vehicle driving in wrong lane"; - else if(sub_cause_code == 2) sub_cause_code_type = "vehicle driving in wrong driving direction"; - } - else if(cause_code == CauseCodeType().RESCUE_AND_RECOVERY_WORK_IN_PROGRESS) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 5) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_EXTREME_WEATHER_CONDITION) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 6) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_VISIBILITY) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 8) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_PRECIPITATION) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 3) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().SLOW_VEHICLE) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 8) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().DANGEROUS_END_OF_QUEUE) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code >= 1 && sub_cause_code <= 8) sub_cause_code_type = "not defined"; - } - else if(cause_code == CauseCodeType().VEHICLE_BREAKDOWN) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "lack of fuel"; - else if(sub_cause_code == 2) sub_cause_code_type = "lack of battery"; - else if(sub_cause_code == 3) sub_cause_code_type = "engine problem"; - else if(sub_cause_code == 4) sub_cause_code_type = "transmission problem"; - else if(sub_cause_code == 5) sub_cause_code_type = "engine cooling problem"; - else if(sub_cause_code == 6) sub_cause_code_type = "braking system problem"; - else if(sub_cause_code == 7) sub_cause_code_type = "steering problem"; - else if(sub_cause_code == 8) sub_cause_code_type = "tyre puncture"; - } - else if(cause_code == CauseCodeType().POST_CRASH) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "accident without e-Call triggered"; - else if(sub_cause_code == 2) sub_cause_code_type = "accident with e-Call manually triggered"; - else if(sub_cause_code == 3) sub_cause_code_type = "accident with e-Call automatical triggered"; - else if(sub_cause_code == 4) sub_cause_code_type = "accident with e-Call triggered without a possible access to a cell network"; - } - else if(cause_code == CauseCodeType().HUMAN_PROBLEM) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "glycaemia problem"; - else if(sub_cause_code == 2) sub_cause_code_type = "heart problem"; - } - else if(cause_code == CauseCodeType().STATIONARY_VEHICLE) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "human problem"; - else if(sub_cause_code == 2) sub_cause_code_type = "vehicle breakdown"; - else if(sub_cause_code == 3) sub_cause_code_type = "post crash"; - else if(sub_cause_code == 4) sub_cause_code_type = "public transport stop"; - else if(sub_cause_code == 5) sub_cause_code_type = "carrying dangerous goods"; - } - else if(cause_code == CauseCodeType().EMERGENCY_VEHICLE_APPROACHING) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "emergency vehicle approaching"; - else if(sub_cause_code == 2) sub_cause_code_type = "prioritized vehicle approaching"; - } - else if(cause_code == CauseCodeType().HAZARDOUS_LOCATION_DANGEROUS_CURVE) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "dangerous left turn curve"; - else if(sub_cause_code == 2) sub_cause_code_type = "dangerous right turn curve"; - else if(sub_cause_code == 3) sub_cause_code_type = "multiple curves starting with unknown turning direction"; - else if(sub_cause_code == 4) sub_cause_code_type = "multiple curves starting with left turn"; - else if(sub_cause_code == 5) sub_cause_code_type = "multiple curves starting with right turn"; - } - else if(cause_code == CauseCodeType().COLLISION_RISK) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "longitudinal collision risk"; - else if(sub_cause_code == 2) sub_cause_code_type = "crossing collision risk"; - else if(sub_cause_code == 3) sub_cause_code_type = "lateral collision risk"; - else if(sub_cause_code == 4) sub_cause_code_type = "collision risk involving vulnerable road user"; - } - else if(cause_code == CauseCodeType().SIGNAL_VIOLATION) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "stop sign violation"; - else if(sub_cause_code == 2) sub_cause_code_type = "traffic light violation"; - else if(sub_cause_code == 3) sub_cause_code_type = "turning regulation violation"; - } - else if(cause_code == CauseCodeType().DANGEROUS_SITUATION) { - if(sub_cause_code == 0) sub_cause_code_type = "unavailable"; - else if(sub_cause_code == 1) sub_cause_code_type = "emergency electronic break lights"; - else if(sub_cause_code == 2) sub_cause_code_type = "pre-crash system activated"; - else if(sub_cause_code == 3) sub_cause_code_type = "ESP (electronic stability program) activated"; - else if(sub_cause_code == 4) sub_cause_code_type = "ABS (anti-lock breaking system) activated"; - else if(sub_cause_code == 5) sub_cause_code_type = "AEB (automatic emergency breaking) activated"; - else if(sub_cause_code == 6) sub_cause_code_type = "break warning activated"; - else if(sub_cause_code == 7) sub_cause_code_type = "collision risk warning activated"; - } - return sub_cause_code_type; - } - else{ - throw std::invalid_argument("SituationContainer is not present!"); +/** + * @brief Get the Sub Cause Code Type object + * + * https://www.etsi.org/deliver/etsi_en/302600_302699/30263703/01.02.01_30/en_30263703v010201v.pdf + * + * @param denm DENM to get the subCauseCodeType value from + * @return causeCodeType value + */ +inline std::string getSubCauseCodeType(const DENM& denm) { + if (denm.denm.situation_is_present) { + int cause_code = getCauseCode(denm); + int sub_cause_code = getSubCauseCode(denm); + std::string sub_cause_code_type = "undefined"; + if (cause_code == CauseCodeType().TRAFFIC_CONDITION) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "not defined"; + else if (sub_cause_code == 2) + sub_cause_code_type = "traffic jam slowly increasing"; + else if (sub_cause_code == 3) + sub_cause_code_type = "traffic jam increasing"; + else if (sub_cause_code == 4) + sub_cause_code_type = "traffic jam strongly increasing"; + else if (sub_cause_code == 5) + sub_cause_code_type = "traffic stationary"; + else if (sub_cause_code == 6) + sub_cause_code_type = "traffic jam slightly decreasing"; + else if (sub_cause_code == 7) + sub_cause_code_type = "traffic jam decreasing"; + else if (sub_cause_code == 8) + sub_cause_code_type = "traffic jam strongly decreasing"; + } else if (cause_code == CauseCodeType().ACCIDENT) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 7) + sub_cause_code_type = "not defined"; + else if (sub_cause_code == 8) + sub_cause_code_type = "assistance requested (e-Call)"; + } else if (cause_code == CauseCodeType().ROADWORKS) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 3) + sub_cause_code_type = "not defined"; + else if (sub_cause_code == 4) + sub_cause_code_type = "short-term stationary roadworks"; + else if (sub_cause_code == 5) + sub_cause_code_type = "street cleaning"; + else if (sub_cause_code == 6) + sub_cause_code_type = "winter service"; + } else if (cause_code == CauseCodeType().IMPASSABILITY) + sub_cause_code_type = "not defined"; + else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_ADHESION) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 10) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().AQUAPLANNNING) + sub_cause_code_type = "not defined"; + else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_SURFACE_CONDITION) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 9) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_OBSTACLE_ON_THE_ROAD) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 7) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_ANIMAL_ON_THE_ROAD) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 4) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().HUMAN_PRESENCE_ON_THE_ROAD) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 3) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().WRONG_WAY_DRIVING) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "vehicle driving in wrong lane"; + else if (sub_cause_code == 2) + sub_cause_code_type = "vehicle driving in wrong driving direction"; + } else if (cause_code == CauseCodeType().RESCUE_AND_RECOVERY_WORK_IN_PROGRESS) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 5) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_EXTREME_WEATHER_CONDITION) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 6) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_VISIBILITY) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 8) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().ADVERSE_WEATHER_CONDITION_PRECIPITATION) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 3) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().SLOW_VEHICLE) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 8) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().DANGEROUS_END_OF_QUEUE) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code >= 1 && sub_cause_code <= 8) + sub_cause_code_type = "not defined"; + } else if (cause_code == CauseCodeType().VEHICLE_BREAKDOWN) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "lack of fuel"; + else if (sub_cause_code == 2) + sub_cause_code_type = "lack of battery"; + else if (sub_cause_code == 3) + sub_cause_code_type = "engine problem"; + else if (sub_cause_code == 4) + sub_cause_code_type = "transmission problem"; + else if (sub_cause_code == 5) + sub_cause_code_type = "engine cooling problem"; + else if (sub_cause_code == 6) + sub_cause_code_type = "braking system problem"; + else if (sub_cause_code == 7) + sub_cause_code_type = "steering problem"; + else if (sub_cause_code == 8) + sub_cause_code_type = "tyre puncture"; + } else if (cause_code == CauseCodeType().POST_CRASH) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "accident without e-Call triggered"; + else if (sub_cause_code == 2) + sub_cause_code_type = "accident with e-Call manually triggered"; + else if (sub_cause_code == 3) + sub_cause_code_type = "accident with e-Call automatical triggered"; + else if (sub_cause_code == 4) + sub_cause_code_type = "accident with e-Call triggered without a possible access to a cell network"; + } else if (cause_code == CauseCodeType().HUMAN_PROBLEM) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "glycaemia problem"; + else if (sub_cause_code == 2) + sub_cause_code_type = "heart problem"; + } else if (cause_code == CauseCodeType().STATIONARY_VEHICLE) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "human problem"; + else if (sub_cause_code == 2) + sub_cause_code_type = "vehicle breakdown"; + else if (sub_cause_code == 3) + sub_cause_code_type = "post crash"; + else if (sub_cause_code == 4) + sub_cause_code_type = "public transport stop"; + else if (sub_cause_code == 5) + sub_cause_code_type = "carrying dangerous goods"; + } else if (cause_code == CauseCodeType().EMERGENCY_VEHICLE_APPROACHING) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "emergency vehicle approaching"; + else if (sub_cause_code == 2) + sub_cause_code_type = "prioritized vehicle approaching"; + } else if (cause_code == CauseCodeType().HAZARDOUS_LOCATION_DANGEROUS_CURVE) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "dangerous left turn curve"; + else if (sub_cause_code == 2) + sub_cause_code_type = "dangerous right turn curve"; + else if (sub_cause_code == 3) + sub_cause_code_type = "multiple curves starting with unknown turning direction"; + else if (sub_cause_code == 4) + sub_cause_code_type = "multiple curves starting with left turn"; + else if (sub_cause_code == 5) + sub_cause_code_type = "multiple curves starting with right turn"; + } else if (cause_code == CauseCodeType().COLLISION_RISK) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "longitudinal collision risk"; + else if (sub_cause_code == 2) + sub_cause_code_type = "crossing collision risk"; + else if (sub_cause_code == 3) + sub_cause_code_type = "lateral collision risk"; + else if (sub_cause_code == 4) + sub_cause_code_type = "collision risk involving vulnerable road user"; + } else if (cause_code == CauseCodeType().SIGNAL_VIOLATION) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "stop sign violation"; + else if (sub_cause_code == 2) + sub_cause_code_type = "traffic light violation"; + else if (sub_cause_code == 3) + sub_cause_code_type = "turning regulation violation"; + } else if (cause_code == CauseCodeType().DANGEROUS_SITUATION) { + if (sub_cause_code == 0) + sub_cause_code_type = "unavailable"; + else if (sub_cause_code == 1) + sub_cause_code_type = "emergency electronic break lights"; + else if (sub_cause_code == 2) + sub_cause_code_type = "pre-crash system activated"; + else if (sub_cause_code == 3) + sub_cause_code_type = "ESP (electronic stability program) activated"; + else if (sub_cause_code == 4) + sub_cause_code_type = "ABS (anti-lock breaking system) activated"; + else if (sub_cause_code == 5) + sub_cause_code_type = "AEB (automatic emergency breaking) activated"; + else if (sub_cause_code == 6) + sub_cause_code_type = "break warning activated"; + else if (sub_cause_code == 7) + sub_cause_code_type = "collision risk warning activated"; } + return sub_cause_code_type; + } else { + throw std::invalid_argument("SituationContainer is not present!"); } +} - /** - * @brief Get the Driving Lane Status in form of bool vector - * - * @param driving_lane_status - * @return std::vector - */ - inline std::vector getDrivingLaneStatus(const DrivingLaneStatus& driving_lane_status){ - return getBitString(driving_lane_status.value, driving_lane_status.bits_unused); - } +/** + * @brief Get the Driving Lane Status in form of bool vector + * + * @param driving_lane_status + * @return std::vector + */ +inline std::vector getDrivingLaneStatus(const DrivingLaneStatus& driving_lane_status) { + return getBitString(driving_lane_status.value, driving_lane_status.bits_unused); +} - /** - * @brief Get the Lightbar Siren In Use in form of bool vector - * - * @param light_bar_siren_in_use - * @return std::vector - */ - inline std::vector getLightBarSirenInUse(const LightBarSirenInUse& light_bar_siren_in_use) { - return getBitString(light_bar_siren_in_use.value, light_bar_siren_in_use.bits_unused); - } +/** + * @brief Get the Lightbar Siren In Use in form of bool vector + * + * @param light_bar_siren_in_use + * @return std::vector + */ +inline std::vector getLightBarSirenInUse(const LightBarSirenInUse& light_bar_siren_in_use) { + return getBitString(light_bar_siren_in_use.value, light_bar_siren_in_use.bits_unused); +} -} // namespace etsi_its_denm_msgs::access +} // namespace etsi_its_denm_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h index 313c9122e..d76862746 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_setters.h @@ -37,184 +37,180 @@ namespace etsi_its_denm_msgs::access { #include - /** - * @brief Set the ItsPduHeader-object for a DENM - * - * @param denm DENM-Message to set the ItsPduHeader - * @param station_id - * @param protocol_version - */ - inline void setItsPduHeader(DENM& denm, const uint32_t station_id, const uint8_t protocol_version = 0){ - setItsPduHeader(denm.header, ItsPduHeader::MESSAGE_ID_DENM, station_id, protocol_version); - } +/** + * @brief Set the ItsPduHeader-object for a DENM + * + * @param denm DENM-Message to set the ItsPduHeader + * @param station_id + * @param protocol_version + */ +inline void setItsPduHeader(DENM& denm, const uint32_t station_id, const uint8_t protocol_version = 0) { + setItsPduHeader(denm.header, ItsPduHeader::MESSAGE_ID_DENM, station_id, protocol_version); +} - /** - * @brief Set the ReferenceTime-value - * - * @param denm DENM 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(DENM& denm, 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"); - denm.denm.management.reference_time = t_its; - } +/** + * @brief Set the ReferenceTime-value + * + * @param denm DENM 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( + DENM& denm, 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"); + denm.denm.management.reference_time = t_its; +} - /** - * @brief Set the StationType for a DENM - * - * @param denm DENM-Message to set the station_type value - * @param value station_type value to set - */ - inline void setStationType(DENM& denm, const int value){ - setStationType(denm.denm.management.station_type, value); - } +/** + * @brief Set the StationType for a DENM + * + * @param denm DENM-Message to set the station_type value + * @param value station_type value to set + */ +inline void setStationType(DENM& denm, const int value) { setStationType(denm.denm.management.station_type, value); } - /** - * @brief Set the ReferencePositionWithConfidence for a DENM - * - * This function sets the latitude, longitude, and altitude of the DENMs reference position. - * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. - * - * @param denm DENM to set the ReferencePosition - * @param latitude The latitude value position in degree as decimal number. - * @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(DENM& denm, const double latitude, const double longitude, const double altitude = AltitudeValue::UNAVAILABLE) { - setReferencePosition(denm.denm.management.event_position, latitude, longitude, altitude); - } +/** + * @brief Set the ReferencePositionWithConfidence for a DENM + * + * This function sets the latitude, longitude, and altitude of the DENMs reference position. + * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. + * + * @param denm DENM to set the ReferencePosition + * @param latitude The latitude value position in degree as decimal number. + * @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(DENM& denm, const double latitude, const double longitude, + const double altitude = AltitudeValue::UNAVAILABLE) { + setReferencePosition(denm.denm.management.event_position, latitude, longitude, altitude); +} - /** - * @brief Set the HeadingValue object - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * - * @param heading object to set - * @param value Heading value in degree as decimal number - */ - inline void setHeadingValue(HeadingValue& heading, const double value) { - int64_t deg = (int64_t)std::round(value*1e1); - throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); - heading.value = deg; - } +/** + * @brief Set the HeadingValue object + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * + * @param heading object to set + * @param value Heading value in degree as decimal number + */ +inline void setHeadingValue(HeadingValue& heading, const double value) { + int64_t deg = (int64_t)std::round(value * 1e1); + throwIfOutOfRange(deg, HeadingValue::MIN, HeadingValue::MAX, "HeadingValue"); + heading.value = deg; +} - /** - * @brief Set the Heading object - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * HeadingConfidence is set to UNAVAILABLE - * - * @param heading object to set - * @param value Heading value in degree as decimal number - */ - inline void setHeading(Heading& heading, const double value) { - heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; - setHeadingValue(heading.heading_value, value); - } +/** + * @brief Set the Heading object + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * HeadingConfidence is set to UNAVAILABLE + * + * @param heading object to set + * @param value Heading value in degree as decimal number + */ +inline void setHeading(Heading& heading, const double value) { + heading.heading_confidence.value = HeadingConfidence::UNAVAILABLE; + setHeadingValue(heading.heading_value, value); +} - /** - * @brief Set the IsHeadingPresent object for DENM - * - * @param denm DENM to set IsHeadingPresent - * @param presence_of_heading IsHeadingPresent-Value (true or false) - */ - inline void setIsHeadingPresent(DENM& denm, bool presence_of_heading){ - if(denm.denm.location_is_present){ - denm.denm.location.event_position_heading_is_present = presence_of_heading; - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); - } +/** + * @brief Set the IsHeadingPresent object for DENM + * + * @param denm DENM to set IsHeadingPresent + * @param presence_of_heading IsHeadingPresent-Value (true or false) + */ +inline void setIsHeadingPresent(DENM& denm, bool presence_of_heading) { + if (denm.denm.location_is_present) { + denm.denm.location.event_position_heading_is_present = presence_of_heading; + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief Set the Heading for a DENM - * - * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West - * HeadingConfidence is set to UNAVAILABLE - * - * @param denm DENM to set the ReferencePosition - * @param value Heading value in degree as decimal number - */ - inline void setHeading(DENM& denm, const double heading_val){ - if(denm.denm.location_is_present){ - setHeading(denm.denm.location.event_position_heading, heading_val); - setIsHeadingPresent(denm, true); - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); - } +/** + * @brief Set the Heading for a DENM + * + * 0.0° equals WGS84 North, 90.0° equals WGS84 East, 180.0° equals WGS84 South and 270.0° equals WGS84 West + * HeadingConfidence is set to UNAVAILABLE + * + * @param denm DENM to set the ReferencePosition + * @param value Heading value in degree as decimal number + */ +inline void setHeading(DENM& denm, const double heading_val) { + if (denm.denm.location_is_present) { + setHeading(denm.denm.location.event_position_heading, heading_val); + setIsHeadingPresent(denm, true); + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief Set the IsSpeedPresent object for DENM - * - * @param denm DENM to set IsSpeedPresent - * @param presence_of_heading IsSpeedPresent-Value (true or false) - */ - inline void setIsSpeedPresent(DENM& denm, bool presence_of_speed){ - if(denm.denm.location_is_present){ - denm.denm.location.event_speed_is_present = presence_of_speed; - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); - } +/** + * @brief Set the IsSpeedPresent object for DENM + * + * @param denm DENM to set IsSpeedPresent + * @param presence_of_heading IsSpeedPresent-Value (true or false) + */ +inline void setIsSpeedPresent(DENM& denm, bool presence_of_speed) { + if (denm.denm.location_is_present) { + denm.denm.location.event_speed_is_present = presence_of_speed; + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief Set the vehicle speed - * - * @param denm DENM to set the speed value - * @param speed_val speed value to set in m/s as decimal number - */ - inline void setSpeed(DENM& denm, const double speed_val){ - if(denm.denm.location_is_present){ - setSpeed(denm.denm.location.event_speed, speed_val); - setIsSpeedPresent(denm, true); - } - else{ - throw std::invalid_argument("LocationContainer is not present!"); - } +/** + * @brief Set the vehicle speed + * + * @param denm DENM to set the speed value + * @param speed_val speed value to set in m/s as decimal number + */ +inline void setSpeed(DENM& denm, const double speed_val) { + if (denm.denm.location_is_present) { + setSpeed(denm.denm.location.event_speed, speed_val); + setIsSpeedPresent(denm, true); + } else { + throw std::invalid_argument("LocationContainer is not present!"); } +} - /** - * @brief Set the ReferencePosition of a DENM from a given UTM-Position - * - * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS - * The z-Coordinate is directly used as altitude value - * The frame_id of the given utm_position must be set to 'utm_' - * - * @param[out] denm DENM for which to set the ReferencePosition - * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position - * @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(DENM& denm, const gm::PointStamped& utm_position, const int& zone, const bool& northp) - { - setFromUTMPosition(denm.denm.management.event_position, utm_position, zone, northp); - } +/** + * @brief Set the ReferencePosition of a DENM from a given UTM-Position + * + * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS + * The z-Coordinate is directly used as altitude value + * The frame_id of the given utm_position must be set to 'utm_' + * + * @param[out] denm DENM for which to set the ReferencePosition + * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position + * @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(DENM& denm, const gm::PointStamped& utm_position, const int& zone, const bool& northp) { + setFromUTMPosition(denm.denm.management.event_position, utm_position, zone, northp); +} - /** - * @brief Set the Driving Lane Status by a vector of bools - * - * @param driving_lane_status - * @param bits - */ - inline void setDrivingLaneStatus(DrivingLaneStatus& driving_lane_status, const std::vector& bits) { - setBitString(driving_lane_status, bits); - } +/** + * @brief Set the Driving Lane Status by a vector of bools + * + * @param driving_lane_status + * @param bits + */ +inline void setDrivingLaneStatus(DrivingLaneStatus& driving_lane_status, const std::vector& bits) { + setBitString(driving_lane_status, bits); +} - /** - * @brief Set the Lightbar Siren In Use by a vector of bools - * - * @param light_bar_siren_in_use - * @param bits - */ - inline void setLightBarSirenInUse(LightBarSirenInUse& light_bar_siren_in_use, const std::vector& bits) { - setBitString(light_bar_siren_in_use, bits); - } +/** + * @brief Set the Lightbar Siren In Use by a vector of bools + * + * @param light_bar_siren_in_use + * @param bits + */ +inline void setLightBarSirenInUse(LightBarSirenInUse& light_bar_siren_in_use, const std::vector& bits) { + setBitString(light_bar_siren_in_use, bits); +} -} // namespace etsi_its_denm_msgs::access +} // namespace etsi_its_denm_msgs::access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h index 4a17bea49..ea1640bd0 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_utils.h @@ -41,8 +41,9 @@ SOFTWARE. * @param n_leap_seconds number of leap-seconds since 2004. (Default: etsi_its_msgs::N_LEAP_SECONDS) * @return uint64_t the corresponding Unix-Nanoseconds */ -inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time){ - double unix_time_with_leap_seconds = reference_time.value*1e-3+etsi_its_msgs::UNIX_SECONDS_2004; - uint16_t n_leap_seconds = etsi_its_msgs::getLeapSecondInsertionsSince2004(static_cast(unix_time_with_leap_seconds)); - return (unix_time_with_leap_seconds - n_leap_seconds) * 1e9; +inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time) { + double unix_time_with_leap_seconds = reference_time.value * 1e-3 + etsi_its_msgs::UNIX_SECONDS_2004; + uint16_t n_leap_seconds = + etsi_its_msgs::getLeapSecondInsertionsSince2004(static_cast(unix_time_with_leap_seconds)); + return (unix_time_with_leap_seconds - n_leap_seconds) * 1e9; } \ No newline at end of file From d54211269c23fcec871d0847e85377d93f8076f0 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Wed, 9 Oct 2024 12:21:16 +0000 Subject: [PATCH 43/52] format access tests --- .../etsi_its_msgs_utils/impl/constants.h | 34 +++++++++--------- .../test/impl/test_cam_access.cpp | 36 +++++++++++-------- .../test/impl/test_cam_ts_access.cpp | 36 +++++++++++-------- .../test/impl/test_cpm_ts_access.cpp | 18 +++++----- .../test/impl/test_denm_access.cpp | 14 ++++---- etsi_its_msgs_utils/test/test_access.cpp | 4 +-- etsi_its_msgs_utils/test/test_access.ros2.cpp | 4 +-- 7 files changed, 76 insertions(+), 70 deletions(-) diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/constants.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/constants.h index 74f12c52b..101af0228 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/constants.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/constants.h @@ -36,19 +36,19 @@ SOFTWARE. namespace etsi_its_msgs { -const uint64_t UNIX_SECONDS_2004 = 1072915200; // Unix-Seconds for 2004-01-01T00:00:00.000Z +const uint64_t UNIX_SECONDS_2004 = 1072915200; // Unix-Seconds for 2004-01-01T00:00:00.000Z /** * @brief std::map that stores all leap second insertions since 2004 with the corresponding unix-date of the insertion * */ -const std::map LEAP_SECOND_INSERTIONS_SINCE_2004 { - {UNIX_SECONDS_2004, 0}, - {1136073599, 1}, // 2005-12-31T23:59:59.000Z - {1230767999, 2}, // 2008-12-31T23:59:59.000Z - {1341100799, 3}, // 2012-06-30T23:59:59.000Z - {1435708799, 4}, // 2015-06-30T23:59:59.000Z - {1483228799, 5} // 2016-12-31T23:59:59.000Z +const std::map LEAP_SECOND_INSERTIONS_SINCE_2004{ + {UNIX_SECONDS_2004, 0}, // 2004-01-01T00:00:00.000Z + {1136073599, 1}, // 2005-12-31T23:59:59.000Z + {1230767999, 2}, // 2008-12-31T23:59:59.000Z + {1341100799, 3}, // 2012-06-30T23:59:59.000Z + {1435708799, 4}, // 2015-06-30T23:59:59.000Z + {1483228799, 5} // 2016-12-31T23:59:59.000Z }; /** @@ -57,14 +57,14 @@ const std::map LEAP_SECOND_INSERTIONS_SINCE_2004 { * @param unix_seconds the current unix seconds for that the leap second insertions since 2004 shall be provided * @return uint16_t the number of leap second insertions since 2004 for unix_seconds */ -inline uint16_t getLeapSecondInsertionsSince2004(const uint64_t unix_seconds) -{ - // Check if the map is empty - if (LEAP_SECOND_INSERTIONS_SINCE_2004.empty()) return 0; - auto it = LEAP_SECOND_INSERTIONS_SINCE_2004.upper_bound(unix_seconds); // Find the first element greater than givenUnixSecond - if (it == LEAP_SECOND_INSERTIONS_SINCE_2004.begin()) return 0; - --it; // Move iterator to the element with a key less than or equal to givenUnixSecond - return it->second; // Return the corresponding value +inline uint16_t getLeapSecondInsertionsSince2004(const uint64_t unix_seconds) { + // Check if the map is empty + if (LEAP_SECOND_INSERTIONS_SINCE_2004.empty()) return 0; + auto it = LEAP_SECOND_INSERTIONS_SINCE_2004.upper_bound( + unix_seconds); // Find the first element greater than givenUnixSecond + if (it == LEAP_SECOND_INSERTIONS_SINCE_2004.begin()) return 0; + --it; // Move iterator to the element with a key less than or equal to givenUnixSecond + return it->second; // Return the corresponding value } -} // namespace etsi_its_msgs \ No newline at end of file +} // namespace etsi_its_msgs \ No newline at end of file diff --git a/etsi_its_msgs_utils/test/impl/test_cam_access.cpp b/etsi_its_msgs_utils/test/impl/test_cam_access.cpp index ea5042140..0f766be13 100644 --- a/etsi_its_msgs_utils/test/impl/test_cam_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_cam_access.cpp @@ -1,14 +1,14 @@ -#include #include +#include namespace cam_access = etsi_its_cam_msgs::access; TEST(etsi_its_cam_msgs, test_set_get_cam) { - cam_msgs::CAM cam; - int station_id = randomInt(cam_msgs::StationID::MIN,cam_msgs::StationID::MAX); - int protocol_version = randomInt(cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MIN,cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MAX); + int station_id = randomInt(cam_msgs::StationID::MIN, cam_msgs::StationID::MAX); + int protocol_version = + randomInt(cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MIN, cam_msgs::ItsPduHeader::PROTOCOL_VERSION_MAX); cam_access::setItsPduHeader(cam, station_id, protocol_version); EXPECT_EQ(cam_msgs::ItsPduHeader::MESSAGE_ID_CAM, cam.header.message_id); EXPECT_EQ(protocol_version, cam.header.protocol_version); @@ -19,19 +19,24 @@ TEST(etsi_its_cam_msgs, test_set_get_cam) { // The value for TimestampIts for 2007-01-01T00:00:00.000Z is // 94694401000 milliseconds, which includes one leap second insertion // since 2004-01-01T00:00:00.000Z. - uint64_t t_2007 = ((uint64_t)1167609600)*1e9; + uint64_t t_2007 = ((uint64_t)1167609600) * 1e9; cam_msgs::TimestampIts t_its; - EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - cam_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); + cam_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); EXPECT_EQ(94694401000, t_its.value); - cam_access::setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000%65536, cam_access::getGenerationDeltaTimeValue(cam)); + cam_access::setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); + EXPECT_EQ(94694401000 % 65536, cam_access::getGenerationDeltaTimeValue(cam)); cam_msgs::TimestampIts t_its2; - uint64_t t_2007_off = t_2007 + 5*1e9; - cam_access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000, cam_access::getTimestampITSFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_its2).value); - EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_its2, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); - EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); + uint64_t t_2007_off = t_2007 + 5 * 1e9; + cam_access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); + EXPECT_EQ(94694401000, + cam_access::getTimestampITSFromGenerationDeltaTime(cam_access::getGenerationDeltaTime(cam), t_its2).value); + EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime( + cam_access::getGenerationDeltaTime(cam), t_its2, + etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9))); + EXPECT_EQ(t_2007, cam_access::getUnixNanosecondsFromGenerationDeltaTime( + cam_access::getGenerationDeltaTime(cam), t_2007_off, + etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9))); int stationType_val = randomInt(cam_msgs::StationType::MIN, cam_msgs::StationType::MAX); cam_access::setStationType(cam, stationType_val); @@ -94,7 +99,8 @@ TEST(etsi_its_cam_msgs, test_set_get_cam) { exterior_lights.at(i) = randomInt(0, 1); } cam.cam.cam_parameters.low_frequency_container_is_present = true; - cam.cam.cam_parameters.low_frequency_container.choice = cam_msgs::LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; + cam.cam.cam_parameters.low_frequency_container.choice = + cam_msgs::LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; cam_access::setExteriorLights(cam, exterior_lights); EXPECT_EQ(exterior_lights, cam_access::getExteriorLights(cam)); } \ No newline at end of file diff --git a/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp b/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp index 1be165756..5cdd703ae 100644 --- a/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_cam_ts_access.cpp @@ -1,14 +1,13 @@ -#include #include +#include namespace cam_ts_access = etsi_its_cam_ts_msgs::access; TEST(etsi_its_cam_ts_msgs, test_set_get_cam) { - cam_ts_msgs::CAM cam; - int station_id = randomInt(cam_ts_msgs::StationId::MIN,cam_ts_msgs::StationId::MAX); - int protocol_version = randomInt(cam_ts_msgs::OrdinalNumber1B::MIN,cam_ts_msgs::OrdinalNumber1B::MAX); + int station_id = randomInt(cam_ts_msgs::StationId::MIN, cam_ts_msgs::StationId::MAX); + int protocol_version = randomInt(cam_ts_msgs::OrdinalNumber1B::MIN, cam_ts_msgs::OrdinalNumber1B::MAX); cam_ts_access::setItsPduHeader(cam, station_id, protocol_version); EXPECT_EQ(cam_ts_msgs::MessageId::CAM, cam.header.message_id.value); EXPECT_EQ(protocol_version, cam.header.protocol_version.value); @@ -19,19 +18,25 @@ TEST(etsi_its_cam_ts_msgs, test_set_get_cam) { // The value for TimestampIts for 2007-01-01T00:00:00.000Z is // 94694401000 milliseconds, which includes one leap second insertion // since 2004-01-01T00:00:00.000Z. - uint64_t t_2007 = ((uint64_t)1167609600)*1e9; + uint64_t t_2007 = ((uint64_t)1167609600) * 1e9; cam_ts_msgs::TimestampIts t_its; - EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - cam_ts_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); + cam_ts_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); EXPECT_EQ(94694401000, t_its.value); - cam_ts_access::setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000%65536, cam_ts_access::getGenerationDeltaTimeValue(cam)); + cam_ts_access::setGenerationDeltaTime(cam, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); + EXPECT_EQ(94694401000 % 65536, cam_ts_access::getGenerationDeltaTimeValue(cam)); cam_ts_msgs::TimestampIts t_its2; - uint64_t t_2007_off = t_2007 + 5*1e9; - cam_ts_access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - EXPECT_EQ(94694401000, cam_ts_access::getTimestampITSFromGenerationDeltaTime(cam_ts_access::getGenerationDeltaTime(cam), t_its2).value); - EXPECT_EQ(t_2007, cam_ts_access::getUnixNanosecondsFromGenerationDeltaTime(cam_ts_access::getGenerationDeltaTime(cam), t_its2, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); - EXPECT_EQ(t_2007, cam_ts_access::getUnixNanosecondsFromGenerationDeltaTime(cam_ts_access::getGenerationDeltaTime(cam), t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9))); + uint64_t t_2007_off = t_2007 + 5 * 1e9; + cam_ts_access::setTimestampITS(t_its2, t_2007_off, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); + EXPECT_EQ( + 94694401000, + cam_ts_access::getTimestampITSFromGenerationDeltaTime(cam_ts_access::getGenerationDeltaTime(cam), t_its2).value); + EXPECT_EQ(t_2007, cam_ts_access::getUnixNanosecondsFromGenerationDeltaTime( + cam_ts_access::getGenerationDeltaTime(cam), t_its2, + etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9))); + EXPECT_EQ(t_2007, cam_ts_access::getUnixNanosecondsFromGenerationDeltaTime( + cam_ts_access::getGenerationDeltaTime(cam), t_2007_off, + etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9))); int stationType_val = randomInt(cam_ts_msgs::TrafficParticipantType::MIN, cam_ts_msgs::TrafficParticipantType::MAX); cam_ts_access::setStationType(cam, stationType_val); @@ -94,7 +99,8 @@ TEST(etsi_its_cam_ts_msgs, test_set_get_cam) { exterior_lights.at(i) = randomInt(0, 1); } cam.cam.cam_parameters.low_frequency_container_is_present = true; - cam.cam.cam_parameters.low_frequency_container.choice = cam_ts_msgs::LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; + cam.cam.cam_parameters.low_frequency_container.choice = + cam_ts_msgs::LowFrequencyContainer::CHOICE_BASIC_VEHICLE_CONTAINER_LOW_FREQUENCY; cam_ts_access::setExteriorLights(cam, exterior_lights); EXPECT_EQ(exterior_lights, cam_ts_access::getExteriorLights(cam)); } \ No newline at end of file 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 d6b42417f..f9476641f 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 @@ -1,14 +1,13 @@ -#include #include +#include namespace cpm_ts_access = etsi_its_cpm_ts_msgs::access; TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { - cpm_ts_msgs::CollectivePerceptionMessage cpm; - int station_id = randomInt(cpm_ts_msgs::StationId::MIN,cpm_ts_msgs::StationId::MAX); - int protocol_version = randomInt(cpm_ts_msgs::OrdinalNumber1B::MIN,cpm_ts_msgs::OrdinalNumber1B::MAX); + int station_id = randomInt(cpm_ts_msgs::StationId::MIN, cpm_ts_msgs::StationId::MAX); + int protocol_version = randomInt(cpm_ts_msgs::OrdinalNumber1B::MIN, cpm_ts_msgs::OrdinalNumber1B::MAX); cpm_ts_access::setItsPduHeader(cpm, station_id, protocol_version); EXPECT_EQ(cpm_ts_msgs::MessageId::CPM, cpm.header.message_id.value); EXPECT_EQ(protocol_version, cpm.header.protocol_version.value); @@ -19,13 +18,13 @@ TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { // The value for TimestampIts for 2007-01-01T00:00:00.000Z is // 94694401000 milliseconds, which includes one leap second insertion // since 2004-01-01T00:00:00.000Z. - - uint64_t t_2007 = ((uint64_t)1167609600)*1e9; + + uint64_t t_2007 = ((uint64_t)1167609600) * 1e9; cpm_ts_msgs::TimestampIts t_its; - EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); - cpm_ts_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + EXPECT_EQ(1, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); + cpm_ts_access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); EXPECT_EQ(94694401000, t_its.value); - cpm_ts_access::setReferenceTime(cpm, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + cpm_ts_access::setReferenceTime(cpm, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007 * 1e-9)); EXPECT_EQ(94694401000, cpm_ts_access::getReferenceTimeValue(cpm)); EXPECT_EQ(t_2007, cpm_ts_access::getUnixNanosecondsFromReferenceTime(cpm_ts_access::getReferenceTime(cpm))); @@ -87,5 +86,4 @@ TEST(etsi_its_cpm_ts_msgs, test_set_get_cpm) { 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_msgs_utils/test/impl/test_denm_access.cpp b/etsi_its_msgs_utils/test/impl/test_denm_access.cpp index 35353a014..628dd620a 100644 --- a/etsi_its_msgs_utils/test/impl/test_denm_access.cpp +++ b/etsi_its_msgs_utils/test/impl/test_denm_access.cpp @@ -1,14 +1,14 @@ -#include #include +#include namespace denm_access = etsi_its_denm_msgs::access; TEST(etsi_its_denm_msgs, test_set_get_denm) { - denm_msgs::DENM denm; - int station_id = randomInt(denm_msgs::StationID::MIN,denm_msgs::StationID::MAX); - int protocol_version = randomInt(denm_msgs::ItsPduHeader::PROTOCOL_VERSION_MIN,denm_msgs::ItsPduHeader::PROTOCOL_VERSION_MAX); + int station_id = randomInt(denm_msgs::StationID::MIN, denm_msgs::StationID::MAX); + int protocol_version = + randomInt(denm_msgs::ItsPduHeader::PROTOCOL_VERSION_MIN, denm_msgs::ItsPduHeader::PROTOCOL_VERSION_MAX); denm_access::setItsPduHeader(denm, station_id, protocol_version); EXPECT_EQ(denm_msgs::ItsPduHeader::MESSAGE_ID_DENM, denm.header.message_id); EXPECT_EQ(protocol_version, denm.header.protocol_version); @@ -19,14 +19,14 @@ TEST(etsi_its_denm_msgs, test_set_get_denm) { // The value for TimestampIts for 2007-01-01T00:00:00.000Z is // 94694401000 milliseconds, which includes one leap second insertion // since 2004-01-01T00:00:00.000Z. - uint64_t t_2007 = ((uint64_t)1167609600)*1e9; + uint64_t t_2007 = ((uint64_t)1167609600) * 1e9; denm_msgs::TimestampIts t_its; denm_access::setTimestampITS(t_its, t_2007, 1); EXPECT_EQ(94694401000, t_its.value); - + denm_access::setReferenceTime(denm, t_2007, 1); EXPECT_EQ(94694401000, denm_access::getReferenceTimeValue(denm)); - uint64_t t_2007_off = t_2007 + 5*1e9; + uint64_t t_2007_off = t_2007 + 5 * 1e9; EXPECT_EQ(t_2007, denm_access::getUnixNanosecondsFromReferenceTime(denm_access::getReferenceTime(denm))); int stationType_val = randomInt(denm_msgs::StationType::MIN, denm_msgs::StationType::MAX); diff --git a/etsi_its_msgs_utils/test/test_access.cpp b/etsi_its_msgs_utils/test/test_access.cpp index 0f1e93987..730e16f5c 100644 --- a/etsi_its_msgs_utils/test/test_access.cpp +++ b/etsi_its_msgs_utils/test/test_access.cpp @@ -1,7 +1,6 @@ - +#include #include #include -#include #include #include @@ -39,7 +38,6 @@ namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs; namespace denm_msgs = etsi_its_denm_msgs; #include - int main(int argc, char *argv[]) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/etsi_its_msgs_utils/test/test_access.ros2.cpp b/etsi_its_msgs_utils/test/test_access.ros2.cpp index 880c27aec..adbb90461 100644 --- a/etsi_its_msgs_utils/test/test_access.ros2.cpp +++ b/etsi_its_msgs_utils/test/test_access.ros2.cpp @@ -1,7 +1,6 @@ - +#include #include #include -#include #include #include @@ -39,7 +38,6 @@ namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs::msg; namespace denm_msgs = etsi_its_denm_msgs::msg; #include - int main(int argc, char *argv[]) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From 5feb424d2cb3ecb7ca859c635b061c63ed9959c6 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 10 Oct 2024 12:40:18 +0000 Subject: [PATCH 44/52] format cpm access functions and improve comments --- .../etsi_its_msgs_utils/cpm_ts_access.h | 2 +- .../etsi_its_msgs_utils/cpm_ts_access.hpp | 6 +- .../etsi_its_msgs_utils/cpm_ts_acess.h | 43 -- .../impl/cpm/cpm_ts_access.h | 2 +- .../impl/cpm/cpm_ts_getters.h | 350 ++++++++------- .../impl/cpm/cpm_ts_setters.h | 421 ++++++++++-------- .../impl/cpm/cpm_ts_utils.h | 9 +- 7 files changed, 427 insertions(+), 406 deletions(-) delete mode 100644 etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h index b009be08b..b713921e5 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h @@ -41,7 +41,7 @@ SOFTWARE. #include namespace etsi_its_cpm_ts_msgs { - namespace gm = geometry_msgs; +namespace gm = geometry_msgs; } // Implementation diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp index 639fecfd8..5c4ed6533 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp @@ -41,9 +41,9 @@ SOFTWARE. #include namespace etsi_its_cpm_ts_msgs { - using namespace msg; - namespace gm = geometry_msgs::msg; -} +using namespace msg; +namespace gm = geometry_msgs::msg; +} // namespace etsi_its_cpm_ts_msgs // Implementation #include \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h deleted file mode 100644 index 00a9b6b80..000000000 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_acess.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -============================================================================= -MIT License - -Copyright (c) 2023-2024 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. -============================================================================= -*/ - -/** - * @file cpm_ts_access.h - * @brief Main CPM access header to include in ROS 1 projects - */ - -#pragma once - -// Messages -#include -#include - -namespace etsi_its_cpm_ts_msgs { - namespace gm = geometry_msgs; -} - -// Implementation -#include \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h index f55fac2a1..4bf45c8f9 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h @@ -50,4 +50,4 @@ SOFTWARE. namespace etsi_its_cpm_ts_msgs::access { #include -} // namespace etsi_its_cpm_ts_msgs::access \ No newline at end of file +} // namespace etsi_its_cpm_ts_msgs::access \ No newline at end of file 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 4592fbf20..fd987c995 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 @@ -67,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 10^-7 degree + * @return Latitude value in degree as decimal number */ inline double getLatitude(const CollectivePerceptionMessage &cpm) { return getLatitude(cpm.payload.management_container.reference_position.latitude); @@ -77,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 10^-7 degree + * @return Longitude value in degree as decimal number */ inline double getLongitude(const CollectivePerceptionMessage &cpm) { return getLongitude(cpm.payload.management_container.reference_position.longitude); @@ -87,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 in centimeters + * @return Altitude value in (above the reference ellipsoid surface) in meter as decimal number */ inline double getAltitude(const CollectivePerceptionMessage &cpm) { return getAltitude(cpm.payload.management_container.reference_position.altitude); @@ -115,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 position, int UTM zone and bool hemisphere + * @return geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id */ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) { int zone; @@ -124,11 +124,14 @@ inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) { } /** - * @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 - */ + * @brief Retrieves the container IDs from a CPM. + * + * This function iterates over the cpm_containers array in the given CPM + * and extracts the container IDs into a vector of uint8_t. + * + * @param cpm The CPM from which to retrieve the container IDs. + * @return A vector containing the container IDs. + */ inline std::vector getCpmContainerIds(const CollectivePerceptionMessage &cpm) { std::vector container_ids; for (int i = 0; i < cpm.payload.cpm_containers.value.array.size(); i++) { @@ -138,12 +141,13 @@ inline std::vector getCpmContainerIds(const CollectivePerceptionMessage } /** - * @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 - */ + * Retrieves the CpmContainer with the specified container ID from the CPM. + * + * @param cpm The CPM from which to retrieve the CpmContainer. + * @param container_id The ID of the CpmContainer to retrieve. + * @return The CpmContainer with the specified container ID. + * @throws std::invalid_argument if no CpmContainer with the specified ID is found in the CPM. + */ 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) { @@ -154,21 +158,24 @@ inline WrappedCpmContainer getCpmContainer(const CollectivePerceptionMessage &cp } /** - * @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 - */ + * @brief Retrieves the perceived object container from a CPM. + * + * This function returns the perceived object container from the given CPM. + * + * @param cpm The CPM from which to retrieve the perceived object container. + * @return The perceived object container. + */ 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 - */ + * Retrieves the number of perceived objects from the given perceived object container. + * + * @param container The perceived object container to retrieve the number of perceived objects from. + * @return The number of perceived objects. + * @throws std::invalid_argument if the container is not a PerceivedObjectContainer. + */ 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"); @@ -178,24 +185,27 @@ inline uint8_t getNumberOfPerceivedObjects(const WrappedCpmContainer &container) } /** - * @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 - */ + * Retrieves the number of perceived objects from the given CPM. + * + * @param cpm The CPM from which to retrieve the number of perceived objects. + * @return The number of perceived objects. + */ inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) { return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm)); } -// getters for the PerceivedObject +////////////////////////////////////////////////////////////////////////// +/////////////////// 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 - */ + * @brief Retrieves the PerceivedObject at the specified index from the given WrappedCpmContainer. + * + * @param container The WrappedCpmContainer from which to retrieve the PerceivedObject (should be a PerceivedObjectContainer). + * @param i The index of the PerceivedObject to retrieve. + * @return The PerceivedObject at the specified index. + * @throws std::invalid_argument if the index is out of range. + */ inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, const uint8_t i) { if (i >= getNumberOfPerceivedObjects(container)) { throw std::invalid_argument("Index out of range"); @@ -204,94 +214,103 @@ inline PerceivedObject getPerceivedObject(const WrappedCpmContainer &container, } /** - * @brief Get the ID of PerceivedObject - * - * @param object PerceivedObject to get the ID from - * @return PerceivedObject ID - */ + * @brief Retrieves the ID of a perceived object. + * + * This function takes a PerceivedObject as input and returns the ID of the object. + * + * @param object The PerceivedObject for which to retrieve the ID. + * @return The ID of the perceived object. + */ 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 - */ + * @brief Retrieves the Cartesian coordinate value from a CartesianCoordinateWithConfidence object. + * + * @param coordinate The CartesianCoordinateWithConfidence object from which to retrieve the value. + * @return The Cartesian coordinate value in centimeters. + */ inline int32_t getCartesianCoordinate(const CartesianCoordinateWithConfidence &coordinate) { 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 - */ + * @brief Retrieves the confidence value from a CartesianCoordinateWithConfidence object. + * + * @param coordinate The CartesianCoordinateWithConfidence object from which to retrieve the confidence value. + * @return The confidence value of the CartesianCoordinateWithConfidence object in centimeters. + */ inline uint16_t getCartesianCoordinateConfidence(const CartesianCoordinateWithConfidence &coordinate) { return coordinate.confidence.value; } /** - * @brief Get the position of the PerceivedObject - * - * @param object PerceivedObject to get the position from - * @return gm::Point position of the PerceivedObject in m - */ + * Returns the position of a perceived object. + * + * @param object The perceived object. + * @return The position of the perceived object as a gm::Point (all values in meters). + */ inline gm::Point getPositionOfPerceivedObject(const PerceivedObject &object) { gm::Point point; - point.x = double(getCartesianCoordinate(object.position.x_coordinate)) / 100; - point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100; + 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; + point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0; } return point; } /** - * @brief Get the CartesianAngle - * - * @param angle CartesianAngle to get the value from - * @return value of the CartesianAnglein 0,1 degrees - */ + * @brief Get the Cartesian angle of the PerceivedObject + * + * @param object PerceivedObject to get the Cartesian angle from + * @return unit16_t Cartesian angle of the PerceivedObject in 0,1 degrees + */ inline uint16_t getCartesianAngle(const CartesianAngle &angle) { return angle.value.value; } /** - * @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 - */ + * @brief Get the confidence of the Cartesian angle + * + * @param angle CartesianAngle to get the confidence from + * @return uint8_t confidence of the Cartesian angle 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 - */ + * @brief Calculates the orientation of a perceived object. + * + * This function calculates the orientation of a perceived object based on the angles provided in the `PerceivedObject` structure. + * The angles are converted to radians and used to set the roll, pitch, and yaw values of a `tf2::Quaternion` object. + * The resulting quaternion is then converted to a `gm::Quaternion` message and returned. + * + * @param object The `PerceivedObject` structure containing the angles of the perceived object. + * @return gm::Quaternion The orientation of the perceived object in radians from -pi to pi. + */ 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) - 180) / 180) * M_PI; + roll = (((double(getCartesianAngle(object.angles.x_angle)) / 10.0) - 180.0) / 180.0) * + M_PI; // TODO: check if 0-360 -> -180-180 is correct } if (object.angles.y_angle_is_present) { - pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10) - 180) / 180) * M_PI; + pitch = (((double(getCartesianAngle(object.angles.y_angle)) / 10.0) - 180.0) / 180.0) * + M_PI; // TODO: check if 0-360 -> -180-180 is correct } - yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10) - 180) / 180) * M_PI; + yaw = (((double(getCartesianAngle(object.angles.z_angle)) / 10.0) - 180.0) / 180.0) * + M_PI; // TODO: check if 0-360 -> -180-180 is correct 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 - */ + * @brief Get the yaw of the PerceivedObject + * + * @param object PerceivedObject to get the yaw from + * @return double yaw of the PerceivedObject in radians from -pi to pi + */ inline double getYawOfPerceivedObject(const PerceivedObject &object) { gm::Quaternion orientation = getOrientationOfPerceivedObject(object); tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); @@ -301,11 +320,11 @@ inline double getYawOfPerceivedObject(const PerceivedObject &object) { } /** - * @brief Get the pose of the PerceivedObject - * - * @param object PerceivedObject to get the pose from - * @return gm::Pose pose of the PerceivedObject - */ + * @brief Get the pose of the PerceivedObject + * + * @param object PerceivedObject to get the pose from + * @return gm::Pose pose of the PerceivedObject (position in m, orientation in rad) + */ inline gm::Pose getPoseOfPerceivedObject(const PerceivedObject &object) { gm::Pose pose; pose.position = getPositionOfPerceivedObject(object); @@ -314,40 +333,42 @@ inline gm::Pose getPoseOfPerceivedObject(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 - */ + * @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 + * @throws std::invalid_argument If the yaw rate is not present in the object. + */ 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; } /** - * @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; } + * @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.0; } /** - * @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 - */ + * @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; + return double(velocity.confidence.value) / 100.0; } /** - * @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 - */ + * @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 (local coordinate system) + * @throws std::invalid_argument If the velocity is no cartesian velocity. + */ 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) { @@ -363,31 +384,32 @@ inline gm::Vector3 getCartesianVelocityOfPerceivedObject(const PerceivedObject & } /** - * @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 - */ + * @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; + return double(acceleration.value.value) / 10.0; } /** - * @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 - */ + * @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; + return double(acceleration.confidence.value) / 10.0; } /** - * @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 - */ + * @brief Get the Cartesian acceleration of the PerceivedObject + * + * @param object PerceivedObject to get the Cartesian acceleration from + * @return gm::Vector3 Cartesian acceleration of the PerceivedObject in m/s^2 (local coordinate system) + * @throws std::invalid_argument If the acceleration is no cartesian acceleration. + */ inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObject &object) { if (!object.acceleration_is_present) throw std::invalid_argument("No acceleration present in PerceivedObject"); if (object.acceleration.choice != Acceleration3dWithConfidence::CHOICE_CARTESIAN_ACCELERATION) { @@ -403,27 +425,31 @@ inline gm::Vector3 getCartesianAccelerationOfPerceivedObject(const PerceivedObje } /** - * @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. - */ + * @brief Gets the x-dimension of a perceived object. + * + * This function extracts the x-dimension from a given PerceivedObject. + * If the x-dimension is not present in the object, it throws an + * std::invalid_argument exception. + * + * @param object The PerceivedObject from which to retrieve the x-dimension. + * @return The x-dimension of the perceived object in decimeters. + * @throws std::invalid_argument if the x-dimension is not present in the PerceivedObject. + */ inline uint16_t getXDimensionOfPerceivedObject(const PerceivedObject &object) { if (!object.object_dimension_x_is_present) throw std::invalid_argument("No x-dimension present in PerceivedObject"); return object.object_dimension_x.value.value; } /** - * @brief Retrieves the Y dimension of a perceived object. + * @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 + * 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. + * @param object The PerceivedObject from which to retrieve the y-dimension. + * @return uint16_t y-dimension of the perceived object in in decimeters. + * @throws std::invalid_argument if the y-dimension is not present in the PerceivedObject. */ inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) { if (!object.object_dimension_y_is_present) throw std::invalid_argument("No y-dimension present in PerceivedObject"); @@ -431,15 +457,15 @@ inline uint16_t getYDimensionOfPerceivedObject(const PerceivedObject &object) { } /** - * @brief Retrieves the Z dimension of a perceived object. + * @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 + * 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. + * @param object The PerceivedObject from which to retrieve the z-dimension. + * @return uint16_t z-dimension of the perceived object in decimeters. + * @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"); @@ -447,26 +473,32 @@ inline uint16_t getZDimensionOfPerceivedObject(const PerceivedObject &object) { } /** - * @brief Get the dimensions of the PerceivedObject - * - * @param object PerceivedObject to get the dimensions from - * @return gm::Vector3 dimensions of the PerceivedObject in m - */ + * @brief 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. + * + * @param object The `PerceivedObject` for which to calculate the dimensions. + * @return The dimensions of the perceived object as a `gm::Vector3` object in meters. + */ inline gm::Vector3 getDimensionsOfPerceivedObject(const PerceivedObject &object) { gm::Vector3 dimensions; - dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10; - dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10; - dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10; + dimensions.x = double(getXDimensionOfPerceivedObject(object)) / 10.0; + dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0; + dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0; return dimensions; } /** - * @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 - */ + * @brief Calculates the UTM position of a perceived object based on the CPMs referencep position. + * + * This function takes a CPM and a PerceivedObject as input parameters and calculates the UTM position of the object. + * The UTM position is calculated by adding the relative position of the object to the UTM position of the reference point in the CPM. + * + * @param cpm The Collective Perception Message (CPM) containing the reference position. + * @param object The PerceivedObject for which the UTM position needs to be calculated. + * @return The UTM position of the perceived object. + */ inline gm::PointStamped getUTMPositionOfPerceivedObject(const CollectivePerceptionMessage &cpm, const PerceivedObject &object) { gm::PointStamped utm_position; 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 64c7fa7e0..21743c8c6 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 @@ -38,24 +38,30 @@ 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 - */ + * @brief Sets the ITS PDU header of a CPM. + * + * This function sets the ITS PDU header of a CPM with the provided station ID and protocol version. + * + * @param cpm The CPM to set the ITS PDU header for. + * @param station_id The station ID to set in the ITS PDU header. + * @param protocol_version The protocol version to set in the ITS PDU header. Default is 0. + */ 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) - */ + * @brief Sets the reference time in a CPM. + * + * This function sets the reference time in a CPM object. The reference time is represented + * by a Unix timestamp in nanoseconds including the number of leap seconds. + * The reference time is stored in the payload management container of the CPM. + * + * @param cpm The CPM object to set the reference time in. + * @param unix_nanosecs The Unix timestamp in nanoseconds representing the reference time. + * @param n_leap_seconds The number of leap seconds to be considered. Defaults to the todays number of leap seconds since 2004. + */ 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) { @@ -66,59 +72,62 @@ inline void setReferenceTime( } /** - * @brief Set the ReferencePositionWithConfidence for a CPM TS - * - * This function sets the latitude, longitude, and altitude of the CPMs reference position. - * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. - * - * @param cpm CPM to set the ReferencePosition - * @param latitude The latitude value position in degree as decimal number. - * @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). - */ + * @brief Set the ReferencePositionWithConfidence for a CPM TS + * + * This function sets the latitude, longitude, and altitude of the CPMs reference position. + * If the altitude is not provided, it is set to AltitudeValue::UNAVAILABLE. + * + * @param cpm CPM to set the ReferencePosition + * @param latitude The latitude value position in degree as decimal number. + * @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); } /** - * @brief Set the ReferencePosition of a CPM from a given UTM-Position - * - * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS - * The z-Coordinate is directly used as altitude value - * The frame_id of the given utm_position must be set to 'utm_' - * - * @param[out] cpm CPM for which to set the ReferencePosition - * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position - * @param[in] zone the UTM zone (zero means UPS) of the given position - * @param[in] northp hemisphere (true means north, false means south) - */ + * @brief Set the ReferencePosition of a CPM from a given UTM-Position + * + * The position is transformed to latitude and longitude by using GeographicLib::UTMUPS + * The z-Coordinate is directly used as altitude value + * The frame_id of the given utm_position must be set to 'utm_' + * + * @param[out] cpm CPM for which to set the ReferencePosition + * @param[in] utm_position geometry_msgs::PointStamped describing the given utm position in meters + * @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); } /** - * @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 - */ + * @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; } /** - * @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 - */ + * @brief Sets the measurement delta time of a PerceivedObject. + * + * This function sets the measurement delta time of a PerceivedObject. + * The measurement delta time represents the time difference + * between the reference time of the CPM and the measurement of the object. + * + * @param object The PerceivedObject to set the measurement delta time for. + * @param delta_time The measurement delta time to set in milliseconds. Default value is 0. + * @throws std::invalid_argument if the delta_time is out of range. + */ 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"); @@ -128,14 +137,18 @@ inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, co } /** - * @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) - */ + * @brief Sets the value and confidence of a CartesianCoordinateWithConfidence object. + * + * This function sets the value and confidence of a CartesianCoordinateWithConfidence object. + * The value is limited to the range defined by CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE + * and CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE. The confidence is limited to the range + * defined by CoordinateConfidence::MIN and CoordinateConfidence::MAX. If the provided confidence + * is out of range, the confidence value is set to CoordinateConfidence::OUT_OF_RANGE. + * + * @param coordinate The CartesianCoordinateWithConfidence object to be modified. + * @param value The value to be set in centimeters. + * @param confidence The confidence to be set in centimeters (default: CoordinateConfidence::UNAVAILABLE). + */ inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const int16_t value, const uint16_t confidence = CoordinateConfidence::UNAVAILABLE) { // limit value range @@ -152,17 +165,16 @@ inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfiden } /** - * @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) - */ + * @brief Sets the position of a perceived object (relative to the CPM's reference position). + * + * This function sets the position of a perceived object using the provided coordinates and confidence values. + * + * @param object The PerceivedObject to set the position for. + * @param point The gm::Point representing the coordinates of the object in meters. + * @param x_confidence The confidence value in meters for the x-coordinate (default: CoordinateConfidence::UNAVAILABLE). + * @param y_confidence The confidence value in meters for the y-coordinate (default: CoordinateConfidence::UNAVAILABLE). + * @param z_confidence The confidence value in meters for the z-coordinate (default: CoordinateConfidence::UNAVAILABLE). + */ inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Point& point, const uint16_t x_confidence = CoordinateConfidence::UNAVAILABLE, const uint16_t y_confidence = CoordinateConfidence::UNAVAILABLE, @@ -176,18 +188,20 @@ inline void setPositionOfPerceivedObject(PerceivedObject& object, const gm::Poin } /** - * @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) - */ + * @brief Sets the position of a perceived object based on a UTM position. + * + * This function sets the position of a perceived object using the provided UTM position and the CPM's reference position. + * It also allows specifying confidence values for the x, y, and z coordinates. + * + * @param cpm The CPM to get the reference position from. + * @param object The PerceivedObject to set the position for. + * @param utm_position gm::PointStamped representing the UTM position of the object including the frame_id (utm_). + * @param x_confidence The confidence value in meters for the x coordinate (default: CoordinateConfidence::UNAVAILABLE). + * @param y_confidence The confidence value in meters for the y coordinate (default: CoordinateConfidence::UNAVAILABLE). + * @param z_confidence The confidence value in meters for the z coordinate (default: CoordinateConfidence::UNAVAILABLE). + * + * @throws std::invalid_argument if the UTM-Position frame_id does not match the reference position frame_id. + */ inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, PerceivedObject& object, const gm::PointStamped& utm_position, const uint16_t x_confidence = CoordinateConfidence::UNAVAILABLE, @@ -209,15 +223,16 @@ inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, Pe } /** - * @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) - */ + * @brief Sets the value and confidence of a VelocityComponent. + * + * This function sets the value and confidence of a VelocityComponent object. The value is limited to a specific range, + * and the confidence is limited to a specific range as well. If the provided value or confidence is out of range, + * it will be set to the corresponding out-of-range value. + * + * @param velocity The VelocityComponent object to set the value and confidence for. + * @param value The value to set for the VelocityComponent in cm/s. + * @param confidence The confidence to set for the VelocityComponent in cm/s. Default value is SpeedConfidence::UNAVAILABLE. + */ inline void setVelocityComponent(VelocityComponent& velocity, const int16_t value, const uint8_t confidence = SpeedConfidence::UNAVAILABLE) { // limit value range @@ -234,17 +249,17 @@ inline void setVelocityComponent(VelocityComponent& velocity, const int16_t valu } /** - * @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) - */ + * Sets the velocity of a perceived object. + * + * This function sets the velocity of a perceived object using the provided Cartesian velocity components. + * Optionally, confidence values can be specified for each velocity component. + * + * @param object The perceived object for which the velocity is being set. + * @param cartesian_velocity The Cartesian velocity components (x, y, z) of the object (in m/s). + * @param x_confidence The confidence value in m/s for the x velocity component (default: SpeedConfidence::UNAVAILABLE). + * @param y_confidence The confidence value in m/s for the y velocity component (default: SpeedConfidence::UNAVAILABLE). + * @param z_confidence The confidence value in m/s for the z velocity component (default: SpeedConfidence::UNAVAILABLE). + */ inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_velocity, const uint8_t x_confidence = SpeedConfidence::UNAVAILABLE, const uint8_t y_confidence = SpeedConfidence::UNAVAILABLE, @@ -260,14 +275,16 @@ inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vect } /** - * @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) - */ + * @brief Sets the value and confidence of a AccelerationComponent. + * + * This function sets the value and confidence of a AccelerationComponent object. The value is limited to a specific range, + * and the confidence is limited to a specific range as well. If the provided value or confidence is out of range, + * it will be set to the corresponding out-of-range value. + * + * @param acceleration The AccelerationComponent object to set the value and confidence for. + * @param value The value to set for the AccelerationComponent in dm/s^2. + * @param confidence The confidence to set for the AccelerationComponent in dm/s^2. Default value is AccelerationConfidence::UNAVAILABLE. + */ inline void setAccelerationComponent(AccelerationComponent& acceleration, const int16_t value, const uint8_t confidence = AccelerationConfidence::UNAVAILABLE) { // limit value range @@ -284,17 +301,17 @@ inline void setAccelerationComponent(AccelerationComponent& acceleration, const } /** - * @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) - */ + * @brief Sets the acceleration of a perceived object. + * + * This function sets the acceleration of a perceived object using the provided Cartesian acceleration components. + * Optionally, confidence values can be specified for each acceleration component. + * + * @param object The perceived object for which the acceleration is being set. + * @param cartesian_acceleration The Cartesian acceleration components (x, y, z) of the object (in m/s^2). + * @param x_confidence The confidence value in m/s^2 for the x acceleration component (default: AccelerationConfidence::UNAVAILABLE). + * @param y_confidence The confidence value in m/s^2 for the y acceleration component (default: AccelerationConfidence::UNAVAILABLE). + * @param z_confidence The confidence value in m/s^2 for the z acceleration component (default: AccelerationConfidence::UNAVAILABLE). + */ inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm::Vector3& cartesian_acceleration, const uint8_t x_confidence = AccelerationConfidence::UNAVAILABLE, const uint8_t y_confidence = AccelerationConfidence::UNAVAILABLE, @@ -313,18 +330,19 @@ inline void setAccelerationOfPerceivedObject(PerceivedObject& object, const gm:: } /** - * @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) - */ + * @brief Sets the yaw angle of a perceived object. + * + * This function sets the yaw angle of a PerceivedObject. The yaw angle is wrapped to the range [0, 360] degrees. + * The function also allows specifying the confidence level of the yaw angle. + * + * @param object The PerceivedObject to set the yaw angle for. + * @param yaw The yaw angle in radians. + * @param confidence The confidence level of the yaw angle in 0,1 degrees (optional, default is AngleConfidence::UNAVAILABLE). + */ 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; + double yaw_in_degrees = yaw * 180 / M_PI + 180; // TODO: check if this is correct 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; @@ -338,14 +356,16 @@ inline void setYawOfPerceivedObject(PerceivedObject& object, const double yaw, } /** - * @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) - */ + * @brief Sets the yaw rate of a perceived object. + * + * This function sets the yaw rate of a PerceivedObject. The yaw rate is limited to the range defined by + * CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE and CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE. + * The function also allows specifying the confidence level of the yaw rate. + * + * @param object The PerceivedObject to set the yaw rate for. + * @param yaw_rate The yaw rate in rad/s. + * @param confidence Confidence of the yaw rate defined in AngularSpeedConfidence (optional, default is AngularSpeedConfidence::UNAVAILABLE). + */ 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; @@ -360,14 +380,19 @@ inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double ya } /** - * @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) - */ + * @brief Sets the object dimension with the given value and confidence. + * + * This function sets the value and confidence of the object dimension based on the provided parameters. + * The value is limited to the range defined by ObjectDimensionValue::MIN and ObjectDimensionValue::MAX. + * If the provided value is outside this range, the dimension value is set to ObjectDimensionValue::OUT_OF_RANGE. + * + * The confidence is limited to the range defined by ObjectDimensionConfidence::MIN and ObjectDimensionConfidence::MAX. + * If the provided confidence is outside this range, the confidence value is set to ObjectDimensionConfidence::OUT_OF_RANGE. + * + * @param dimension The object dimension to be set. + * @param value The value of the object dimension in decimeters. + * @param confidence The confidence of the object dimension in decimeters (optional, default is ObjectDimensionConfidence::UNAVAILABLE). + */ inline void setObjectDimension(ObjectDimension& dimension, const uint16_t value, const uint8_t confidence = ObjectDimensionConfidence::UNAVAILABLE) { // limit value range @@ -386,14 +411,15 @@ inline void setObjectDimension(ObjectDimension& dimension, const uint16_t value, } /** - * @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) - */ + * @brief Sets the x-dimension of a perceived object. + * + * This function sets the x-dimension of the given `PerceivedObject` to the specified value. + * The x-dimension usually represents the length of the object. + * + * @param object The `PerceivedObject` to modify. + * @param value The value to set as the x-dimension in meters. + * @param confidence The confidence of the x-dimension value in meters (optional, default is `ObjectDimensionConfidence::UNAVAILABLE`). + */ 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); @@ -401,14 +427,15 @@ inline void setXDimensionOfPerceivedObject(PerceivedObject& object, const double } /** - * @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) - */ + * @brief Sets the y-dimension of a perceived object. + * + * This function sets the y-dimension of the given `PerceivedObject` to the specified value. + * The y-dimension usually represents the width of the object. + * + * @param object The `PerceivedObject` to modify. + * @param value The value to set as the y-dimension in meters. + * @param confidence The confidence of the y-dimension value in meters (optional, default is `ObjectDimensionConfidence::UNAVAILABLE`). + */ 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); @@ -416,14 +443,15 @@ inline void setYDimensionOfPerceivedObject(PerceivedObject& object, const double } /** - * @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) - */ + * @brief Sets the z-dimension of a perceived object. + * + * This function sets the z-dimension of the given `PerceivedObject` to the specified value. + * The z-dimension usually represents the height of the object. + * + * @param object The `PerceivedObject` to modify. + * @param value The value to set as the z-dimension in meters. + * @param confidence The confidence of the z-dimension value in meters (optional, default is `ObjectDimensionConfidence::UNAVAILABLE`). + */ 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); @@ -431,16 +459,16 @@ inline void setZDimensionOfPerceivedObject(PerceivedObject& object, const double } /** - * @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) - */ + * @brief Sets all dimensions of a perceived object. + * + * This function sets the dimensions of a perceived object using the provided dimensions and confidence values. + * + * @param object The perceived object to set the dimensions for. + * @param dimensions The dimensions of the object as a gm::Vector3 (x, y, z) in meters. + * @param x_confidence The confidence in meters for the x dimension (optional, default: ObjectDimensionConfidence::UNAVAILABLE). + * @param y_confidence The confidence in meters for the y dimension (optional, default: ObjectDimensionConfidence::UNAVAILABLE). + * @param z_confidence The confidence in meters for the z dimension (optional, default: ObjectDimensionConfidence::UNAVAILABLE). + */ inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Vector3& dimensions, const uint8_t x_confidence = ObjectDimensionConfidence::UNAVAILABLE, const uint8_t y_confidence = ObjectDimensionConfidence::UNAVAILABLE, @@ -451,28 +479,31 @@ inline void setDimensionsOfPerceivedObject(PerceivedObject& object, const gm::Ve } /** - * @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 - */ + * @brief Initializes a PerceivedObject with the given point and delta time. + * + * This function sets the position and measurement delta time of the PerceivedObject. + * + * @param object The PerceivedObject to be initialized. + * @param point The position of the PerceivedObject relative to the CPM's reference position in meters. + * @param delta_time The measurement delta time of the PerceivedObject in milliseconds (default = 0). + */ inline void initPerceivedObject(PerceivedObject& object, const gm::Point& point, const int16_t delta_time = 0) { setPositionOfPerceivedObject(object, point); setMeasurementDeltaTimeOfPerceivedObject(object, delta_time); } /** - * @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 - */ + * @brief Initializes a PerceivedObject with the given point (utm-position) and delta time. + * + * This function initializes a PerceivedObject within a position and measurement delta time. + * It sets the position of a perceived object using the provided UTM position and the CPM's reference position. + * It sets the measurement delta time using the provided delta_time value. + * + * @param cpm The CPM to get the reference position from. + * @param object The PerceivedObject to be initialized. + * @param point The gm::PointStamped representing the UTM position of the object including the frame_id (utm_). + * @param delta_time The measurement delta time in milliseconds (default: 0). + */ inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, PerceivedObject& object, const gm::PointStamped& point, const int16_t delta_time = 0) { setUTMPositionOfPerceivedObject(cpm, object, point); @@ -480,12 +511,12 @@ inline void initPerceivedObjectWithUTMPosition(CollectivePerceptionMessage& cpm, } /** - * @brief Initializes the Perceived Object Container within a WrappedCpmContainer. + * @brief Initializes a WrappedCpmContainer as a PerceivedObjectContainer with the given number of objects. * * 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 container A reference to the WrappedCpmContainer to be initialized as a PerceivedObjectContainer. * @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) { @@ -495,7 +526,7 @@ inline void initPerceivedObjectContainer(WrappedCpmContainer& container, const u } /** - * @brief Adds a PerceivedObject to the PerceivedObjectContainer within the given WrappedCpmContainer. + * @brief Adds a PerceivedObject to the PerceivedObjectContainer / 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 diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h index c576f1e08..ec5dedd03 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h @@ -41,8 +41,9 @@ SOFTWARE. * @param n_leap_seconds number of leap-seconds since 2004. (Default: etsi_its_msgs::N_LEAP_SECONDS) * @return uint64_t the corresponding Unix-Nanoseconds */ -inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time){ - double unix_time_with_leap_seconds = reference_time.value*1e-3+etsi_its_msgs::UNIX_SECONDS_2004; - uint16_t n_leap_seconds = etsi_its_msgs::getLeapSecondInsertionsSince2004(static_cast(unix_time_with_leap_seconds)); - return (unix_time_with_leap_seconds - n_leap_seconds) * 1e9; +inline uint64_t getUnixNanosecondsFromReferenceTime(const TimestampIts& reference_time) { + double unix_time_with_leap_seconds = reference_time.value * 1e-3 + etsi_its_msgs::UNIX_SECONDS_2004; + uint16_t n_leap_seconds = + etsi_its_msgs::getLeapSecondInsertionsSince2004(static_cast(unix_time_with_leap_seconds)); + return (unix_time_with_leap_seconds - n_leap_seconds) * 1e9; } \ No newline at end of file From 01199c928943296a33f4a8b786a127646bd806ac Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 10 Oct 2024 12:43:03 +0000 Subject: [PATCH 45/52] fix wrong data type in cpm setters --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 21743c8c6..a43a16290 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 @@ -149,10 +149,10 @@ inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, co * @param value The value to be set in centimeters. * @param confidence The confidence to be set in centimeters (default: CoordinateConfidence::UNAVAILABLE). */ -inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const int16_t value, +inline void setCartesianCoordinateWithConfidence(CartesianCoordinateWithConfidence& coordinate, const int32_t value, const uint16_t confidence = CoordinateConfidence::UNAVAILABLE) { // limit value range - int16_t limited_value = std::max(CartesianCoordinateLarge::NEGATIVE_OUT_OF_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; From 66f6f6bcd6066c3f7bc9929cdc94f7c5c93bc25d Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 10 Oct 2024 13:08:05 +0000 Subject: [PATCH 46/52] small cleanup in cpm rviz plugin --- .../include/displays/CPM/cpm_render_object.hpp | 2 +- etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp | 5 ++--- etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp | 3 +-- 3 files changed, 4 insertions(+), 6 deletions(-) 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 360b01461..50111071a 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 @@ -53,7 +53,7 @@ class CPMRenderObject * @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); + CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, uint8_t number_of_object=0); /** * @brief This function validates all float variables that are part of a CPMRenderObject 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 b32ab9c46..6b3028afa 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -91,16 +91,15 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM return; } - uint16_t n_leap_seconds = getLeapSecondInsertionsSince2004(static_cast(now.seconds())); uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects( etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(*msg)); - CPMRenderObject cpm(*msg, now, n_leap_seconds, 0); + CPMRenderObject cpm(*msg, 0); // TODO: do we need this? cpm_render_objects_.clear(); for (int i = 0; i < number_of_objects; i++) { - CPMRenderObject cpm(*msg, now, n_leap_seconds, i); + CPMRenderObject cpm(*msg, i); if (!cpm.validateFloats()) { setStatus(rviz_common::properties::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)"); 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 a1807ced1..3ca74618a 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 @@ -27,8 +27,7 @@ SOFTWARE. namespace etsi_its_msgs { namespace displays { -CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, rclcpp::Time receive_time, - uint16_t n_leap_seconds, uint8_t number_of_object) { +CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, uint8_t number_of_object) { 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); From 189bbef5669d630e314021b138b2ab6796eee1e1 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 10 Oct 2024 14:42:55 +0000 Subject: [PATCH 47/52] do not use std::min/max for range checks because of ROS 1 incompatibilities --- .../impl/cpm/cpm_ts_setters.h | 39 +++++++++++++------ 1 file changed, 27 insertions(+), 12 deletions(-) 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 a43a16290..58a750534 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 @@ -152,9 +152,13 @@ inline void setMeasurementDeltaTimeOfPerceivedObject(PerceivedObject& object, co 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; + if (value < CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE) { + coordinate.value.value = CartesianCoordinateLarge::NEGATIVE_OUT_OF_RANGE; + } else if (value > CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE) { + coordinate.value.value = CartesianCoordinateLarge::POSITIVE_OUT_OF_RANGE; + } else { + coordinate.value.value = value; + } // limit confidence range if (confidence > CoordinateConfidence::MAX || confidence < CoordinateConfidence::MIN) { @@ -236,9 +240,13 @@ inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, Pe 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; + if (value < VelocityComponentValue::NEGATIVE_OUT_OF_RANGE) { + velocity.value.value = VelocityComponentValue::NEGATIVE_OUT_OF_RANGE; + } else if (value > VelocityComponentValue::POSITIVE_OUT_OF_RANGE) { + velocity.value.value = VelocityComponentValue::POSITIVE_OUT_OF_RANGE; + } else { + velocity.value.value = value; + } // limit confidence range if (confidence > SpeedConfidence::MAX || confidence < SpeedConfidence::MIN) { @@ -288,9 +296,13 @@ inline void setVelocityOfPerceivedObject(PerceivedObject& object, const gm::Vect 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; + if (value < AccelerationValue::NEGATIVE_OUT_OF_RANGE) { + acceleration.value.value = AccelerationValue::NEGATIVE_OUT_OF_RANGE; + } else if (value > AccelerationValue::POSITIVE_OUT_OF_RANGE) { + acceleration.value.value = AccelerationValue::POSITIVE_OUT_OF_RANGE; + } else { + acceleration.value.value = value; + } // limit confidence range if (confidence > AccelerationConfidence::MAX || confidence < AccelerationConfidence::MIN) { @@ -370,9 +382,12 @@ inline void setYawRateOfPerceivedObject(PerceivedObject& object, const double ya 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)); + // limit value range + if (yaw_rate_in_degrees < CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE) { + yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::NEGATIVE_OUTOF_RANGE; + } else if (yaw_rate_in_degrees > CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE) { + yaw_rate_in_degrees = CartesianAngularVelocityComponentValue::POSITIVE_OUT_OF_RANGE; + } } object.z_angular_velocity.value.value = yaw_rate_in_degrees; object.z_angular_velocity.confidence.value = confidence; From c2fb23f63aeb43e55945523f948cfc2678b17b88 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Thu, 10 Oct 2024 18:58:18 +0200 Subject: [PATCH 48/52] improve log of exception --- .../include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 58a750534..1f8b29129 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 @@ -213,7 +213,9 @@ inline void setUTMPositionOfPerceivedObject(CollectivePerceptionMessage& cpm, Pe 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"); + throw std::invalid_argument("UTM-Position frame_id (" + utm_position.header.frame_id + + ") does not match the reference position frame_id (" + reference_position.header.frame_id + + ")"); } setCartesianCoordinateWithConfidence(object.position.x_coordinate, (utm_position.point.x - reference_position.point.x) * 100, x_confidence); From f07fba52ee6623bd3f1b0e43bef39ded40b385a6 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Fri, 11 Oct 2024 15:38:33 +0200 Subject: [PATCH 49/52] fix handling of empty object list in CPM plugin --- .../src/displays/CPM/cpm_display.cpp | 2 +- .../src/displays/CPM/cpm_render_object.cpp | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) 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 6b3028afa..5d9ffac1e 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -94,7 +94,7 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects( etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(*msg)); - CPMRenderObject cpm(*msg, 0); // TODO: do we need this? + CPMRenderObject cpm(*msg, 0); // TODO: do not use render object here; can be done with StationId and ReferenceTime cpm_render_objects_.clear(); 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 3ca74618a..b76db5cdd 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 @@ -28,22 +28,23 @@ namespace etsi_its_msgs { namespace displays { CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, uint8_t number_of_object) { - + try { 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); - geometry_msgs::msg::PointStamped utm_position = etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject(cpm, object); - 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_.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); + } catch(const std::exception& e) { + RCLCPP_ERROR(rclcpp::get_logger("CPMRenderObject"), "Unavailable PerceivedObject from CPM: %s", e.what()); + } + + uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_cpm_ts_msgs::access::getReferenceTime(cpm)); + header_.stamp = rclcpp::Time(nanosecs); - station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm); + station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm); } bool CPMRenderObject::validateFloats() { From f592d198ab30bb9b3b7a292aa176965a11bddfa7 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Fri, 11 Oct 2024 16:36:46 +0000 Subject: [PATCH 50/52] improve CPM RViz plugin --- .../include/displays/CPM/cpm_display.hpp | 3 - .../displays/CPM/cpm_render_object.hpp | 65 +++++-------------- .../src/displays/CPM/cpm_display.cpp | 38 ++++------- .../src/displays/CPM/cpm_render_object.cpp | 54 ++++++++------- 4 files changed, 62 insertions(+), 98 deletions(-) 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 479a39b59..8a8438a9c 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -78,9 +78,6 @@ class CPMDisplay : public rviz_common::RosTopicDisplay cpms_; - // Rendering objects - std::vector> cpm_render_objects_; - 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 50111071a..15f61dcbc 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 @@ -40,20 +40,17 @@ namespace displays /** * @class CPMRenderObject - * @brief + * @brief This class is used to render a CPM object in RViz */ class CPMRenderObject { public: /** - * @brief Construct a new CPMRenderObject object + * @brief Construct a new CPMRenderObject object from a CPM message * * @param cpm - * @param receive_time - * @param n_leap_seconds - * @param number_of_object */ - CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, uint8_t number_of_object=0); + CPMRenderObject(const etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm); /** * @brief This function validates all float variables that are part of a CPMRenderObject @@ -67,58 +64,28 @@ class CPMRenderObject * @param now reference point in time to calculate the age with * @return age in seconds as double value */ - double getAge(rclcpp::Time now); + double getAge(const rclcpp::Time now); - /** - * @brief Get header of CPM-object - * - * @return std_msgs::msg::Header - */ std_msgs::msg::Header getHeader(); - - /** - * @brief Get the StationID of CPM-object - * - * @return int - */ uint32_t getStationID(); + geometry_msgs::msg::PointStamped getReferencePosition(); - /** - * @brief Get pose of CPM-object - * - * @return geometry_msgs::msg::Pose - */ - geometry_msgs::msg::Pose getPose(); - - /** - * @brief Get dimensions of CPM-Object - * - * @return geometry_msgs::msg::Vector3 (x equals length, y equals width, z equals height) - */ - geometry_msgs::msg::Vector3 getDimensions(); - - /** - * @brief Get speed of CPM-object - * - * @return double - */ - geometry_msgs::msg::Vector3 getVelocity(); - - /** - * @brief Get number of objects in CPM-object - * - * @return uint8_t - */ uint8_t getNumberOfObjects(); + geometry_msgs::msg::Pose getPoseOfObject(const uint8_t idx); + geometry_msgs::msg::Vector3 getDimensionsOfObject(const uint8_t idx); + geometry_msgs::msg::Vector3 getVelocityOfObject(const uint8_t idx); + + struct Object { + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Vector3 dimensions; + geometry_msgs::msg::Vector3 velocity; + }; private: - // member variables std_msgs::msg::Header header_; uint32_t station_id_; - geometry_msgs::msg::Pose pose_; - geometry_msgs::msg::Vector3 dimensions_; - geometry_msgs::msg::Vector3 velocity_; - + geometry_msgs::msg::PointStamped reference_position_; + std::vector objects_; }; } // namespace displays 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 5d9ffac1e..a2ee6f4e4 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -91,29 +91,21 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM return; } - uint8_t number_of_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects( - etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(*msg)); + CPMRenderObject cpm(*msg); - CPMRenderObject cpm(*msg, 0); // TODO: do not use render object here; can be done with StationId and ReferenceTime - - cpm_render_objects_.clear(); - - for (int i = 0; i < number_of_objects; i++) { - CPMRenderObject cpm(*msg, i); - if (!cpm.validateFloats()) { - setStatus(rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - cpm_render_objects_.push_back(std::make_shared(cpm)); + if (!cpm.validateFloats()) { + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; } // Check if Station ID is already present in list auto it = cpms_.find(cpm.getStationID()); - if (it != cpms_.end()) + if (it != cpms_.end()) { it->second = cpm; // Key exists, update the value - else + } else { cpms_.insert(std::make_pair(cpm.getStationID(), cpm)); + } return; } @@ -121,7 +113,7 @@ void CPMDisplay::processMessage(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionM void CPMDisplay::update(float, float) { // Check for outdated CPMs for (auto it = cpms_.begin(); it != cpms_.end();) { - if (it->second.getAge(rviz_node_->now()) / 10e9 > buffer_timeout_->getFloat()) { + if (it->second.getAge(rviz_node_->now()) > buffer_timeout_->getFloat()) { it = cpms_.erase(it); } else { ++it; @@ -130,10 +122,8 @@ void CPMDisplay::update(float, float) { bboxs_.clear(); texts_.clear(); for (auto it = cpms_.begin(); it != cpms_.end(); ++it) { - for (const auto& cpm_ptr : cpm_render_objects_) { - // Dereference the shared_ptr to access the CPMRenderObject - CPMRenderObject& cpm = *cpm_ptr; - + CPMRenderObject cpm = it->second; + for (int i = 0; i < cpm.getNumberOfObjects(); i++) { Ogre::Vector3 sn_position; Ogre::Quaternion sn_orientation; if (!context_->getFrameManager()->getTransform(cpm.getHeader(), sn_position, sn_orientation)) { @@ -171,9 +161,9 @@ void CPMDisplay::update(float, float) { auto child_scene_node = scene_node_->createChildSceneNode(); // Set position of scene node to the position relative to the fixed frame - geometry_msgs::msg::Pose pose = cpm.getPose(); + geometry_msgs::msg::Pose pose = cpm.getPoseOfObject(i); - geometry_msgs::msg::Vector3 dimensions = cpm.getDimensions(); + geometry_msgs::msg::Vector3 dimensions = cpm.getDimensionsOfObject(i); tf2::doTransform(pose, pose, transform_to_fixed_frame); Ogre::Vector3 position(pose.position.x, pose.position.y, pose.position.z); Ogre::Quaternion orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); @@ -206,7 +196,7 @@ void CPMDisplay::update(float, float) { text += "\n"; } if (show_speed_->getBool()) { - geometry_msgs::msg::Vector3 velocity = cpm.getVelocity(); + geometry_msgs::msg::Vector3 velocity = cpm.getVelocityOfObject(i); //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"; 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 b76db5cdd..26eb053e3 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 @@ -27,45 +27,55 @@ SOFTWARE. namespace etsi_its_msgs { namespace displays { -CPMRenderObject::CPMRenderObject(etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm, uint8_t number_of_object) { - try { - 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); - geometry_msgs::msg::PointStamped utm_position = etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject(cpm, object); - 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_.frame_id = utm_position.header.frame_id; - } catch(const std::exception& e) { - RCLCPP_ERROR(rclcpp::get_logger("CPMRenderObject"), "Unavailable PerceivedObject from CPM: %s", e.what()); - } +CPMRenderObject::CPMRenderObject(const etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm) { + + station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm); + reference_position_ = etsi_its_cpm_ts_msgs::access::getUTMPosition(cpm); uint64_t nanosecs = etsi_its_cpm_ts_msgs::access::getUnixNanosecondsFromReferenceTime(etsi_its_cpm_ts_msgs::access::getReferenceTime(cpm)); header_.stamp = rclcpp::Time(nanosecs); - - station_id_ = etsi_its_cpm_ts_msgs::access::getStationID(cpm); + header_.frame_id = reference_position_.header.frame_id; + + int n_perceived_objects = etsi_its_cpm_ts_msgs::access::getNumberOfPerceivedObjects(cpm); + for (int i = 0; i < n_perceived_objects; i++) { + etsi_its_cpm_ts_msgs::msg::PerceivedObject perceived_obj = etsi_its_cpm_ts_msgs::access::getPerceivedObject( + etsi_its_cpm_ts_msgs::access::getPerceivedObjectContainer(cpm), i); + geometry_msgs::msg::PointStamped utm_position = etsi_its_cpm_ts_msgs::access::getUTMPositionOfPerceivedObject(cpm, perceived_obj); + + Object obj; + obj.pose.position = utm_position.point; + obj.pose.orientation = etsi_its_cpm_ts_msgs::access::getOrientationOfPerceivedObject(perceived_obj); + obj.dimensions = etsi_its_cpm_ts_msgs::access::getDimensionsOfPerceivedObject(perceived_obj); + obj.velocity = etsi_its_cpm_ts_msgs::access::getCartesianVelocityOfPerceivedObject(perceived_obj); + objects_.push_back(obj); + } } bool CPMRenderObject::validateFloats() { bool valid = true; - valid = valid && rviz_common::validateFloats(pose_); - valid = valid && rviz_common::validateFloats(dimensions_); - valid = valid && rviz_common::validateFloats(velocity_); + for (size_t i = 0; i < objects_.size(); i++) { + valid = valid && rviz_common::validateFloats(objects_[i].pose); + valid = valid && rviz_common::validateFloats(objects_[i].dimensions); + valid = valid && rviz_common::validateFloats(objects_[i].velocity); + } return valid; } -double CPMRenderObject::getAge(rclcpp::Time now) { return (now - header_.stamp).seconds(); } +double CPMRenderObject::getAge(const rclcpp::Time now) { return (now - header_.stamp).seconds(); } std_msgs::msg::Header CPMRenderObject::getHeader() { return header_; } uint32_t CPMRenderObject::getStationID() { return station_id_; } -geometry_msgs::msg::Pose CPMRenderObject::getPose() { return pose_; } +geometry_msgs::msg::PointStamped CPMRenderObject::getReferencePosition() { return reference_position_; } + +geometry_msgs::msg::Pose CPMRenderObject::getPoseOfObject(const uint8_t idx) { return objects_[idx].pose; } + +geometry_msgs::msg::Vector3 CPMRenderObject::getDimensionsOfObject(const uint8_t idx) { return objects_[idx].dimensions; } -geometry_msgs::msg::Vector3 CPMRenderObject::getDimensions() { return dimensions_; } +geometry_msgs::msg::Vector3 CPMRenderObject::getVelocityOfObject(const uint8_t idx) { return objects_[idx].velocity; } -geometry_msgs::msg::Vector3 CPMRenderObject::getVelocity() { return velocity_; } +uint8_t CPMRenderObject::getNumberOfObjects() { return objects_.size(); } } // namespace displays } // namespace etsi_its_msgs \ No newline at end of file From ce0fc785692cd55d76a76ed8b425fdc843b02bf6 Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Tue, 15 Oct 2024 16:15:36 +0200 Subject: [PATCH 51/52] add suggestions from MR --- etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp | 1 + .../include/displays/CPM/cpm_render_object.hpp | 1 + etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp | 2 +- etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp | 5 ++--- etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp | 2 +- 5 files changed, 6 insertions(+), 5 deletions(-) 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 8a8438a9c..5b0827d99 100644 --- a/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -40,6 +40,7 @@ class ManualObject; namespace rviz_common { namespace properties { +class BoolProperty; class ColorProperty; class FloatProperty; } // namespace properties 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 15f61dcbc..63a8f6732 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 @@ -23,6 +23,7 @@ SOFTWARE. ============================================================================= */ #include "etsi_its_cpm_ts_msgs/msg/collective_perception_message.hpp" +#include #include #include #include diff --git a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp index 5646ea49c..152d4d2b8 100644 --- a/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CAM/cam_display.cpp @@ -205,7 +205,7 @@ void CAMDisplay::update(float, float) child_scene_node->setPosition(position); child_scene_node->setOrientation(orientation); - // create boundind-box object + // create bounding-box object std::shared_ptr bbox = std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); // set the dimensions of bounding box 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 a2ee6f4e4..cfa3767c2 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -128,8 +128,7 @@ void CPMDisplay::update(float, float) { Ogre::Quaternion sn_orientation; if (!context_->getFrameManager()->getTransform(cpm.getHeader(), sn_position, sn_orientation)) { // Check if transform exists - // setMissingTransformToFixedFrame(cpm.getHeader().frame_id); - setMissingTransformToFixedFrame("map"); + setMissingTransformToFixedFrame(cpm.getHeader().frame_id); return; } // We don't want to use the transform in sn_position and sn_orientation though, because they are only in float precision. @@ -172,7 +171,7 @@ void CPMDisplay::update(float, float) { child_scene_node->setPosition(position); child_scene_node->setOrientation(orientation); - // create boundind-box object + // create bounding-box object std::shared_ptr bbox = std::make_shared(rviz_rendering::Shape::Cube, scene_manager_, child_scene_node); diff --git a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp index 03474f15e..a99e0ed84 100644 --- a/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/DENM/denm_display.cpp @@ -72,7 +72,7 @@ void DENMDisplay::processMessage(etsi_its_denm_msgs::msg::DENM::ConstSharedPtr m { // Generate DENM render object from message rclcpp::Time now = rviz_node_->now(); - DENMRenderObject denm(*msg); // 5 leap seconds in 2023 + DENMRenderObject denm(*msg); if (!denm.validateFloats()) { setStatus( rviz_common::properties::StatusProperty::Error, "Topic", From bd4310854febd0aafa935dc935d95b52bf5765cc Mon Sep 17 00:00:00 2001 From: Jean-Pierre Busch Date: Tue, 15 Oct 2024 16:48:59 +0200 Subject: [PATCH 52/52] update defaults of CPM plugin --- etsi_its_rviz_plugins/CMakeLists.txt | 3 --- etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp | 4 ++-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/etsi_its_rviz_plugins/CMakeLists.txt b/etsi_its_rviz_plugins/CMakeLists.txt index 6a329d7f9..4f345b8b7 100644 --- a/etsi_its_rviz_plugins/CMakeLists.txt +++ b/etsi_its_rviz_plugins/CMakeLists.txt @@ -26,11 +26,8 @@ if(${ROS_VERSION} EQUAL 2) set(display_headers_to_moc include/displays/CAM/cam_display.hpp - include/displays/CAM/cam_render_object.hpp include/displays/DENM/denm_display.hpp - include/displays/DENM/denm_render_object.hpp include/displays/CPM/cpm_display.hpp - include/displays/CPM/cpm_render_object.hpp ) foreach(header "${display_headers_to_moc}") 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 cfa3767c2..1525865a9 100644 --- a/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -47,7 +47,7 @@ namespace displays { CPMDisplay::CPMDisplay() { // General Properties buffer_timeout_ = - new rviz_common::properties::FloatProperty("Timeout", 0.1f, "Time (in s) until objects disappear", this); + new rviz_common::properties::FloatProperty("Timeout", 1.0f, "Time period (in s) in which CPM are valid and should be displayed (now - reference_time of CPM)", this); buffer_timeout_->setMin(0); bb_scale_ = new rviz_common::properties::FloatProperty("Scale", 1.0f, "Scale of objects", this); bb_scale_->setMin(0.01); @@ -56,7 +56,7 @@ CPMDisplay::CPMDisplay() { new rviz_common::properties::BoolProperty("Metadata", true, "Show metadata as text next to objects", this); text_color_property_ = new rviz_common::properties::ColorProperty("Color", QColor(25, 0, 255), "Text color", show_meta_); - char_height_ = new rviz_common::properties::FloatProperty("Scale", 4.0, "Scale of text", show_meta_); + char_height_ = new rviz_common::properties::FloatProperty("Scale", 0.5, "Scale of text", show_meta_); show_station_id_ = new rviz_common::properties::BoolProperty("StationID", true, "Show StationID", show_meta_); show_speed_ = new rviz_common::properties::BoolProperty("Speed", true, "Show speed", show_meta_); }