diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/asn1_primitives/asn1_primitives_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/asn1_primitives/asn1_primitives_getters.h new file mode 100644 index 00000000..7117520e --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/asn1_primitives/asn1_primitives_getters.h @@ -0,0 +1,67 @@ +/* +============================================================================= +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. +============================================================================= +*/ + +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_GETTERS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_GETTERS_H + +/** + * @file impl/asn1_primitives_getters.h + * @brief File containing getter-functions that are used in the context of ETIS ITS Messages related to ASN.1 datatype primitives + */ + +/** + * @brief Get a Bit String in form of bool vector + * + * @param buffer as uint8_t vector + * @param bits_unused number of bits to ignore at the end of the bit string + * @return std::vector + */ +inline std::vector getBitString(const std::vector& buffer, const int bits_unused) { + // bit string size + const int bits_per_byte = 8; + const int n_bytes = buffer.size(); + const int n_bits = n_bytes * bits_per_byte; + std::vector bits; + bits.resize(n_bits - bits_unused, 0); + + // 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 = bits_per_byte - 1; bit_idx_in_byte >= 0; 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 < bits_unused) break; + + // extract bit from bitstring and set output array entry appropriately + bool byte_has_true_bit = buffer[byte_idx] & (1 << bit_idx_in_byte); + if (byte_has_true_bit) bits[bits_per_byte-bit_idx-1] = true; + } + } + return bits; +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_GETTERS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/asn1_primitives/asn1_primitives_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/asn1_primitives/asn1_primitives_setters.h new file mode 100644 index 00000000..89aaa8a2 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/asn1_primitives/asn1_primitives_setters.h @@ -0,0 +1,71 @@ +/* +============================================================================= +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. +============================================================================= +*/ + +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_SETTERS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_SETTERS_H + +/** + * @file impl/asn1_primitives_setters.h + * @brief File containing setter-functions that are used in the context of ETIS ITS Messages related to ASN.1 datatype primitives + */ + +/** + * @brief Set a Bit String by a vector of bools + * + * @tparam T + * @param bitstring BitString to set + * @param bits vector of bools + */ +template +inline void setBitString(T& bitstring, const std::vector& bits) { + // bit string size + const int bits_per_byte = 8; + const int n_bytes = (bits.size() - 1) / bits_per_byte + 1; + const int n_bits = n_bytes * bits_per_byte; + + // init output + bitstring.bits_unused = n_bits - bits.size(); + bitstring.value = std::vector(n_bytes); + + // 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 = bits_per_byte - 1; bit_idx_in_byte >= 0; 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 < bitstring.bits_unused) break; + + // set bit in output bitstring appropriately + if (bit_idx < bits.size()) { + bitstring.value[byte_idx] |= bits[bits_per_byte - bit_idx - 1] << bit_idx_in_byte; + } + } + } +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_SETTERS_H \ 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 844171a5..d13648d3 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,16 +29,18 @@ SOFTWARE. * @brief Main CAM access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_SETTERS_H #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 +#undef ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H #pragma once @@ -48,6 +50,7 @@ SOFTWARE. #include +#include #include #include 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 51e2af81..e3b8b353 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 @@ -32,6 +32,8 @@ SOFTWARE. #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H #define ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_GETTERS_COMMON_H +#include + /** * @brief Get the Station ID object * 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 172db6ff..c97c5369 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 @@ -31,8 +31,6 @@ SOFTWARE. #pragma once -#include - namespace etsi_its_cam_msgs::access { #include 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 f91b4e39..3415e5c3 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 @@ -32,7 +32,9 @@ SOFTWARE. #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H #define ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_SETTERS_COMMON_H +#include #include +#include /** * @brief Set the GenerationDeltaTime-Value 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 52a3f15c..3bc151e9 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,16 +29,18 @@ SOFTWARE. * @brief Main CAM TS access implementation header */ +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_SETTERS_H #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 +#undef ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H #pragma once @@ -48,6 +50,7 @@ SOFTWARE. #include +#include #include #include 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 a5ef2a5a..0505f051 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 @@ -31,8 +31,6 @@ SOFTWARE. #pragma once -#include - namespace etsi_its_cam_ts_msgs::access { #include 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 f2f8a8d1..2b36c611 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 @@ -29,7 +29,7 @@ SOFTWARE. * @brief Utility functions for the ETSI ITS CAM (EN and TS) */ -#include +#include #include #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CAM_CAM_UTILS_H 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 c8a09683..a3ceafa1 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 @@ -74,37 +74,6 @@ inline double getAltitude(const Altitude& altitude) { return ((double)altitude.a */ inline double getSpeed(const Speed& speed) { return ((double)speed.speed_value.value) * 1e-2; } -/** - * @brief Get a Bit String in form of bool vector - * - * @param buffer as uint8_t vector - * @param bits_unused number of bits to ignore at the end of the bit string - * @return std::vector - */ -inline std::vector getBitString(const std::vector& buffer, const int bits_unused) { - // bit string size - const int bits_per_byte = 8; - const int n_bytes = buffer.size(); - const int n_bits = n_bytes * bits_per_byte; - std::vector bits; - bits.resize(n_bits - bits_unused, 0); - - // 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; - - // extract bit from bitstring and set output array entry appropriately - bool byte_has_true_bit = buffer[byte_idx] & (1 << bit_idx_in_byte); - if (byte_has_true_bit) bits[bit_idx] = 1; - } - } - return bits; -} - /** * @brief Get the UTM Position defined by the given ReferencePosition * 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 3adc6a7f..b09dc003 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h @@ -32,7 +32,7 @@ SOFTWARE. #ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H -#include +#include #include #include #include @@ -186,36 +186,4 @@ inline void setFromUTMPosition(T& reference_position, const gm::PointStamped& ut setReferencePosition(reference_position, latitude, longitude, utm_position.point.z); } -/** - * @brief Set a Bit String by a vector of bools - * - * @tparam T - * @param bitstring BitString to set - * @param bits vector of bools - */ -template -inline void setBitString(T& bitstring, const std::vector& bits) { - // bit string size - const int bits_per_byte = 8; - const int n_bytes = (bits.size() - 1) / bits_per_byte + 1; - const int n_bits = n_bytes * bits_per_byte; - - // init output - bitstring.bits_unused = n_bits - bits.size(); - bitstring.value = std::vector(n_bytes); - - // 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; - - // set bit in output bitstring appropriately - bitstring.value[byte_idx] |= bits[bit_idx] << bit_idx_in_byte; - } - } -} - #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_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h index 93737232..eaa52abe 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 @@ -33,7 +33,7 @@ SOFTWARE. #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H #include -#include +#include #include #include 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 3978c9f9..ecda70f5 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 @@ -33,8 +33,7 @@ SOFTWARE. #define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H #include -#include -#include +#include #include #include diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/checks.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/checks.h new file mode 100644 index 00000000..cb416a2b --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/checks.h @@ -0,0 +1,61 @@ +/* +============================================================================= +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/checks.h + * @brief Sanity-check functions etc. + */ + +#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H +#define ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H + +/** + * @brief Throws an exception if a given value is out of a defined range. + * + * @tparam T1 + * @tparam T2 + * @param val The value to check if it is in the range. + * @param min The minimum value of the range. + * @param max The maximum value of the range. + * @param val_desc Description of the value for the exception message. + */ +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) + ")!"); +} + +/** + * @brief Throws an exception if the given value is not present. + * @param is_present Whether the value is present. + * @param val_desc Description of the value for the exception message. + */ +inline void throwIfNotPresent(const bool is_present, const std::string val_desc) { + if (!is_present) throw std::invalid_argument(val_desc + " is not present!"); +} + +#endif // ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h index 4bf45c8f..7a9ac992 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_access.h @@ -29,13 +29,13 @@ SOFTWARE. * @brief Main CPM access implementation header */ -#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_GETTERS_COMMON_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_SETTERS_COMMON_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_GETTERS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V1_3_1_SETTERS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_GETTERS_H #undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H #pragma once diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h index c5dd1475..da5eb1b4 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_setters.h @@ -31,6 +31,7 @@ SOFTWARE. #pragma once +#include #include 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 index ec5dedd0..d87f713e 100644 --- a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cpm/cpm_ts_utils.h @@ -29,7 +29,6 @@ SOFTWARE. * @brief Utility functions for the ETSI ITS CPM (TS) */ -#include #include #pragma once 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 630db783..3daeb66a 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,13 +29,15 @@ SOFTWARE. * @brief Main DENM access implementation header */ -#undef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_SETTERS_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 +#undef ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H #pragma once @@ -45,6 +47,7 @@ SOFTWARE. #include +#include #include #include 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 7a125580..8e54594e 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 @@ -33,6 +33,7 @@ SOFTWARE. namespace etsi_its_denm_msgs::access { +#include #include /** 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 d7686274..8169890c 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 @@ -35,6 +35,8 @@ SOFTWARE. namespace etsi_its_denm_msgs::access { +#include +#include #include /** 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 ea1640bd..01231042 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 @@ -29,7 +29,6 @@ SOFTWARE. * @brief Utility functions for the ETSI ITS DENM (EN) */ -#include #include #pragma once diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_access.h new file mode 100644 index 00000000..66714941 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_access.h @@ -0,0 +1,40 @@ +/* +============================================================================= +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/mapem/mapem_ts_access.h + * @brief Main MAPEM access implementation header + */ + +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_GETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_ASN1_PRIMITIVES_ASN1_PRIMITIVES_SETTERS_H +#undef ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H + +#pragma once + +#include +#include +#include diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_getters.h new file mode 100644 index 00000000..e2213480 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_getters.h @@ -0,0 +1,211 @@ +/* +============================================================================= +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/mapem/mapem_ts_getters.h + * @brief Getter functions for the ETSI ITS MAPEM + */ + +#pragma once + +namespace etsi_its_mapem_ts_msgs { + +namespace access { + +#include +#include + + /** + * @brief Get the value of MinuteOfTheYear object MapData object + * + * @param map object to get the MinuteOfTheYear from + * @return MinuteOfTheYear the minute of the year object + */ + inline MinuteOfTheYear getMinuteOfTheYear(const MapData& map) { + throwIfNotPresent(map.time_stamp_is_present, "map.time_stamp"); + return map.time_stamp; + } + + /** + * @brief Get the value of MinuteOfTheYear value from MapData object + * + * @param map object to get the MinuteOfTheYear value from + * @return uint32_t the minute of the year value + */ + inline uint32_t getMinuteOfTheYearValue(const MapData& map) { + MinuteOfTheYear moy = getMinuteOfTheYear(map); + return moy.value; + } + + /** + * @brief Get the value of MinuteOfTheYear object from mapem + * + * @param mapem object to get the MinuteOfTheYear + * @return MinuteOfTheYear the minute of the year object + */ + inline MinuteOfTheYear getMinuteOfTheYear(const MAPEM& mapem) { + return getMinuteOfTheYear(mapem.map); + } + + /** + * @brief Get the value of MinuteOfTheYear value from mapem + * + * @param mapem object to get the MinuteOfTheYear value from + * @return uint32_t the minute of the year value + */ + inline uint32_t getMinuteOfTheYearValue(const MAPEM& mapem) { + return getMinuteOfTheYearValue(mapem.map); + } + + /** + * @brief Get the IntersectionID value + * + * @param intsct_id IntersectionID object to get the value from + * @return uint16_t the IntersectionID value + */ + inline uint16_t getIntersectionID(const IntersectionID& intsct_id) { + return intsct_id.value; + } + + /** + * @brief Get the IntersectionId of an IntersectionGeometry object + * + * @param intsct IntersectionGeometry object + * @return uint16_t the IntersectionId value + */ + inline uint16_t getIntersectionID(const IntersectionGeometry& intsct) { + return getIntersectionID(intsct.id.id); + } + + /** + * @brief Get the Latitude value + * + * @param latitude object 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; + } + + /** + * @brief Get the Longitude value + * + * @param longitude object 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; + } + + /** + * @brief Get the Elevation value + * + * @param elevation object to get the Elevation value from + * @return Elevation value (above the reference ellipsoid surface) in meter as decimal number + */ + inline double getElevation(const Elevation& elevation) { + return ((double)elevation.value)*1e-1; + } + + /** + * @brief Get the Latitude value from a given Position3D object + * + * @param ref_point Position3D object to get the Latitude value from + * @return Latitude value in degree as decimal number + */ + inline double getLatitude(const Position3D& ref_point) { + return getLatitude(ref_point.lat); + } + + /** + * @brief Get the Longitude value from a given Position3D object + * + * @param ref_point Position3D object to get the Longitude value from + * @return Longitude value in degree as decimal number + */ + inline double getLongitude(const Position3D& ref_point) { + return getLongitude(ref_point.lon); + } + + /** + * @brief Get the Elevation value from a given Position3D object + * + * @param ref_point Position3D object to get the Elevation value from + * @return Elevation value (above the reference ellipsoid surface) in meter as decimal number + */ + inline double getElevation(const Position3D& ref_point) { + throwIfNotPresent(ref_point.elevation_is_present, "Position3D.elevation_is_present"); + return getElevation(ref_point.elevation); + } + + /** + * @brief Get the LaneDirection in form of bool vector + * + * @param lane_direction LaneDirection object to get the bool vector from + * @return std::vector + */ + inline std::vector getLaneDirection(const LaneDirection& lane_direction) { + return getBitString(lane_direction.value, lane_direction.bits_unused); + } + + /** + * @brief Get the LaneDirection in form of bool vector from a LaneAttributes object + * + * @param lane_attributes LaneAttributes object to get the bool vector from + * @return std::vector + */ + inline std::vector getLaneDirection(const LaneAttributes& lane_attributes) { + return getLaneDirection(lane_attributes.directional_use); + } + + /** + * @brief Get the LaneDirection in form of bool vector from a GenericLane object + * + * @param generic_lane GenericLane object to get the bool vector from + * @return std::vector + */ + inline std::vector getLaneDirection(const GenericLane& generic_lane) { + return getLaneDirection(generic_lane.lane_attributes); + } + + /** + * @brief Get the Point From NodeXY object + * + * @tparam T representing different NodeXY types (NodeXY20b, NodeXY22b...) + * @param node_xy the NodeXY object + * @return gm::Point geometry_msgs::Point representing the node point (values given in meters) + */ + template + inline gm::Point getPointFromNodeXY(const T& node_xy) { + gm::Point p; + p.x = ((double)node_xy.x.value) * 1e-2; + p.y = ((double)node_xy.y.value) * 1e-2; + return p; + } + +} // namespace access + +} // namespace etsi_its_mapem_ts_msgs diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_setters.h new file mode 100644 index 00000000..69acbf7e --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_setters.h @@ -0,0 +1,200 @@ +/* +============================================================================= +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/mapem/mapem_ts_setters.h + * @brief Setter functions for the ETSI ITS MAPEM + */ + +#pragma once + + +#include + +namespace etsi_its_mapem_ts_msgs { + +namespace access { + +#include + + /** + * @brief Set the MinuteOfTheYear object + * + * @param moy MinuteOfTheYear object + * @param moy_value value to set + */ + inline void setMinuteOfTheYear(MinuteOfTheYear& moy, const uint32_t moy_value) { + throwIfOutOfRange(moy_value, MinuteOfTheYear::MIN, MinuteOfTheYear::MAX, "MinuteOfTheYear"); + moy.value = moy_value; + } + + /** + * @brief Set the MinuteOfTheYear for a given MapData object + * + * @param map MapData object + * @param moy_value value to set + */ + inline void setMinuteOfTheYear(MapData& map, const uint32_t moy_value) { + setMinuteOfTheYear(map.time_stamp, moy_value); + } + + /** + * @brief Set the Minute Of The Year object + * + * @param mapem + */ + inline void setMinuteOfTheYear(MAPEM& mapem, const uint32_t moy_value) { + setMinuteOfTheYear(mapem.map, moy_value); + mapem.map.time_stamp_is_present = true; + } + + /** + * @brief Set the IntersectionID value + * + * @param intsct_id IntersectionID object + * @param id_value value to set + */ + inline void setIntersectionID(IntersectionID& intsct_id, const uint16_t id_value) { + throwIfOutOfRange(id_value, IntersectionID::MIN, IntersectionID::MAX, "IntersectionID"); + intsct_id.value = id_value; + } + + /** + * @brief Set the IntersectionID for an IntersectionGeometry object + * + * @param intsct IntersectionGeometry object + * @param id_value value to set + */ + inline void setIntersectionID(IntersectionGeometry& intsct, const uint16_t id_value) { + setIntersectionID(intsct.id.id, id_value); + } + + /** + * @brief Set the Latitude object + * + * @param latitude object to set + * @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); + throwIfOutOfRange(angle_in_10_micro_degree, Latitude::MIN, Latitude::MAX, "Latitude"); + latitude.value = angle_in_10_micro_degree; + } + + /** + * @brief Set the Longitude object + * + * @param longitude object to set + * @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); + throwIfOutOfRange(angle_in_10_micro_degree, Longitude::MIN, Longitude::MAX, "Longitude"); + longitude.value = angle_in_10_micro_degree; + } + + /** + * @brief Set the Elevation object + * + * @param elevation object to set + * @param value Elevation value (above the reference ellipsoid surface) in meter as decimal number + */ + inline void setElevation(Elevation& elevation, const double value) { + int64_t alt_in_dm = (int64_t)std::round(value*1e1); + if(alt_in_dm>=Elevation::MIN && alt_in_dm<=Elevation::MAX) elevation.value = alt_in_dm; + else if(alt_in_dmElevation::MAX) elevation.value = Elevation::MAX; + } + + /** + * @brief Set the Position3D object + * + * @param pos object to set + * @param latitude Latitude value in degree as decimal number + * @param longitude Longitude value in degree as decimal number + */ + inline void setPosition3D(Position3D& pos, const double latitude, const double longitude) { + setLatitude(pos.lat, latitude); + setLongitude(pos.lon, longitude); + pos.elevation_is_present = false; + } + + /** + * @brief Set the Position3D object + * + * @param pos 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 setPosition3D(Position3D& pos, const double latitude, const double longitude, const double altitude) { + setPosition3D(pos, latitude, longitude); + setElevation(pos.elevation, altitude); + pos.elevation_is_present = true; + } + + /** + * @brief Set the Position3D of IntersectionGeometry object + * + * @param intsct IntersectionGeometry object + * @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 setPosition3D(IntersectionGeometry& intsct, double latitude, double longitude, double altitude) { + setPosition3D(intsct.ref_point, latitude, longitude, altitude); + } + + /** + * @brief Set the Position3D 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 Position3D 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 setPosition3DFromUTMPosition(Position3D& 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()); + } + setPosition3D(reference_position, latitude, longitude, utm_position.point.z); + } + +} // namespace access + +} // namespace etsi_its_mapem_ts_msgs diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_utils.h new file mode 100644 index 00000000..8d5c7b05 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/mapem/mapem_ts_utils.h @@ -0,0 +1,188 @@ +/* +============================================================================= +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/mapem/mapem_ts_utils.h + * @brief Utility functions for the ETSI ITS MAPEM + */ + +#include +#include + +#pragma once + +namespace etsi_its_mapem_ts_msgs { + +namespace access { + + /** + * @brief Get the unix seconds of the beginning of a year that corresponds to a given unix timestamp + * + * @param unixSecond timestamp that defines the year for that the unix seconds for the beginning of the year should be derived + * @return uint64_t unix seconds of the beginning of the year + */ + inline uint64_t getUnixSecondsOfYear(const uint64_t unixSecond) { + + // Get current time as a time_point + time_t ts = static_cast(unixSecond); // Convert uint64_t to time_t + + struct tm* timeinfo; + timeinfo = gmtime(&ts); + + // Set the timeinfo to the beginning of the year + timeinfo->tm_sec = 0; + timeinfo->tm_min = 0; + timeinfo->tm_hour = 0; + timeinfo->tm_mday = 1; + timeinfo->tm_mon = 0; + + return timegm(timeinfo); // Convert struct tm back to Unix timestamp + } + + /** + * @brief Get the unix nanoseconds from MinuteOfTheYear object + * + * @param moy given MinuteOfTheYear object + * @param unix_nanoseconds_estimate unix timestamp to derive the current year from in nanoseconds + * @return uint64_t unix timestamp according to the given MinuteOfTheYear in nanoseconds + */ + inline uint64_t getUnixNanosecondsFromMinuteOfTheYear(const MinuteOfTheYear& moy, const uint64_t unix_nanoseconds_estimate) { + return ((uint64_t)(moy.value*60) + getUnixSecondsOfYear(unix_nanoseconds_estimate*1e-9))*1e9; + } + + /** + * @brief Get the unix nanoseconds from MapData object + * + * @param map given MapData object + * @param unix_nanoseconds_estimate unix timestamp to derive the current year from in nanoseconds + * @return uint64_t unix timestamp according to the given MinuteOfTheYear in nanoseconds + */ + inline uint64_t getUnixNanosecondsFromMapData(const MapData& map, const uint64_t unix_nanoseconds_estimate) { + return getUnixNanosecondsFromMinuteOfTheYear(getMinuteOfTheYear(map), unix_nanoseconds_estimate); + } + + /** + * @brief Get the unix nanoseconds from MinuteOfTheYear object + * + * @param mapem given MAPEM object + * @param unix_nanoseconds_estimate unix timestamp to derive the current year from in nanoseconds + * @return uint64_t unix timestamp according to the stored MinuteOfTheYear in nanoseconds + */ + inline uint64_t getUnixNanoseconds(const MAPEM& mapem, const uint64_t unix_timestamp_estimate) { + return getUnixNanosecondsFromMapData(mapem.map, unix_timestamp_estimate); + } + + /** + * @brief Get the UTM Position defined by the given Position3D + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param[in] reference_position Position3D 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 Position3D& reference_position, int& zone, bool& northp) { + gm::PointStamped utm_point; + double latitude = getLatitude(reference_position.lat); + double longitude = getLongitude(reference_position.lon); + if(reference_position.elevation_is_present) utm_point.point.z = getElevation(reference_position.elevation); + 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 UTM Position defined by the given Position3D along with the grid-convergence angle + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param[in] reference_position Position3D 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) + * @param[out] conv_angle grid-convergence angle in degree + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ + inline gm::PointStamped getUTMPositionWithConvergenceAngle(const Position3D& reference_position, int& zone, bool& northp, double& conv_angle) { + gm::PointStamped utm_point; + double latitude = getLatitude(reference_position.lat); + double longitude = getLongitude(reference_position.lon); + if(reference_position.elevation_is_present) utm_point.point.z = getElevation(reference_position.elevation); + try { + double scale; + GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y, conv_angle, scale); + 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 UTM Position of ref_point defined by the Position3D along with the grid-convergence angle in an IntersectionGeometry object + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param[in] intsctn IntersectionGeometry 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 getRefPointUTMPosition(const IntersectionGeometry& intsctn, int& zone, bool& northp) { + return getUTMPosition(intsctn.ref_point, zone, northp); + } + + /** + * @brief Get the UTM Position of ref_point defined by the Position3D in an IntersectionGeometry object + * + * The position is transformed into UTM by using GeographicLib::UTMUPS + * The altitude value is directly used as z-Coordinate + * + * @param[in] intsctn IntersectionGeometry 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) + * @param[out] conv_angle grid-convergence angle in degree + * @return gm::PointStamped geometry_msgs::PointStamped of the given position + */ + inline gm::PointStamped getRefPointUTMPositionWithConvergenceAngle(const IntersectionGeometry& intsctn, int& zone, bool& northp, double& conv_angle) { + return getUTMPositionWithConvergenceAngle(intsctn.ref_point, zone, northp, conv_angle); + } + +} // namespace etsi_its_mapem_ts_msgs +} // namespace access diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_access.h new file mode 100644 index 00000000..6f0e9c03 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_access.h @@ -0,0 +1,38 @@ +/* +============================================================================= +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/spatem/spatem_ts_access.h + * @brief Main SPATEM access implementation header + */ + +#undef ETSI_ITS_MSGS_UTILS_IMPL_CHECKS_H + +#pragma once + +#include +#include +#include diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_getters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_getters.h new file mode 100644 index 00000000..f8fe8377 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_getters.h @@ -0,0 +1,167 @@ +/* +============================================================================= +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/spatem/spatem_ts_getters.h + * @brief Getter functions for the ETSI ITS SPATEM + */ + +#pragma once + +namespace etsi_its_spatem_ts_msgs { + +namespace access { + +#include + + /** + * @brief Get the intersection-id + * + * @param intsct_id intersection-id object to get the value from + * @return uint16_t id of the intersection + */ + inline uint16_t getIntersectionID(const IntersectionID& intsct_id) { + return intsct_id.value; + } + + /** + * @brief Get the intersection-id of an IntersectionReferenceID object + * + * @param intsct_ref_id IntersectionReferenceID object + * @return uint16_t id of the intersection + */ + inline uint16_t getIntersectionID(const IntersectionReferenceID& intsct_ref_id) { + return getIntersectionID(intsct_ref_id.id); + } + + /** + * @brief Get the intersection-id of an IntersectionState object + * + * @param intsct IntersectionState object + * @return uint16_t id of the intersection + */ + inline uint16_t getIntersectionID(const IntersectionState& intsct) { + return getIntersectionID(intsct.id); + } + + /** + * @brief Get the MinuteOfTheYear object from a given IntersectionState object + * + * @param intsct IntersectionState object to get the MinuteOfTheYear from + * @return MinuteOfTheYear object + */ + inline MinuteOfTheYear getMinuteOfTheYear(const IntersectionState& intsct) { + throwIfNotPresent(intsct.moy_is_present, "intsct.moy"); + return intsct.moy; + } + + /** + * @brief Get the DSecond object from a given IntersectionState object + * + * @param intsct IntersectionState object to get the DSecond from + * @return DSecond object + */ + inline DSecond getDSecond(const IntersectionState& intsct) { + throwIfNotPresent(intsct.time_stamp_is_present, "intsct.time_stamp"); + return intsct.time_stamp; + } + + /** + * @brief Get the value of a DSecond object in seconds + * + * @param dsecond DSecond object to get the value from + * @return double value of DSecond given in seconds + */ + inline double getDSecondValue(const DSecond& dsecond) { + return ((double)dsecond.value)*1e-3; + } + + /** + * @brief Get the value of an DSecond object from a given IntersectionState object + * + * @param intsct + * @return double + */ + inline double getDSecondValue(const IntersectionState& intsct) { + return getDSecondValue(getDSecond(intsct)); + } + + /** + * @brief Get the Signal Group-ID of an SignalGroupID object + * + * @param signal_group_id SignalGroupID object to get the id from + * @return uint8_t the id of the signal group + */ + inline uint8_t getSignalGroupID(const SignalGroupID& signal_group_id) { + return signal_group_id.value; + } + + /** + * @brief Get the Signal Group-ID of an MovementState object + * + * @param mvmt_state MovementState object to get the id from + * @return uint8_t the id of the signal group + */ + inline uint8_t getSignalGroupID(const MovementState& mvmt_state) { + return getSignalGroupID(mvmt_state.signal_group); + } + + /** + * @brief Get the current MovementEvent of a given MovementState object + * + * @param mvmt_event MovementState object to get the MovementEvent from + * @return MovementEvent object + */ + inline MovementEvent getCurrentMovementEvent(const MovementState& mvmt_state) { + if(mvmt_state.state_time_speed.array.size()<=0) { + throw std::runtime_error("MovementEventList is empty."); + } + return mvmt_state.state_time_speed.array[0]; + } + + /** + * @brief Get the Current MovementPhaseState object of a given MovementState object + * + * @param mvmt_state MovementState object to get the MovementPhaseState from + * @return MovementPhaseState object + */ + inline MovementPhaseState getCurrentMovementPhaseState(const MovementState& mvmt_state) { + return getCurrentMovementEvent(mvmt_state).event_state; + } + + /** + * @brief Get the Current MovementPhaseState object of a given MovementState object + * + * @param mvmt_state MovementState object to get the MovementPhaseState from + * @return MovementPhaseState object + */ + inline uint8_t getCurrentMovementPhaseStateValue(const MovementState& mvmt_state) { + return getCurrentMovementPhaseState(mvmt_state).value; + } + +} // namespace access + +} // namespace etsi_its_spatem_ts_msgs diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_setters.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_setters.h new file mode 100644 index 00000000..a1bab8fd --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_setters.h @@ -0,0 +1,172 @@ +/* +============================================================================= +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/spatem/spatem_ts_setters.h + * @brief Setter functions for the ETSI ITS SPATEM + */ + +#pragma once + + +namespace etsi_its_spatem_ts_msgs { + +namespace access { + +#include + + /** + * @brief Sets the IntersectionID value. + * + * @param intsct_id The IntersectionID object to set. + * @param id The value to set. + * @throws std::out_of_range if the id is out of the valid range. + */ + inline void setIntersectionID(IntersectionID& intsct_id, const uint16_t id) { + throwIfOutOfRange(id, IntersectionID::MIN, IntersectionID::MAX, "IntersectionID"); + intsct_id.value = id; + } + + /** + * @brief Sets the IntersectionID value in an IntersectionReferenceID object. + * + * @param intsct_ref_id The IntersectionReferenceID object to set. + * @param id The value to set. + * @throws std::out_of_range if the id is out of the valid range. + */ + inline void setIntersectionID(IntersectionReferenceID& intsct_ref_id, const uint16_t id) { + setIntersectionID(intsct_ref_id.id, id); + } + + /** + * @brief Sets the IntersectionID value in an IntersectionState object. + * + * @param intsct The IntersectionState object to set. + * @param id The value to set. + * @throws std::out_of_range if the id is out of the valid range. + */ + inline void setIntersectionID(IntersectionState& intsct, const uint16_t id) { + setIntersectionID(intsct.id, id); + } + + /** + * @brief Sets the MinuteOfTheYear value. + * + * @param moy The MinuteOfTheYear object to set. + * @param moy_value The value to set. + * @throws std::out_of_range if the moy_value is out of the valid range. + */ + inline void setMinuteOfTheYear(MinuteOfTheYear& moy, const uint32_t moy_value) { + throwIfOutOfRange(moy_value, MinuteOfTheYear::MIN, MinuteOfTheYear::MAX, "MinuteOfTheYear"); + moy.value = moy_value; + } + + /** + * @brief Sets the MinuteOfTheYear value in an IntersectionState object. + * + * @param intsct The IntersectionState object to set. + * @param moy_value The value to set. + * @throws std::out_of_range if the moy_value is out of the valid range. + */ + inline void setMinuteOfTheYear(IntersectionState& intsct, const uint32_t moy_value) { + setMinuteOfTheYear(intsct.moy, moy_value); + intsct.moy_is_present = true; + } + + /** + * @brief Sets the DSecond value. + * + * @param dsecond The DSecond object to set. + * @param dsecond_value The value to set. + * @throws std::out_of_range if the dsecond_value is out of the valid range. + */ + inline void setDSecond(DSecond& dsecond, const uint32_t dsecond_value) { + throwIfOutOfRange(dsecond_value, DSecond::MIN, DSecond::MAX, "DSecond"); + dsecond.value = dsecond_value; + } + + /** + * @brief Sets the DSecond value using a double. + * + * @param dsecond The DSecond object to set. + * @param dsecond_value The value to set in seconds. + * @throws std::out_of_range if the dsecond_value is out of the valid range. + */ + inline void setDSecond(DSecond& dsecond, const double dsecond_value) { + uint32_t dsecond_value_ms = (uint32_t)(dsecond_value*1e3); + setDSecond(dsecond, dsecond_value_ms); + } + + /** + * @brief Sets the DSecond value in an IntersectionState object. + * + * @param intsct The IntersectionState object to set. + * @param dsecond_value The value to set. + * @throws std::out_of_range if the dsecond_value is out of the valid range. + */ + inline void setDSecond(IntersectionState& intsct, const uint32_t dsecond_value) { + setDSecond(intsct.time_stamp, dsecond_value); + intsct.time_stamp_is_present = true; + } + + /** + * @brief Sets the DSecond value in an IntersectionState object using a double. + * + * @param intsct The IntersectionState object to set. + * @param dsecond_value The value to set in seconds. + * @throws std::out_of_range if the dsecond_value is out of the valid range. + */ + inline void setDSecond(IntersectionState& intsct, const double dsecond_value) { + setDSecond(intsct.time_stamp, dsecond_value); + intsct.time_stamp_is_present = true; + } + + /** + * @brief Sets the SignalGroupID value. + * + * @param signal_group_id The SignalGroupID object to set. + * @param id The value to set. + * @throws std::out_of_range if the id is out of the valid range. + */ + inline void setSignalGroupID(SignalGroupID& signal_group_id, const uint8_t id) { + throwIfOutOfRange(id, SignalGroupID::MIN, SignalGroupID::MAX, "SignalGroupID"); + signal_group_id.value = id; + } + + /** + * @brief Sets the SignalGroupID value in a MovementState object. + * + * @param movement_state The MovementState object to set. + * @param id The value to set. + * @throws std::out_of_range if the id is out of the valid range. + */ + inline void setSignalGroupID(MovementState& movement_state, const uint8_t id) { + setSignalGroupID(movement_state.signal_group, id); + } + +} // namespace access + +} // namespace etsi_its_spatem_ts_msgs diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_utils.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_utils.h new file mode 100644 index 00000000..0fde6948 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/spatem/spatem_ts_utils.h @@ -0,0 +1,76 @@ +/* +============================================================================= +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/spatem/spatem_ts_utils.h + * @brief Utility functions for the ETSI ITS SPATEM + */ + +#include + +#pragma once + +namespace etsi_its_spatem_ts_msgs { + +namespace access { + + /** + * @brief Get the unix seconds of the beginning of a year that corresponds to a given unix timestamp + * + * @param unixSecond timestamp that defines the year for that the unix seconds for the beginning of the year should be derived + * @return uint64_t unix seconds of the beginning of the year + */ + inline uint64_t getUnixSecondsOfYear(const uint64_t unixSecond) { + + // Get current time as a time_point + time_t ts = static_cast(unixSecond); // Convert uint64_t to time_t + + struct tm* timeinfo; + timeinfo = gmtime(&ts); + + // Set the timeinfo to the beginning of the year + timeinfo->tm_sec = 0; + timeinfo->tm_min = 0; + timeinfo->tm_hour = 0; + timeinfo->tm_mday = 1; + timeinfo->tm_mon = 0; + + return timegm(timeinfo); // Convert struct tm back to Unix timestamp + } + + /** + * @brief Get the unix nanoseconds from MinuteOfTheYear object + * + * @param moy given MinuteOfTheYear object + * @param unix_nanoseconds_estimate unix timestamp to derive the current year from in nanoseconds + * @return uint64_t unix timestamp according to the given MinuteOfTheYear in nanoseconds + */ + inline uint64_t getUnixNanosecondsFromMinuteOfTheYear(const MinuteOfTheYear& moy, const uint64_t unix_nanoseconds_estimate) { + return ((uint64_t)(moy.value*60) + getUnixSecondsOfYear(unix_nanoseconds_estimate*1e-9))*1e9; + } + +} // namespace etsi_its_spatem_ts_msgs +} // namespace access 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/mapem_ts_access.h similarity index 68% rename from etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_checks.h rename to etsi_its_msgs_utils/include/etsi_its_msgs_utils/mapem_ts_access.h index b643b8f0..3b82b6aa 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/mapem_ts_access.h @@ -25,18 +25,19 @@ SOFTWARE. */ /** - * @file impl/cdd/cdd_checks.h - * @brief Sanity-check functions etc. for the ETSI ITS Common Data Dictionary (CDD) + * @file mapem_ts_access.h + * @brief Main MAPEM access header to include in ROS 1 projects */ -#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H -#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H +#pragma once -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) + ")!"); +// Messages +#include +#include + +namespace etsi_its_mapem_ts_msgs { + namespace gm = geometry_msgs; } -#endif // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_CHECKS_H \ No newline at end of file +// Implementation +#include \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/mapem_ts_access.hpp b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/mapem_ts_access.hpp new file mode 100644 index 00000000..02fa9080 --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/mapem_ts_access.hpp @@ -0,0 +1,44 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================= +*/ + +/** + * @file mapem_ts_access.hpp + * @brief Main MAPEM access header to include in ROS 2 projects + */ + +#pragma once + +// Messages +#include +#include + +namespace etsi_its_mapem_ts_msgs { + using namespace msg; + namespace gm = geometry_msgs::msg; +} + +// Implementation +#include \ No newline at end of file diff --git a/etsi_its_msgs_utils/include/etsi_its_msgs_utils/spatem_ts_access.h b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/spatem_ts_access.h new file mode 100644 index 00000000..58cf8c8a --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/spatem_ts_access.h @@ -0,0 +1,43 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================= +*/ + +/** + * @file spatem_ts_access.h + * @brief Main SPATEM access header to include in ROS 1 projects + */ + +#pragma once + +// Messages +#include +#include + +namespace etsi_its_spatem_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/spatem_ts_access.hpp b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/spatem_ts_access.hpp new file mode 100644 index 00000000..27cbe0ac --- /dev/null +++ b/etsi_its_msgs_utils/include/etsi_its_msgs_utils/spatem_ts_access.hpp @@ -0,0 +1,44 @@ +/* +============================================================================= +MIT License + +Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +============================================================================= +*/ + +/** + * @file spatem_ts_access.hpp + * @brief Main SPATEM access header to include in ROS 2 projects + */ + +#pragma once + +// Messages +#include +#include + +namespace etsi_its_spatem_ts_msgs { + using namespace msg; + namespace gm = geometry_msgs::msg; +} + +// Implementation +#include \ No newline at end of file diff --git a/etsi_its_msgs_utils/test/impl/test_mapem_ts_access.cpp b/etsi_its_msgs_utils/test/impl/test_mapem_ts_access.cpp new file mode 100644 index 00000000..712c07d6 --- /dev/null +++ b/etsi_its_msgs_utils/test/impl/test_mapem_ts_access.cpp @@ -0,0 +1,53 @@ +#include +#include + +namespace mapem_ts_access = etsi_its_mapem_ts_msgs::access; + +TEST(etsi_its_mapem_ts_msgs, test_set_get_mapem) { + mapem_ts_msgs::IntersectionGeometry intsct; + + unsigned int intersection_id = randomInt(mapem_ts_msgs::IntersectionID::MIN, mapem_ts_msgs::IntersectionID::MAX); + mapem_ts_access::setIntersectionID(intsct, intersection_id); + EXPECT_EQ(intersection_id, mapem_ts_access::getIntersectionID(intsct)); + + // Set specific position to test utm projection + double latitude = 50.787467; + double longitude = 6.046498; + double altitude = 209.0; + mapem_ts_access::setPosition3D(intsct, latitude, longitude, altitude); + int zone; + bool northp; + gm::PointStamped utm = mapem_ts_access::getRefPointUTMPosition(intsct, 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); + mapem_ts_access::setPosition3DFromUTMPosition(intsct.ref_point, utm, zone, northp); + EXPECT_NEAR(latitude, mapem_ts_access::getLatitude(intsct.ref_point), 1e-7); + EXPECT_NEAR(longitude, mapem_ts_access::getLongitude(intsct.ref_point), 1e-7); + EXPECT_NEAR(altitude, mapem_ts_access::getElevation(intsct.ref_point), 1e-2); + + mapem_ts_msgs::MAPEM mapem; + + unsigned int moy = randomInt(mapem_ts_msgs::MinuteOfTheYear::MIN, mapem_ts_msgs::MinuteOfTheYear::MAX); + mapem_ts_access::setMinuteOfTheYear(mapem, moy); + EXPECT_EQ(moy, mapem_ts_access::getMinuteOfTheYearValue(mapem)); + // Generate dummy time: 04.01.2007 1:15 + struct tm timeinfo; + timeinfo.tm_sec = 0; + timeinfo.tm_min = 15; + timeinfo.tm_hour = 1; + timeinfo.tm_mday = 4; + timeinfo.tm_mon = 0; + timeinfo.tm_year = 107; //years since 1900 + uint64_t unix_stamp = timegm(&timeinfo); + // Set time to beginning of 2007: 01.01.2007 0:00 + timeinfo.tm_sec = 0; + timeinfo.tm_min = 0; + timeinfo.tm_hour = 0; + timeinfo.tm_mday = 1; + timeinfo.tm_mon = 0; + timeinfo.tm_year = 107; //years since 1900 + EXPECT_EQ((timegm(&timeinfo)+60*moy)*1e9, mapem_ts_access::getUnixNanoseconds(mapem, unix_stamp*1e9)); +} \ No newline at end of file diff --git a/etsi_its_msgs_utils/test/impl/test_spatem_ts_access.cpp b/etsi_its_msgs_utils/test/impl/test_spatem_ts_access.cpp new file mode 100644 index 00000000..2ecdf478 --- /dev/null +++ b/etsi_its_msgs_utils/test/impl/test_spatem_ts_access.cpp @@ -0,0 +1,53 @@ +#include +#include + +namespace spatem_ts_access = etsi_its_spatem_ts_msgs::access; + +TEST(etsi_its_spatem_ts_msgs, test_set_get_spatem) { + + spatem_ts_msgs::IntersectionState intsct; + unsigned int id = randomInt(spatem_ts_msgs::IntersectionID::MIN, spatem_ts_msgs::IntersectionID::MAX); + spatem_ts_access::setIntersectionID(intsct, id); + EXPECT_EQ(id, spatem_ts_access::getIntersectionID(intsct)); + + unsigned int moy = randomInt(spatem_ts_msgs::MinuteOfTheYear::MIN, spatem_ts_msgs::MinuteOfTheYear::MAX); + spatem_ts_access::setMinuteOfTheYear(intsct, moy); + EXPECT_EQ(moy, spatem_ts_access::getMinuteOfTheYear(intsct).value); + EXPECT_EQ(true, intsct.moy_is_present); + // Generate dummy time: 04.01.2007 1:15 + struct tm timeinfo; + timeinfo.tm_sec = 0; + timeinfo.tm_min = 15; + timeinfo.tm_hour = 1; + timeinfo.tm_mday = 4; + timeinfo.tm_mon = 0; + timeinfo.tm_year = 107; //years since 1900 + uint64_t unix_stamp = timegm(&timeinfo); + // Set time to beginning of 2007: 01.01.2007 0:00 + timeinfo.tm_sec = 0; + timeinfo.tm_min = 0; + timeinfo.tm_hour = 0; + timeinfo.tm_mday = 1; + timeinfo.tm_mon = 0; + timeinfo.tm_year = 107; //years since 1900 + EXPECT_EQ((timegm(&timeinfo)+60*moy)*1e9, spatem_ts_access::getUnixNanosecondsFromMinuteOfTheYear(spatem_ts_access::getMinuteOfTheYear(intsct), unix_stamp*1e9)); + + unsigned int dsecond = randomInt(spatem_ts_msgs::DSecond::MIN, spatem_ts_msgs::DSecond::MAX); + spatem_ts_access::setDSecond(intsct, dsecond); + EXPECT_EQ(dsecond, spatem_ts_access::getDSecond(intsct).value); + EXPECT_EQ(true, intsct.time_stamp_is_present); + + double dsecond_double = randomDouble(((double)spatem_ts_msgs::DSecond::MIN)*1e-3, ((double)spatem_ts_msgs::DSecond::MAX)*1e-3); + spatem_ts_access::setDSecond(intsct, dsecond_double); + EXPECT_NEAR(dsecond_double, spatem_ts_access::getDSecondValue(intsct), 1e-3); + EXPECT_EQ(true, intsct.time_stamp_is_present); + + spatem_ts_msgs::MovementState movement_state; + unsigned int signal_group_id = randomInt(spatem_ts_msgs::SignalGroupID::MIN, spatem_ts_msgs::SignalGroupID::MAX); + spatem_ts_access::setSignalGroupID(movement_state, signal_group_id); + EXPECT_EQ(signal_group_id, spatem_ts_access::getSignalGroupID(movement_state)); + movement_state.state_time_speed.array.resize(1); + unsigned int event_state = randomInt(spatem_ts_msgs::MovementPhaseState::UNAVAILABLE, spatem_ts_msgs::MovementPhaseState::CAUTION_CONFLICTING_TRAFFIC); + movement_state.state_time_speed.array[0].event_state.value = event_state; + EXPECT_EQ(event_state, spatem_ts_access::getCurrentMovementPhaseStateValue(movement_state)); +} \ 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 index 730e16f5..c1c3c648 100644 --- a/etsi_its_msgs_utils/test/test_access.cpp +++ b/etsi_its_msgs_utils/test/test_access.cpp @@ -14,6 +14,12 @@ #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); @@ -38,6 +44,12 @@ namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs; namespace denm_msgs = etsi_its_denm_msgs; #include +namespace mapem_ts_msgs = etsi_its_mapem_ts_msgs; +#include + +namespace spatem_ts_msgs = etsi_its_spatem_ts_msgs; +#include + int main(int argc, char *argv[]) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/etsi_its_msgs_utils/test/test_access.ros2.cpp b/etsi_its_msgs_utils/test/test_access.ros2.cpp index adbb9046..0acfd230 100644 --- a/etsi_its_msgs_utils/test/test_access.ros2.cpp +++ b/etsi_its_msgs_utils/test/test_access.ros2.cpp @@ -14,6 +14,12 @@ #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); @@ -38,6 +44,12 @@ namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs::msg; namespace denm_msgs = etsi_its_denm_msgs::msg; #include +namespace mapem_ts_msgs = etsi_its_mapem_ts_msgs::msg; +#include + +namespace spatem_ts_msgs = etsi_its_spatem_ts_msgs::msg; +#include + int main(int argc, char *argv[]) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/etsi_its_rviz_plugins/CMakeLists.txt b/etsi_its_rviz_plugins/CMakeLists.txt index 4f345b8b..cec3b778 100644 --- a/etsi_its_rviz_plugins/CMakeLists.txt +++ b/etsi_its_rviz_plugins/CMakeLists.txt @@ -12,7 +12,11 @@ if(${ROS_VERSION} EQUAL 2) endif() find_package(ament_cmake REQUIRED) - find_package(etsi_its_msgs REQUIRED) + find_package(etsi_its_cam_msgs REQUIRED) + find_package(etsi_its_cpm_ts_msgs REQUIRED) + find_package(etsi_its_denm_msgs REQUIRED) + find_package(etsi_its_mapem_ts_msgs REQUIRED) + find_package(etsi_its_spatem_ts_msgs REQUIRED) find_package(etsi_its_msgs_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -28,6 +32,7 @@ if(${ROS_VERSION} EQUAL 2) include/displays/CAM/cam_display.hpp include/displays/DENM/denm_display.hpp include/displays/CPM/cpm_display.hpp + include/displays/MAPEM/mapem_display.hpp ) foreach(header "${display_headers_to_moc}") @@ -41,6 +46,8 @@ if(${ROS_VERSION} EQUAL 2) src/displays/DENM/denm_render_object.cpp src/displays/CPM/cpm_display.cpp src/displays/CPM/cpm_render_object.cpp + src/displays/MAPEM/mapem_display.cpp + src/displays/MAPEM/intersection_render_object.cpp ) add_library(${PROJECT_NAME} SHARED @@ -64,7 +71,11 @@ if(${ROS_VERSION} EQUAL 2) ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp - etsi_its_msgs + etsi_its_cam_msgs + etsi_its_cpm_ts_msgs + etsi_its_denm_msgs + etsi_its_mapem_ts_msgs + etsi_its_spatem_ts_msgs etsi_its_msgs_utils rviz_common rviz_default_plugins @@ -77,7 +88,11 @@ if(${ROS_VERSION} EQUAL 2) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( - etsi_its_msgs + etsi_its_cam_msgs + etsi_its_cpm_ts_msgs + etsi_its_denm_msgs + etsi_its_mapem_ts_msgs + etsi_its_spatem_ts_msgs etsi_its_msgs_utils rclcpp rviz_common diff --git a/etsi_its_rviz_plugins/config/demo.rviz b/etsi_its_rviz_plugins/config/demo.rviz index 110e79e2..6e428e2d 100644 --- a/etsi_its_rviz_plugins/config/demo.rviz +++ b/etsi_its_rviz_plugins/config/demo.rviz @@ -105,6 +105,34 @@ Visualization Manager: Reliability Policy: Reliable Value: /etsi_its_conversion/cpm_ts/out Value: true + - Class: etsi_its_msgs/MAPEMDisplay + Egress Lane Color: 255; 170; 0 + Enabled: true + Ingress Lane Color: 85; 85; 255 + MAPEM Lane Width: 1 + MAPEM Timeout: 120 + Metadata: + Scale: 4 + Text Color: 255; 255; 255 + Value: true + Name: MAPEMDisplay + SPATEM Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /etsi_its_conversion/spatem_ts/out + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /etsi_its_conversion/mapem_ts/out + Value: true + Visualize SPATEMs: + SPATEM Sphere Scale: 3 + SPATEM Timeout: 1 + Value: true Enabled: true Name: Incoming - Class: rviz_common/Group @@ -166,6 +194,34 @@ Visualization Manager: Reliability Policy: Reliable Value: /etsi_its_conversion/cpm_ts/in Value: true + - Class: etsi_its_msgs/MAPEMDisplay + Egress Lane Color: 255; 170; 0 + Enabled: true + Ingress Lane Color: 85; 85; 255 + MAPEM Lane Width: 1 + MAPEM Timeout: 120 + Metadata: + Scale: 4 + Text Color: 255; 255; 255 + Value: true + Name: MAPEMDisplay + SPATEM Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /etsi_its_conversion/spatem_ts/in + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /etsi_its_conversion/mapem_ts/in + Value: true + Visualize SPATEMs: + SPATEM Sphere Scale: 1 + SPATEM Timeout: 1 + Value: true Enabled: true Name: Outgoing Enabled: true diff --git a/etsi_its_rviz_plugins/include/displays/MAPEM/intersection_render_object.hpp b/etsi_its_rviz_plugins/include/displays/MAPEM/intersection_render_object.hpp new file mode 100644 index 00000000..627fa309 --- /dev/null +++ b/etsi_its_rviz_plugins/include/displays/MAPEM/intersection_render_object.hpp @@ -0,0 +1,131 @@ +/** ============================================================================ +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_mapem_ts_msgs/msg/mapem.hpp" +#include "etsi_its_spatem_ts_msgs/msg/spatem.hpp" +#include +#include + +#include + +#include + +#include "rviz_common/validate_floats.hpp" + +namespace etsi_its_msgs +{ +namespace displays +{ + +enum LaneDirection {ingress, egress, bidirectional, no_travel, unknown_direction}; +enum LaneType {vehicle, crosswalk, bike_lane, sidewalk, median, striping, tracked_vehicle, parking, unknown_type}; + +typedef struct IntersectionLane { + uint8_t lane_id; + LaneType type = LaneType::unknown_type; + LaneDirection direction = LaneDirection::unknown_direction; + std::vector nodes; // relative to ref_point of intersection + std::vector signal_group_ids; +} IntersectionLane; + +typedef struct IntersectionMovementState { + std_msgs::msg::Header header; + uint8_t signal_group_id; + etsi_its_spatem_ts_msgs::msg::MovementPhaseState phase_state; +} IntersectionMovementState; + +/** + * @class IntersectionRenderObject + * @brief + */ +class IntersectionRenderObject +{ + public: + IntersectionRenderObject(etsi_its_mapem_ts_msgs::msg::IntersectionGeometry intersection, bool timestamp_is_present, etsi_its_mapem_ts_msgs::msg::MinuteOfTheYear mapem_stamp, rclcpp::Time receive_time); + + /** + * @brief This function validates all float variables that are part of a IntersectionRenderObject + * + */ + bool validateFloats(); + + /** + * @brief Get age of corresponding MAPEM + * + * @param now reference point in time to calculate the age with + * @return age in seconds as double value + */ + double getAge(rclcpp::Time now); + + /** + * @brief Remove outdated MovementStates + * + * @param now reference point in time to calculate the age with + * @param timeout age threshold for that MovementStates should be removed + */ + void removeOutdatedMovemenStates(rclcpp::Time now, double timeout); + + /** + * @brief Get the IntersectionID + * + * @return unsigned int intersection_id + */ + unsigned int getIntersectionID(); + + /** + * @brief Get the header + * + * @return std_msgs::msg::Header + */ + std_msgs::msg::Header getHeader(); + + /** + * @brief Get the ref_position object + * + * @return geometry_msgs::msg::Point + */ + geometry_msgs::msg::Point getRefPosition(); + + /** + * @brief Return a tf2::Quaternion describing the rotation offset between true-north and grid-north in the UTM zone + * + * @return tf2::Quaternion + */ + tf2::Quaternion getGridConvergenceQuaternion(); + + // public member variables + std::vector lanes; + std::unordered_map movement_states; + + private: + // member variables + std_msgs::msg::Header header; + unsigned int intersection_id; + std::vector layer_ids; + geometry_msgs::msg::PointStamped ref_point; + double grid_convergence_angle; +}; + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file diff --git a/etsi_its_rviz_plugins/include/displays/MAPEM/mapem_display.hpp b/etsi_its_rviz_plugins/include/displays/MAPEM/mapem_display.hpp new file mode 100644 index 00000000..3ae8ff4c --- /dev/null +++ b/etsi_its_rviz_plugins/include/displays/MAPEM/mapem_display.hpp @@ -0,0 +1,108 @@ +/** ============================================================================ +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_mapem_ts_msgs/msg/mapem.hpp" +#include "etsi_its_spatem_ts_msgs/msg/spatem.hpp" + +#include "displays/MAPEM/intersection_render_object.hpp" + +#include "rviz_common/ros_topic_display.hpp" +#include "rviz_rendering/objects/billboard_line.hpp" +#include "rviz_rendering/objects/movable_text.hpp" +#include "rviz_rendering/objects/shape.hpp" + +#include + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz_common +{ +namespace properties +{ + class ColorProperty; + class FloatProperty; + class RosTopicProperty; + class QosProfileProperty; +} // namespace properties +} // namespace rviz_common + +namespace etsi_its_msgs +{ +namespace displays +{ + +/** + * @class MAPEMDisplay + * @brief Displays an etsi_its_mapem_ts_msgs::MAPEM + */ +class MAPEMDisplay : public + rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + MAPEMDisplay(); + ~MAPEMDisplay() override; + + void onInitialize() override; + + void reset() override; + +protected Q_SLOTS: + void changedSPATEMViz(); + void changedSPATEMTopic(); + +protected: + void processMessage(etsi_its_mapem_ts_msgs::msg::MAPEM::ConstSharedPtr msg) override; + void update(float wall_dt, float ros_dt) override; + void SPATEMCallback(etsi_its_spatem_ts_msgs::msg::SPATEM::ConstSharedPtr msg); + + Ogre::ManualObject *manual_object_; + + rclcpp::Node::SharedPtr rviz_node_; + rclcpp::Subscription::SharedPtr spatem_subscriber_; + rclcpp::QoS spatem_qos_profile_ = rclcpp::QoS(1); + + // Properties + rviz_common::properties::BoolProperty *show_meta_, *viz_spatem_; + rviz_common::properties::FloatProperty *mapem_timeout_, *spatem_timeout_, *char_height_, *lane_width_property_, *spatem_sphere_scale_property_; + rviz_common::properties::ColorProperty *color_property_ingress_, *color_property_egress_, *text_color_property_; + rviz_common::properties::RosTopicProperty *spatem_topic_property_; + rviz_common::properties::QosProfileProperty *spatem_qos_property_; + + std::unordered_map intersections_; + std::vector> intsct_ref_points_, signal_groups_; + std::vector> lane_lines_; + std::vector> texts_; + + uint64_t received_spats_=0; +}; + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file diff --git a/etsi_its_rviz_plugins/package.xml b/etsi_its_rviz_plugins/package.xml index 9d81df70..2f8dac43 100644 --- a/etsi_its_rviz_plugins/package.xml +++ b/etsi_its_rviz_plugins/package.xml @@ -22,7 +22,11 @@ rviz_ogre_vendor qtbase5-dev rviz_ogre_vendor - etsi_its_msgs + etsi_its_cam_msgs + etsi_its_cpm_ts_msgs + etsi_its_denm_msgs + etsi_its_mapem_ts_msgs + etsi_its_spatem_ts_msgs etsi_its_msgs_utils pluginlib python3-pyproj diff --git a/etsi_its_rviz_plugins/plugins_description.xml b/etsi_its_rviz_plugins/plugins_description.xml index 6ce44ad6..92ee6ec4 100644 --- a/etsi_its_rviz_plugins/plugins_description.xml +++ b/etsi_its_rviz_plugins/plugins_description.xml @@ -33,4 +33,15 @@ etsi_its_cpm_ts_msgs/msg/CollectivePerceptionMessage + + + + Displays data from a etsi_its_mapem_ts_msgs::MAPEM. + + etsi_its_mapem_ts_msgs/msg/MAPEM + \ No newline at end of file diff --git a/etsi_its_rviz_plugins/src/displays/MAPEM/intersection_render_object.cpp b/etsi_its_rviz_plugins/src/displays/MAPEM/intersection_render_object.cpp new file mode 100644 index 00000000..321d821d --- /dev/null +++ b/etsi_its_rviz_plugins/src/displays/MAPEM/intersection_render_object.cpp @@ -0,0 +1,167 @@ +/** ============================================================================ +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/MAPEM/intersection_render_object.hpp" +#include + +namespace etsi_its_msgs +{ +namespace displays +{ + + IntersectionRenderObject::IntersectionRenderObject(etsi_its_mapem_ts_msgs::msg::IntersectionGeometry intersection, bool timestamp_is_present, etsi_its_mapem_ts_msgs::msg::MinuteOfTheYear mapem_stamp, rclcpp::Time receive_time) { + + intersection_id = etsi_its_mapem_ts_msgs::access::getIntersectionID(intersection); + + int zone; + bool northp; + ref_point = etsi_its_mapem_ts_msgs::access::getRefPointUTMPositionWithConvergenceAngle(intersection, zone, northp, grid_convergence_angle); + + if(timestamp_is_present) { + uint64_t nanosecs = etsi_its_mapem_ts_msgs::access::getUnixNanosecondsFromMinuteOfTheYear(mapem_stamp, receive_time.nanoseconds()); + header.stamp = rclcpp::Time(nanosecs); + } + else { + header.stamp = receive_time; + } + header.frame_id = ref_point.header.frame_id; + + // Parse the lanes + for(size_t i=0; i lane_dir = etsi_its_mapem_ts_msgs::access::getLaneDirection(gen_lane); + if(lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_INGRESS_PATH] && lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_EGRESS_PATH]) intsct_lane.direction = LaneDirection::bidirectional; + else if(!lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_INGRESS_PATH] && !lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_EGRESS_PATH]) intsct_lane.direction = LaneDirection::no_travel; + else if(lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_INGRESS_PATH] && !lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_EGRESS_PATH]) intsct_lane.direction = LaneDirection::ingress; + else if(!lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_INGRESS_PATH] && lane_dir[etsi_its_mapem_ts_msgs::msg::LaneDirection::BIT_INDEX_EGRESS_PATH]) intsct_lane.direction = LaneDirection::egress; + else intsct_lane.direction = LaneDirection::unknown_direction; + // LaneType + if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_VEHICLE) intsct_lane.type = LaneType::vehicle; + else if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_CROSSWALK) intsct_lane.type = LaneType::crosswalk; + else if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_BIKE_LANE) intsct_lane.type = LaneType::bike_lane; + else if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_SIDEWALK) intsct_lane.type = LaneType::sidewalk; + else if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_MEDIAN) intsct_lane.type = LaneType::median; + else if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_STRIPING) intsct_lane.type = LaneType::striping; + else if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_TRACKED_VEHICLE) intsct_lane.type = LaneType::tracked_vehicle; + else if(gen_lane.lane_attributes.lane_type.choice == etsi_its_mapem_ts_msgs::msg::LaneTypeAttributes::CHOICE_PARKING) intsct_lane.type = LaneType::parking; + else intsct_lane.type = LaneType::unknown_type; + // Nodes + if(gen_lane.node_list.choice == etsi_its_mapem_ts_msgs::msg::NodeListXY::CHOICE_NODES) { + etsi_its_mapem_ts_msgs::msg::NodeSetXY node_set = gen_lane.node_list.nodes; + for(size_t j=0; jsecond.header.stamp).seconds() > timeout) it = movement_states.erase(it); + else ++it; + } + } + + unsigned int IntersectionRenderObject::getIntersectionID() { + return intersection_id; + } + + std_msgs::msg::Header IntersectionRenderObject::getHeader() { + return header; + } + + geometry_msgs::msg::Point IntersectionRenderObject::getRefPosition() { + return ref_point.point; + } + + tf2::Quaternion IntersectionRenderObject::getGridConvergenceQuaternion() { + tf2::Quaternion q; + // yaw offset due to grid-convergence + q.setRPY(0, 0, grid_convergence_angle * M_PI / 180.0); + return q; + } + + +} // namespace displays +} // namespace etsi_its_msgs \ No newline at end of file diff --git a/etsi_its_rviz_plugins/src/displays/MAPEM/mapem_display.cpp b/etsi_its_rviz_plugins/src/displays/MAPEM/mapem_display.cpp new file mode 100644 index 00000000..dbd5b86d --- /dev/null +++ b/etsi_its_rviz_plugins/src/displays/MAPEM/mapem_display.cpp @@ -0,0 +1,416 @@ +/** ============================================================================ +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/MAPEM/mapem_display.hpp" + +#include + +#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/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/ros_topic_property.hpp" +#include "rviz_common/properties/qos_profile_property.hpp" + +#include "rviz_common/properties/parse_color.hpp" + +namespace etsi_its_msgs +{ +namespace displays +{ + +const Ogre::ColourValue color_grey(0.5, 0.5, 0.5, 1.0); +const Ogre::ColourValue color_green(0.18, 0.79, 0.21, 1.0); +const Ogre::ColourValue color_orange(0.9, 0.7, 0.09, 1.0); +const Ogre::ColourValue color_red(0.8, 0.2, 0.2, 1.0); + +MAPEMDisplay::MAPEMDisplay() { + // General Properties + mapem_timeout_ = new rviz_common::properties::FloatProperty( + "MAPEM Timeout", 120.0f, + "Time (in s) until MAP disappears", this); + mapem_timeout_->setMin(0); + spatem_topic_property_ = new rviz_common::properties::RosTopicProperty("SPATEM Topic", "/etsi_its_conversion/spatem_ts/out", + rosidl_generator_traits::data_type(), + "Topic of corresponding SPATEMs", this, SLOT(changedSPATEMTopic())); + spatem_qos_property_ = new rviz_common::properties::QosProfileProperty(spatem_topic_property_, qos_profile); + viz_spatem_ = new rviz_common::properties::BoolProperty("Visualize SPATEMs", false, + "Show SPATEMs corresponding to received MAPEMs", this, SLOT(changedSPATEMViz())); + spatem_timeout_ = new rviz_common::properties::FloatProperty( + "SPATEM Timeout", 0.1f, + "Time (in s) until SPAT disappears", viz_spatem_); + spatem_timeout_->setMin(0); + spatem_sphere_scale_property_ = new rviz_common::properties::FloatProperty( + "SPATEM Sphere Scale", 1.0f, + "Scaling factor to adjuste size of SPATEM spheres", viz_spatem_); + spatem_sphere_scale_property_->setMin(0.1); + color_property_ingress_ = new rviz_common::properties::ColorProperty( + "Ingress Lane Color", QColor(85, 85, 255), + "Color to visualize Ingress-Lanes", this); + color_property_egress_ = new rviz_common::properties::ColorProperty( + "Egress Lane Color", QColor(255, 170, 0), + "Color to visualize Egress-Lanes", this); + lane_width_property_ = new rviz_common::properties::FloatProperty( + "MAPEM Lane Width", 1.0, "Width of MAPEM-Lanes", this); + lane_width_property_->setMin(0.1); + show_meta_ = new rviz_common::properties::BoolProperty("Metadata", true, + "Show metadata as text next to MAP reference point", this); + text_color_property_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(255, 255, 255), + "Text color", show_meta_); + char_height_ = new rviz_common::properties::FloatProperty("Scale", 4.0, "Scale of text", show_meta_); + + +} + +MAPEMDisplay::~MAPEMDisplay() { + if (initialized() ) { + scene_manager_->destroyManualObject(manual_object_); + } +} + +void MAPEMDisplay::onInitialize() { + RTDClass::onInitialize(); + + auto nodeAbstraction = context_->getRosNodeAbstraction().lock(); + rviz_node_ = nodeAbstraction->get_raw_node(); + spatem_topic_property_->initialize(nodeAbstraction); + spatem_qos_property_->initialize( + [this](rclcpp::QoS profile) { + spatem_qos_profile_ = profile; + changedSPATEMTopic(); + }); + + manual_object_ = scene_manager_->createManualObject(); + manual_object_->setDynamic(true); + scene_node_->attachObject(manual_object_); +} + +void MAPEMDisplay::reset() { + RTDClass::reset(); + manual_object_->clear(); +} + +void MAPEMDisplay::changedSPATEMViz() { + if(!viz_spatem_->getBool()) { + deleteStatus("SPATEM Topic"); + spatem_subscriber_.reset(); + } + else changedSPATEMTopic(); +} + +void MAPEMDisplay::changedSPATEMTopic() { + spatem_subscriber_.reset(); + received_spats_=0; + if(spatem_topic_property_->isEmpty()) { + setStatus( + rviz_common::properties::StatusProperty::Warn, + "SPATEM Topic", + QString("Error subscribing: Empty topic name")); + return; + } + + std::map> published_topics = rviz_node_->get_topic_names_and_types(); + bool topic_available = false; + std::string topic_message_type; + for (const auto & topic : published_topics) { + // Only add topics whose type matches. + if(topic.first == spatem_topic_property_->getTopicStd()) { + topic_available = true; + for (const auto & type : topic.second) { + topic_message_type = type; + if (type == "etsi_its_spatem_ts_msgs/msg/SPATEM") { + spatem_subscriber_ = rviz_node_->create_subscription(spatem_topic_property_->getTopicStd(), spatem_qos_profile_, std::bind(&MAPEMDisplay::SPATEMCallback, this, std::placeholders::_1)); + return; + } + } + } + } + if(!topic_available) { + setStatus( + rviz_common::properties::StatusProperty::Warn, "SPATEM Topic", + QString("Error subscribing to ") + QString::fromStdString(spatem_topic_property_->getTopicStd()) + + QString(": Topic is not available!")); + } + else { + setStatus( + rviz_common::properties::StatusProperty::Warn, "SPATEM Topic", + QString("Error subscribing to ") + QString::fromStdString(spatem_topic_property_->getTopicStd()) + + QString(": Message type ") + QString::fromStdString(topic_message_type) + QString::fromStdString(" does not equal etsi_its_spatem_ts_msgs::msg::SPATEM!")); + } +} + +void MAPEMDisplay::SPATEMCallback(etsi_its_spatem_ts_msgs::msg::SPATEM::ConstSharedPtr msg) { + rclcpp::Time now = rviz_node_->now(); + // iterate over all IntersectionStates + for(size_t i = 0; ispat.intersections.array.size(); i++) { + etsi_its_spatem_ts_msgs::msg::IntersectionState intersection_state = msg->spat.intersections.array[i]; + unsigned int intersection_id = etsi_its_spatem_ts_msgs::access::getIntersectionID(intersection_state); + // Check if IntersectionID is already present in intersections-list + auto it = intersections_.find(intersection_id); + if (it == intersections_.end()) continue; // intersection is not available, continue loop + // derive stamp from Intersection State + std_msgs::msg::Header header; + if(intersection_state.moy_is_present) { + etsi_its_spatem_ts_msgs::msg::MinuteOfTheYear moy = etsi_its_spatem_ts_msgs::access::getMinuteOfTheYear(intersection_state); + uint64_t nanosecs = etsi_its_spatem_ts_msgs::access::getUnixNanosecondsFromMinuteOfTheYear(moy, now.nanoseconds()); + if(intersection_state.time_stamp_is_present) { + double secs_in_minute = etsi_its_spatem_ts_msgs::access::getDSecondValue(intersection_state); + nanosecs += (uint64_t)(secs_in_minute*1e9); + } + header.stamp = rclcpp::Time(nanosecs); + } + else { + header.stamp = now; + } + // iterate over all MovemenStates + for(size_t j=0; jsecond.movement_states.find(mvmt_state.signal_group_id); + if (mvmnt_it != it->second.movement_states.end()) mvmnt_it->second = mvmt_state; // SignalGroup exists, update the value + else it->second.movement_states.insert(std::make_pair(mvmt_state.signal_group_id, mvmt_state)); + } + } + + ++received_spats_; + setStatus( + rviz_common::properties::StatusProperty::Ok, "SPATEM Topic", QString::number(received_spats_) + " messages received"); +} + +void MAPEMDisplay::processMessage(etsi_its_mapem_ts_msgs::msg::MAPEM::ConstSharedPtr msg) { + // Process MAPEM message + rclcpp::Time now = rviz_node_->now(); + // Intersections + if(!msg->map.intersections_is_present) return; + etsi_its_mapem_ts_msgs::msg::MinuteOfTheYear moy = msg->map.time_stamp; + for(size_t i = 0; imap.intersections.array.size(); i++) + { + IntersectionRenderObject intsct(msg->map.intersections.array[i], msg->map.time_stamp_is_present, moy, now); + if(!intsct.validateFloats()) + { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + // Check if IntersectionID is already present in list + auto it = intersections_.find(intsct.getIntersectionID()); + if (it != intersections_.end()) { + // Intersection exists, update the intersection but keep the MovementStates + intsct.movement_states = it->second.movement_states; + it->second = intsct; + } + else intersections_.insert(std::make_pair(intsct.getIntersectionID(), intsct)); + } + + return; +} + +void MAPEMDisplay::update(float, float) { + // Check for outdated intersections and movement states + rclcpp::Time now = rviz_node_->now(); + for (auto it = intersections_.begin(); it != intersections_.end(); ) { + if (it->second.getAge(now) > mapem_timeout_->getFloat()) it = intersections_.erase(it); + else { + it->second.removeOutdatedMovemenStates(now, spatem_timeout_->getFloat()); + ++it; + } + } + // Render all valid intersections + intsct_ref_points_.clear(); + lane_lines_.clear(); + signal_groups_.clear(); + texts_.clear(); + for(auto it = intersections_.begin(); it != intersections_.end(); ++it) { + IntersectionRenderObject intsctn = it->second; + Ogre::Vector3 sn_position; + Ogre::Quaternion sn_orientation; + if (!context_->getFrameManager()->getTransform(intsctn.getHeader(), sn_position, sn_orientation)) { + setMissingTransformToFixedFrame(intsctn.getHeader().frame_id); + return; + } + setTransformOk(); + + // set pose of scene node + scene_node_->setPosition(sn_position); + scene_node_->setOrientation(sn_orientation); + + auto child_scene_node = scene_node_->createChildSceneNode(); + // Set position of scene node + geometry_msgs::msg::Point ref_position = intsctn.getRefPosition(); + Ogre::Vector3 position(ref_position.x, ref_position.y, ref_position.z); + tf2::Quaternion rot_offset = intsctn.getGridConvergenceQuaternion(); + Ogre::Quaternion orientation(rot_offset.w(), rot_offset.x(), rot_offset.y(), rot_offset.z()); + + // set pose of child scene node of intersection + child_scene_node->setPosition(position); + child_scene_node->setOrientation(orientation); + + // create sphere object + std::shared_ptr sphere = std::make_shared(rviz_rendering::Shape::Sphere, scene_manager_, child_scene_node); + + // set the dimensions of sphere + double scale = spatem_sphere_scale_property_->getFloat(); + Ogre::Vector3 dims; + dims.x = 1.0 * scale; + dims.y = 1.0 * scale; + dims.z = 1.0 * scale; + sphere->setScale(dims); + + // set the color of sphere + sphere->setColor(color_grey); + intsct_ref_points_.push_back(sphere); + + // visualize the lanes + for(size_t i = 0; i line = std::make_shared(scene_manager_, child_scene_node); + Ogre::ColourValue lane_color; + if(intsctn.lanes[i].direction == LaneDirection::ingress) lane_color = rviz_common::properties::qtToOgre(color_property_ingress_->getColor()); + else if(intsctn.lanes[i].direction == LaneDirection::egress) lane_color = rviz_common::properties::qtToOgre(color_property_egress_->getColor()); + else lane_color = color_grey; + line->setColor(lane_color.r, lane_color.g, lane_color.b, lane_color.a); + double line_width = lane_width_property_->getFloat(); + line->setLineWidth(line_width); + for(size_t j = 0; jaddPoint(p); + } + lane_lines_.push_back(line); + // Signal Groups + if(viz_spatem_->getBool() && intsctn.lanes[i].signal_group_ids.size() && intsctn.lanes[i].direction != LaneDirection::egress) { + std::shared_ptr sg = std::make_shared(rviz_rendering::Shape::Sphere, scene_manager_, child_scene_node); + sg->setScale(dims); + // Set color according to state + // Check if SignalGroup is present in IntersectionMovementState of Intersection + std::unordered_map::iterator mvmnt_it; + for(size_t j=0; jsecond.phase_state.value) { + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::UNAVAILABLE: + sg->setColor(color_grey); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::DARK: + sg->setColor(color_grey); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::STOP_THEN_PROCEED: + sg->setColor(color_red); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::STOP_AND_REMAIN: + sg->setColor(color_red); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::PRE_MOVEMENT: + sg->setColor(color_orange); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::PERMISSIVE_MOVEMENT_ALLOWED: + sg->setColor(color_green); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::PROTECTED_MOVEMENT_ALLOWED: + sg->setColor(color_green); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::PERMISSIVE_CLEARANCE: + sg->setColor(color_orange); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::PROTECTED_CLEARANCE: + sg->setColor(color_orange); + break; + + case etsi_its_spatem_ts_msgs::msg::MovementPhaseState::CAUTION_CONFLICTING_TRAFFIC: + sg->setColor(color_orange); + break; + + default: + sg->setColor(color_grey); + break; + + } + } + else { + sg->setColor(color_grey); + } + Ogre::Vector3 p; + p.x = intsctn.lanes[i].nodes.front().x; + p.y = intsctn.lanes[i].nodes.front().y; + p.z = intsctn.lanes[i].nodes.front().z; + sg->setPosition(p); + signal_groups_.push_back(sg); + } + } + + // Visualize meta-information as text + if(show_meta_->getBool()) { + std::string text; + text+="IntersectionID: " + std::to_string(intsctn.getIntersectionID()); + 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::MAPEMDisplay, rviz_common::Display) \ No newline at end of file diff --git a/utils/scripts/pcap2bag.py b/utils/scripts/pcap2bag.py index f2fe4bc2..54cde1db 100755 --- a/utils/scripts/pcap2bag.py +++ b/utils/scripts/pcap2bag.py @@ -31,8 +31,7 @@ import numpy as np import pyshark from rosbags.rosbag2 import Writer -from rosbags.serde import serialize_cdr -from rosbags.typesys import get_types_from_msg, register_types +from rosbags.typesys import Stores, get_types_from_msg, get_typestore from tqdm import tqdm UDP_PACKET_MSG = """ @@ -41,12 +40,13 @@ uint16 src_port uint8[] data """ -register_types(get_types_from_msg(UDP_PACKET_MSG, "udp_msgs/msg/UdpPacket")) -from rosbags.typesys.types import builtin_interfaces__msg__Time as Time -from rosbags.typesys.types import std_msgs__msg__Header as Header -from rosbags.typesys.types import udp_msgs__msg__UdpPacket as UdpPacket +typestore = get_typestore(Stores.ROS2_HUMBLE) +typestore.register(get_types_from_msg(UDP_PACKET_MSG, 'udp_msgs/msg/UdpPacket')) +Time = typestore.types['builtin_interfaces/msg/Time'] +Header = typestore.types['std_msgs/msg/Header'] +UdpPacket = typestore.types['udp_msgs/msg/UdpPacket'] def parseCli(): @@ -79,13 +79,15 @@ def main(): # parse packets from pcap print(f"Loading packets from {args.pcap} ...", end="", flush=True) - pcap = pyshark.FileCapture("rx_r1a.pcap", include_raw=True, use_json=True) + pcap = pyshark.FileCapture(args.pcap, include_raw=True, use_json=True) pcap.load_packets() print(" done") # convert packets to ROS messages msgs = [] for packet in tqdm(pcap, total=len(pcap), desc="Converting packets to ROS messages"): + if "btpb_raw" not in packet or "its_raw" not in packet: + continue btp_header = hexStringToUint8Array(packet.btpb_raw.value) its_payload = hexStringToUint8Array(packet.its_raw.value) @@ -105,11 +107,11 @@ def main(): topic = args.topic msg_type = UdpPacket.__msgtype__ - connection = bag.add_connection(topic, msg_type) + connection = bag.add_connection(topic, msg_type, typestore=typestore) for msg in tqdm(msgs, desc=f"Writing ROS messages to {args.output_bag}"): timestamp = msg.header.stamp.sec * 1e9 + msg.header.stamp.nanosec - bag.write(connection, timestamp, serialize_cdr(msg, msg_type)) + bag.write(connection, timestamp, typestore.serialize_cdr(msg, msg_type)) if __name__ == "__main__": diff --git a/utils/scripts/requirements.txt b/utils/scripts/requirements.txt index 01e2e594..037b1e1b 100644 --- a/utils/scripts/requirements.txt +++ b/utils/scripts/requirements.txt @@ -1,4 +1,4 @@ numpy>=1.23.3 pyshark~=0.6 # requires tshark installation -rosbags~=0.9.16 +rosbags~=0.10.6 tqdm~=4.66.1 \ No newline at end of file