diff --git a/etsi_its_msgs_utils/CMakeLists.txt b/etsi_its_msgs_utils/CMakeLists.txt index 7fe623f33..9e377f8b2 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} @@ -44,39 +46,16 @@ 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 - ) - 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 - $ - $ - ) - 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}-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 + tf2_geometry_msgs ) endif() @@ -90,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) @@ -100,6 +80,7 @@ elseif(${ROS_VERSION} EQUAL 1) CATKIN_DEPENDS etsi_its_msgs geometry_msgs + tf2_geometry_msgs ) include_directories( @@ -113,15 +94,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/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..b713921e5 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.h @@ -0,0 +1,48 @@ +/* +============================================================================= +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 +#include +#include +#include +#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/cpm_ts_access.hpp b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp new file mode 100644 index 000000000..5c4ed6533 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/cpm_ts_access.hpp @@ -0,0 +1,49 @@ +/* +============================================================================= +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 +#include +#include +#include +#include +#include + +namespace etsi_its_cpm_ts_msgs { +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/impl/cam/cam_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_access.h index 0ded5ced3..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 @@ -29,6 +29,17 @@ 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 +#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 @@ -42,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 0ffeb51ff..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 @@ -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 @@ -37,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 @@ -47,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 @@ -57,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 @@ -67,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 @@ -77,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); } @@ -87,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); } @@ -97,10 +90,20 @@ 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); } +/** + * @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 * @@ -109,18 +112,29 @@ inline double getAltitude(const CAM& cam){ * @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); } +/** + * @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 * * @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); } /** @@ -129,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); } /** @@ -139,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); } @@ -149,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); } /** @@ -159,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!"); } } @@ -181,26 +196,103 @@ 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); +} + +/** + * @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 + * + * @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 * * @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!"); } -} \ No newline at end of file +} + +/** + * @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); +} + +#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 77e1953d0..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,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 @@ -40,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; } /** @@ -55,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); } @@ -65,35 +70,36 @@ 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); } /** - * @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); } /** @@ -105,8 +111,34 @@ inline void setReferencePosition(CAM& cam, const double latitude, const double l * @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); +} + +/** + * @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); } /** @@ -116,9 +148,12 @@ inline void setHeading(CAM& cam, const double heading_val){ * @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); } /** @@ -127,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); } @@ -137,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); } /** @@ -147,9 +184,28 @@ 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; +} + +/** + * @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, altitude); } /** @@ -164,30 +220,93 @@ inline void setLateralAcceleration(CAM& cam, const double lat_accel){ * @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); } +/** + * @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 * * @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!"); } -} \ No newline at end of file +} + +/** + * @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 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); +} + +#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 8f4e3a9a3..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 @@ -29,6 +29,17 @@ 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 +#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 @@ -42,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 e3ed27a27..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,17 +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); - } - -} // namespace etsi_its_cam_ts_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, MessageId::CAM, station_id, protocol_version); +} + +} // 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 ba54b749a..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 @@ -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 @@ -41,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; } @@ -57,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; } /** @@ -70,9 +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); -} \ No newline at end of file +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H \ No newline at end of file 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..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 @@ -29,9 +29,14 @@ 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 + 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 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..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 @@ -28,10 +28,11 @@ 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 -#include +#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 @@ -39,9 +40,7 @@ SOFTWARE. * @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 @@ -49,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 @@ -59,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 @@ -69,31 +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; -} - -/** - * @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; -} +inline double getAltitude(const Altitude& altitude) { return ((double)altitude.altitude_value.value) * 1e-2; } /** * @brief Get the Vehicle Width @@ -101,9 +72,7 @@ inline double getVehicleLength(const VehicleLength& vehicle_length){ * @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 @@ -111,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 @@ -132,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; @@ -149,61 +114,35 @@ inline std::vector getBitString(const std::vector& buffer, const } /** - * @brief Get Acceleration Control in form of bool vector + * @brief Get the UTM Position defined by the given ReferencePosition * - * @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 + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate * - * @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 - * - * @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 - * - * @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); +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; } -/** - * @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 +#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 1f957139c..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 @@ -29,14 +29,13 @@ 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 #include #include - - +#include /** * @brief Set the TimestampITS object @@ -46,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; } @@ -59,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; } @@ -71,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; } @@ -83,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; + } } /** @@ -102,59 +107,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 * @@ -162,7 +114,7 @@ inline void setVehicleLength(VehicleLength& vehicle_length, 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; } @@ -174,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; } @@ -192,6 +144,60 @@ 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 * @@ -212,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; @@ -226,62 +230,4 @@ inline void setBitString(T& bitstring, const std::vector& bits) { } } -/** - * @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 +#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 3c008b332..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 @@ -28,11 +28,13 @@ 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 +#include /** * @brief Get the lateral acceleration @@ -40,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; } /** @@ -50,34 +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; } -/** - * @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; -} \ 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 6dfc54752..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 @@ -28,13 +28,14 @@ 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 -#include +#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 /** * @brief Set the Station Id object @@ -55,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; } @@ -74,38 +77,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 * @@ -113,10 +84,14 @@ inline void setReferencePosition(ReferencePosition& ref_position, const double l * @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; + } } /** @@ -132,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; + } } /** @@ -158,30 +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) - */ -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); -} \ 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..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 @@ -28,9 +28,12 @@ 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 /** @@ -39,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; } /** @@ -49,34 +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; } -/** - * @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; -} \ 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 1b482ddc7..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 @@ -29,14 +29,14 @@ 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 #include #include #include - +#include /** * @brief Set the Station Id object @@ -57,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; @@ -76,38 +77,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 * @@ -115,10 +84,14 @@ inline void setReferencePosition(ReferencePositionWithConfidence& ref_position, * @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; + } } /** @@ -134,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; + } } /** @@ -160,30 +137,4 @@ inline void setLateralAcceleration(AccelerationComponent& accel, const double va 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 +#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/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/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..4bf45c8f9 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h @@ -0,0 +1,53 @@ +/* +============================================================================= +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 + */ + +#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 +#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..fd987c995 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_getters.h @@ -0,0 +1,516 @@ +/* +============================================================================= +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 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 in milliseconds + */ +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); +} + +/** + * @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 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); +} + +/** + * @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[in] cpm CPM to get the UTM Position from + * @param[out] zone the UTM zone (zero means UPS) + * @param[out] northp hemisphere (true means north, false means south) + * @return geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id + */ +inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm, int &zone, bool &northp) { + return getUTMPosition(cpm.payload.management_container.reference_position, zone, northp); +} + +/** + * @brief Get the UTM Position defined within the ManagementContainer of the CPM + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param cpm CPM from which to extract the UTM position. + * @return geometry_msgs::PointStamped of the given position with the UTM zone and hemisphere as frame_id + */ +inline gm::PointStamped getUTMPosition(const CollectivePerceptionMessage &cpm) { + int zone; + bool northp; + return getUTMPosition(cpm, zone, northp); +} + +/** + * @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++) { + container_ids.push_back(cpm.payload.cpm_containers.value.array[i].container_id.value); + } + return container_ids; +} + +/** + * 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) { + 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 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); +} + +/** + * 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"); + } + uint8_t number = container.container_data.perceived_object_container.number_of_perceived_objects.value; + return number; +} + +/** + * Retrieves the number of perceived objects from the given CPM. + * + * @param cpm The CPM from which to retrieve the number of perceived objects. + * @return The number of perceived objects. + */ +inline uint8_t getNumberOfPerceivedObjects(const CollectivePerceptionMessage &cpm) { + return getNumberOfPerceivedObjects(getPerceivedObjectContainer(cpm)); +} + +////////////////////////////////////////////////////////////////////////// +/////////////////// getters for the PerceivedObject ////////////////////// +////////////////////////////////////////////////////////////////////////// + +/** + * @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"); + } + return container.container_data.perceived_object_container.perceived_objects.array[i]; +} + +/** + * @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 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 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; +} + +/** + * 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.0; + point.y = double(getCartesianCoordinate(object.position.y_coordinate)) / 100.0; + if (object.position.z_coordinate_is_present) { + point.z = double(getCartesianCoordinate(object.position.z_coordinate)) / 100.0; + } + return point; +} + +/** + * @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 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 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.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.0) - 180.0) / 180.0) * + M_PI; // TODO: check if 0-360 -> -180-180 is correct + } + 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 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); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + return yaw; +} + +/** + * @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); + pose.orientation = getOrientationOfPerceivedObject(object); + return pose; +} + +/** + * @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.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 + */ +inline double getVelocityComponentConfidence(const VelocityComponent &velocity) { + 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 (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) { + 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 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.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 + */ +inline double getAccelerationComponentConfidence(const AccelerationComponent &acceleration) { + 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 (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) { + 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 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. + * + * 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 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"); + 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 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"); + return object.object_dimension_z.value.value; +} + +/** + * @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.0; + dimensions.y = double(getYDimensionOfPerceivedObject(object)) / 10.0; + dimensions.z = double(getZDimensionOfPerceivedObject(object)) / 10.0; + return dimensions; +} + +/** + * @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; + gm::PointStamped reference_position = getUTMPosition(cpm); + gm::Point relative_position = getPositionOfPerceivedObject(object); + + utm_position.header.frame_id = reference_position.header.frame_id; + utm_position.point.x = reference_position.point.x + relative_position.x; + utm_position.point.y = reference_position.point.y + relative_position.y; + utm_position.point.z = reference_position.point.z + relative_position.z; + + return utm_position; +} + +} // 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 new file mode 100644 index 000000000..1f8b29129 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h @@ -0,0 +1,590 @@ +/* +============================================================================= +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 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 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) { + 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. + * 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 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 + */ +inline void setIdOfPerceivedObject(PerceivedObject& object, const uint16_t id) { + object.object_id.value = id; + object.object_id_is_present = true; +} + +/** + * @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"); + } else { + object.measurement_delta_time.value = delta_time; + } +} + +/** + * @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 int32_t value, + const uint16_t confidence = CoordinateConfidence::UNAVAILABLE) { + // limit value range + 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) { + coordinate.confidence.value = CoordinateConfidence::OUT_OF_RANGE; + } else { + coordinate.confidence.value = confidence; + } +} + +/** + * @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, + 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; + } +} + +/** + * @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, + 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 (" + 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); + 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 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 + 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) { + velocity.confidence.value = SpeedConfidence::OUT_OF_RANGE; + } else { + velocity.confidence.value = confidence; + } +} + +/** + * 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, + 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; +} + +/** + * @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 + 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) { + acceleration.confidence.value = AccelerationConfidence::OUT_OF_RANGE; + } else { + acceleration.confidence.value = confidence; + } +} + +/** + * @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, + 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; +} + +/** + * @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; // 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; + + 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 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; + if (yaw_rate_in_degrees != CartesianAngularVelocityComponentValue::UNAVAILABLE) { + // 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; + object.z_angular_velocity_is_present = true; +} + +/** + * @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 + 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 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); + object.object_dimension_x_is_present = true; +} + +/** + * @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); + object.object_dimension_y_is_present = true; +} + +/** + * @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); + object.object_dimension_z_is_present = true; +} + +/** + * @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, + 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 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 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); + setMeasurementDeltaTimeOfPerceivedObject(object, delta_time); +} + +/** + * @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 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) { + 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 / 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"); + } +} + +/** + * @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 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..ec5dedd03 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h @@ -0,0 +1,49 @@ +/* +============================================================================= +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) { + 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_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/denm/denm_access.h index 3f66aab84..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 @@ -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 @@ -42,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 772b6a7ad..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,384 +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 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 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 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 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 + * @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 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 + * + * @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 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"; +/** + * @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; } - return cause_code_type; - } - else{ - throw std::invalid_argument("SituationContainer is not present!"); - } +/** + * @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"; + + 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!"); } +} - /** - * @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 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 492b14d92..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,148 +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 ReferencePosition for a DENM - * - * Altitude is set to 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 - */ - inline void setReferencePosition(DENM& denm, const double latitude, const double longitude) - { - setReferencePosition(denm.denm.management.event_position, latitude, longitude); - } +/** + * @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 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) - { - 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 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 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 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 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 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 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 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 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 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 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 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 +} // 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 cfcf9a8a8..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,7 +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, 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/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 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..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,121 +1,106 @@ -#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 cam_access = etsi_its_cam_msgs::access; TEST(etsi_its_cam_msgs, test_set_get_cam) { + cam_msgs::CAM cam; - 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 // 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_cam_msgs::access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + 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(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; - 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))); - - int stationType_val = randomInt(StationType::MIN, StationType::MAX); - setStationType(cam, stationType_val); - EXPECT_EQ(stationType_val, getStationType(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))); + + 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..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,121 +1,106 @@ -#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 cam_ts_access = etsi_its_cam_ts_msgs::access; TEST(etsi_its_cam_ts_msgs, test_set_get_cam) { + cam_ts_msgs::CAM cam; - 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 // 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_cam_ts_msgs::access::setTimestampITS(t_its, t_2007, etsi_its_msgs::getLeapSecondInsertionsSince2004(t_2007*1e-9)); + 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(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; - 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))); - - int stationType_val = randomInt(TrafficParticipantType::MIN, TrafficParticipantType::MAX); - setStationType(cam, stationType_val); - EXPECT_EQ(stationType_val, getStationType(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))); + + 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 new file mode 100644 index 000000000..f9476641f --- /dev/null +++ b/etsi_its_msgs_utils/test/impl/test_cpm_ts_access.cpp @@ -0,0 +1,89 @@ +#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); + 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, cpm_ts_access::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; + 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(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))); + + double latitude = randomDouble(-90.0, 90.0); + double longitude = randomDouble(-180.0, 180.0); + 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); + 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; + cpm_ts_access::setReferencePosition(cpm, latitude, longitude, altitude); + int zone; + bool 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); + 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); + + cpm_ts_msgs::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_msgs_utils/test/impl/test_denm_access.cpp b/etsi_its_msgs_utils/test/impl/test_denm_access.cpp index af837f97b..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,97 +1,76 @@ -#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 denm_access = etsi_its_denm_msgs::access; TEST(etsi_its_denm_msgs, test_set_get_denm) { + denm_msgs::DENM denm; - 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 // 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; - etsi_its_denm_msgs::access::setTimestampITS(t_its, t_2007, 1); + 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); - - setReferenceTime(denm, t_2007, 1); - EXPECT_EQ(94694401000, getReferenceTimeValue(denm)); - uint64_t t_2007_off = t_2007 + 5*1e9; - EXPECT_EQ(t_2007, getUnixNanosecondsFromReferenceTime(getReferenceTime(denm), 1)); - int stationType_val = randomInt(StationType::MIN, StationType::MAX); - setStationType(denm, stationType_val); - EXPECT_EQ(stationType_val, getStationType(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))); + + 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..730e16f5c --- /dev/null +++ b/etsi_its_msgs_utils/test/test_access.cpp @@ -0,0 +1,44 @@ +#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..adbb90461 --- /dev/null +++ b/etsi_its_msgs_utils/test/test_access.ros2.cpp @@ -0,0 +1,44 @@ +#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_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 diff --git a/etsi_its_rviz_plugins/CMakeLists.txt b/etsi_its_rviz_plugins/CMakeLists.txt index 07c3ed0d6..4f345b8b7 100644 --- a/etsi_its_rviz_plugins/CMakeLists.txt +++ b/etsi_its_rviz_plugins/CMakeLists.txt @@ -26,9 +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 ) foreach(header "${display_headers_to_moc}") @@ -40,6 +39,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..5b0827d99 --- /dev/null +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_display.hpp @@ -0,0 +1,87 @@ +/** ============================================================================ +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 BoolProperty; +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..63a8f6732 --- /dev/null +++ b/etsi_its_rviz_plugins/include/displays/CPM/cpm_render_object.hpp @@ -0,0 +1,93 @@ +/** ============================================================================ +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 + +#include "rviz_common/validate_floats.hpp" + +namespace etsi_its_msgs +{ +namespace displays +{ + +/** + * @class CPMRenderObject + * @brief This class is used to render a CPM object in RViz + */ +class CPMRenderObject +{ + public: + /** + * @brief Construct a new CPMRenderObject object from a CPM message + * + * @param cpm + */ + CPMRenderObject(const etsi_its_cpm_ts_msgs::msg::CollectivePerceptionMessage cpm); + + /** + * @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(const rclcpp::Time now); + + std_msgs::msg::Header getHeader(); + uint32_t getStationID(); + geometry_msgs::msg::PointStamped getReferencePosition(); + + 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: + std_msgs::msg::Header header_; + uint32_t station_id_; + geometry_msgs::msg::PointStamped reference_position_; + std::vector objects_; +}; + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file 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/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/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 new file mode 100644 index 000000000..1525865a9 --- /dev/null +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_display.cpp @@ -0,0 +1,224 @@ +/** ============================================================================ +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/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/transformation/transformation_manager.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", 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); + 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", 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_); +} + +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) { + 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); + + 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; + } + } + bboxs_.clear(); + texts_.clear(); + for (auto it = cpms_.begin(); it != cpms_.end(); ++it) { + 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)) { + // 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.getPoseOfObject(i); + + 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); + + // set pose of child scene node of bounding-box + child_scene_node->setPosition(position); + child_scene_node->setOrientation(orientation); + + // 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 + 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()) { + 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"; + } + 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..26eb053e3 --- /dev/null +++ b/etsi_its_rviz_plugins/src/displays/CPM/cpm_render_object.cpp @@ -0,0 +1,81 @@ +/** ============================================================================ +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(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); + 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; + 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(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::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::getVelocityOfObject(const uint8_t idx) { return objects_[idx].velocity; } + +uint8_t CPMRenderObject::getNumberOfObjects() { return objects_.size(); } + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file 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..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, getLeapSecondInsertionsSince2004((uint64_t)now.seconds())); // 5 leap seconds in 2023 + DENMRenderObject denm(*msg); 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);