From 75e3413bdfe194de355f923cb47b99a8d5457873 Mon Sep 17 00:00:00 2001 From: Lennart Reiher Date: Fri, 7 Mar 2025 12:06:08 +0100 Subject: [PATCH 1/3] only create udp pub/sub if required --- .../etsi_its_conversion/src/Converter.cpp | 36 +++++++++++++------ 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp index 69ddb0d1..6cf91a9c 100644 --- a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp +++ b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp @@ -206,9 +206,13 @@ void Converter::loadParameters() { } // check ros2udp_etsi_types - for (const auto& e : ros2udp_etsi_types_) { - if (std::find(kRos2UdpEtsiTypesParamDefault.begin(), kRos2UdpEtsiTypesParamDefault.end(), e) == kRos2UdpEtsiTypesParamDefault.end()) - ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', skipping", e.c_str(), kRos2UdpEtsiTypesParam.c_str()); + for (auto it = ros2udp_etsi_types_.begin(); it != ros2udp_etsi_types_.end(); ) { + if (std::find(kRos2UdpEtsiTypesParamDefault.begin(), kRos2UdpEtsiTypesParamDefault.end(), *it) == kRos2UdpEtsiTypesParamDefault.end()) { + ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', removing", it->c_str(), kRos2UdpEtsiTypesParam.c_str()); + it = ros2udp_etsi_types_.erase(it); + } else { + ++it; + } } // load udp2ros_etsi_types_ @@ -227,9 +231,13 @@ void Converter::loadParameters() { } // check udp2ros_etsi_types - for (const auto& e : udp2ros_etsi_types_) { - if (std::find(kUdp2RosEtsiTypesParamDefault.begin(), kUdp2RosEtsiTypesParamDefault.end(), e) == kUdp2RosEtsiTypesParamDefault.end()) - ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', skipping", e.c_str(), kUdp2RosEtsiTypesParam.c_str()); + for (auto it = udp2ros_etsi_types_.begin(); it != udp2ros_etsi_types_.end(); ) { + if (std::find(kUdp2RosEtsiTypesParamDefault.begin(), kUdp2RosEtsiTypesParamDefault.end(), *it) == kUdp2RosEtsiTypesParamDefault.end()) { + ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', removing", it->c_str(), kRos2UdpEtsiTypesParam.c_str()); + it = udp2ros_etsi_types_.erase(it); + } else { + ++it; + } } if (!has_btp_destination_port_ && udp2ros_etsi_types_.size() > 1) { ROS12_LOG(WARN, "Parameter '%s' is set to 'false', but multiple 'udp2ros_etsi_types' are configured, dropping all but the first", kHasBtpDestinationPortParam.c_str()); @@ -278,8 +286,12 @@ void Converter::setup() { // create subscribers and publishers #ifdef ROS1 - publisher_udp_ = std::make_shared(private_node_handle_.advertise(kOutputTopicUdp, publisher_queue_size_)); - subscriber_udp_ = std::make_shared(private_node_handle_.subscribe(kInputTopicUdp, subscriber_queue_size_, &Converter::udpCallback, this)); + if (!ros2udp_etsi_types_.empty()) { + publisher_udp_ = std::make_shared(private_node_handle_.advertise(kOutputTopicUdp, publisher_queue_size_)); + } + if (!udp2ros_etsi_types_.empty()) { + subscriber_udp_ = std::make_shared(private_node_handle_.subscribe(kInputTopicUdp, subscriber_queue_size_, &Converter::udpCallback, this)); + } if (std::find(udp2ros_etsi_types_.begin(), udp2ros_etsi_types_.end(), "cam") != udp2ros_etsi_types_.end()) { publishers_["cam"] = std::make_shared(private_node_handle_.advertise(kOutputTopicCam, publisher_queue_size_)); ROS12_LOG(INFO, "Converting UDP messages of type CAM on '%s' to native ROS messages on '%s'", subscriber_udp_->getTopic().c_str(), publishers_["cam"]->getTopic().c_str()); @@ -361,8 +373,12 @@ void Converter::setup() { ROS12_LOG(INFO, "Converting native ROS VAM (TS) on '%s' to UDP messages on '%s'", subscribers_["vam_ts"]->getTopic().c_str(), publisher_udp_->getTopic().c_str()); } #else - publisher_udp_ = this->create_publisher(kOutputTopicUdp, publisher_queue_size_); - subscriber_udp_ = this->create_subscription(kInputTopicUdp, subscriber_queue_size_, std::bind(&Converter::udpCallback, this, std::placeholders::_1)); + if (!ros2udp_etsi_types_.empty()) { + publisher_udp_ = this->create_publisher(kOutputTopicUdp, publisher_queue_size_); + } + if (!udp2ros_etsi_types_.empty()) { + subscriber_udp_ = this->create_subscription(kInputTopicUdp, subscriber_queue_size_, std::bind(&Converter::udpCallback, this, std::placeholders::_1)); + } if (std::find(udp2ros_etsi_types_.begin(), udp2ros_etsi_types_.end(), "cam") != udp2ros_etsi_types_.end()) { publisher_cam_ = this->create_publisher(kOutputTopicCam, publisher_queue_size_); ROS12_LOG(INFO, "Converting UDP messages of type CAM on '%s' to native ROS messages on '%s'", subscriber_udp_->get_topic_name(), publisher_cam_->get_topic_name()); From 09b40962a54a050304f2f87fecb6216323499cd7 Mon Sep 17 00:00:00 2001 From: Lennart Reiher Date: Wed, 12 Mar 2025 09:58:07 +0000 Subject: [PATCH 2/3] check params for supported etsi types and not only param default types --- README.md | 4 ++-- .../include/etsi_its_conversion/Converter.hpp | 2 ++ etsi_its_conversion/etsi_its_conversion/src/Converter.cpp | 8 +++++--- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 73920fdc..c87058b5 100644 --- a/README.md +++ b/README.md @@ -208,8 +208,8 @@ rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_types:=[ca | `has_btp_destination_port` | `bool` | whether incoming/outgoing UDP messages include a [2-byte BTP destination port](https://www.etsi.org/deliver/etsi_en/302600_302699/3026360501/02.01.00_20/en_3026360501v020100a.pdf) | | `btp_destination_port_offset` | `int` | number of bytes before an optional 2-byte BTP destination port, see `has_btp_destination_port` (always `0` in outgoing UDP payload) | | `etsi_message_payload_offset` | `int` | number of bytes before actual ETSI message payload (always `0` or `4` (if `has_btp_destination_port`) in outgoing UDP payload) | -| `ros2udp_etsi_types` | `string[]` | list of ETSI types to convert from `etsi_its_msgs` to `udp_msgs` (defaults to all norms and specifications of all possible ETSI types) | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `spatem_ts`, `vam_ts` | -| `udp2ros_etsi_types` | `string[]` | list of ETSI types to convert from `udp_msgs` to `etsi_its_msgs` (defaults only to the norm or specification of all possible ETSI types) | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `spatem_ts`, `vam_ts` | +| `ros2udp_etsi_types` | `string[]` | list of ETSI types to convert from `etsi_its_msgs` to `udp_msgs` (defaults to all supported ETSI types, including EN and TS versions) | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `spatem_ts`, `vam_ts` | +| `udp2ros_etsi_types` | `string[]` | list of ETSI types to convert from `udp_msgs` to `etsi_its_msgs` (defaults to all supported ETSI types, either EN or TS version) | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `spatem_ts`, `vam_ts` | | `subscriber_queue_size` | `int` | queue size for incoming ROS messages | | `publisher_queue_size` | `int` | queue size for outgoing ROS messages | | `check_constraints_before_encoding` | `bool` | whether an asn constraint check should be performed before encoding using asn1c's `asn_check_constraints` function (setting to `true` could lead to segmentation faults because of infinite recursion; [known asn1c issue](https://github.com/vlm/asn1c/issues/410)) | diff --git a/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp b/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp index 6cf12f69..339e69d8 100644 --- a/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp +++ b/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp @@ -173,6 +173,8 @@ class Converter : public rclcpp::Node { static const int kEtsiMessagePayloadOffsetParamDefault; static const std::string kRos2UdpEtsiTypesParam; static const std::string kUdp2RosEtsiTypesParam; + static const std::vector kRos2UdpEtsiTypesParamSupportedOptions; + static const std::vector kUdp2RosEtsiTypesParamSupportedOptions; static const std::vector kRos2UdpEtsiTypesParamDefault; static const std::vector kUdp2RosEtsiTypesParamDefault; static const std::string kSubscriberQueueSizeParam; diff --git a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp index 6cf91a9c..1ceac559 100644 --- a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp +++ b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp @@ -102,7 +102,9 @@ const std::string Converter::kEtsiMessagePayloadOffsetParam{"etsi_message_payloa const int Converter::kEtsiMessagePayloadOffsetParamDefault{78}; const std::string Converter::kRos2UdpEtsiTypesParam{"ros2udp_etsi_types"}; const std::string Converter::kUdp2RosEtsiTypesParam{"udp2ros_etsi_types"}; -const std::vector Converter::kRos2UdpEtsiTypesParamDefault{"cam", "cam_ts", "cpm_ts", "denm", "denm_ts", "mapem_ts", "spatem_ts", "vam_ts"}; +const std::vector kRos2UdpEtsiTypesParamSupportedOptions{"cam", "cam_ts", "cpm_ts", "denm", "denm_ts", "mapem_ts", "spatem_ts", "vam_ts"}; +const std::vector kUdp2RosEtsiTypesParamSupportedOptions = kRos2UdpEtsiTypesParamSupportedOptions; +const std::vector Converter::kRos2UdpEtsiTypesParamDefault = kRos2UdpEtsiTypesParamSupportedOptions; const std::vector Converter::kUdp2RosEtsiTypesParamDefault{"cam", "cpm_ts", "denm", "mapem_ts", "spatem_ts", "vam_ts"}; const std::string Converter::kSubscriberQueueSizeParam{"subscriber_queue_size"}; const int Converter::kSubscriberQueueSizeParamDefault{10}; @@ -207,7 +209,7 @@ void Converter::loadParameters() { // check ros2udp_etsi_types for (auto it = ros2udp_etsi_types_.begin(); it != ros2udp_etsi_types_.end(); ) { - if (std::find(kRos2UdpEtsiTypesParamDefault.begin(), kRos2UdpEtsiTypesParamDefault.end(), *it) == kRos2UdpEtsiTypesParamDefault.end()) { + if (std::find(kRos2UdpEtsiTypesParamSupportedOptions.begin(), kRos2UdpEtsiTypesParamSupportedOptions.end(), *it) == kRos2UdpEtsiTypesParamSupportedOptions.end()) { ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', removing", it->c_str(), kRos2UdpEtsiTypesParam.c_str()); it = ros2udp_etsi_types_.erase(it); } else { @@ -232,7 +234,7 @@ void Converter::loadParameters() { // check udp2ros_etsi_types for (auto it = udp2ros_etsi_types_.begin(); it != udp2ros_etsi_types_.end(); ) { - if (std::find(kUdp2RosEtsiTypesParamDefault.begin(), kUdp2RosEtsiTypesParamDefault.end(), *it) == kUdp2RosEtsiTypesParamDefault.end()) { + if (std::find(kUdp2RosEtsiTypesParamSupportedOptions.begin(), kUdp2RosEtsiTypesParamSupportedOptions.end(), *it) == kUdp2RosEtsiTypesParamSupportedOptions.end()) { ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', removing", it->c_str(), kRos2UdpEtsiTypesParam.c_str()); it = udp2ros_etsi_types_.erase(it); } else { From 4be3fbb9a3c34b72ef0cc32b28380e239ffe6191 Mon Sep 17 00:00:00 2001 From: Lennart Reiher Date: Wed, 12 Mar 2025 11:24:59 +0000 Subject: [PATCH 3/3] remove redundant kEtsiTypesParamSupportedOptions variable --- .../include/etsi_its_conversion/Converter.hpp | 3 +-- .../etsi_its_conversion/src/Converter.cpp | 9 ++++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp b/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp index 339e69d8..2f24c835 100644 --- a/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp +++ b/etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp @@ -173,8 +173,7 @@ class Converter : public rclcpp::Node { static const int kEtsiMessagePayloadOffsetParamDefault; static const std::string kRos2UdpEtsiTypesParam; static const std::string kUdp2RosEtsiTypesParam; - static const std::vector kRos2UdpEtsiTypesParamSupportedOptions; - static const std::vector kUdp2RosEtsiTypesParamSupportedOptions; + static const std::vector kEtsiTypesParamSupportedOptions; static const std::vector kRos2UdpEtsiTypesParamDefault; static const std::vector kUdp2RosEtsiTypesParamDefault; static const std::string kSubscriberQueueSizeParam; diff --git a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp index 1ceac559..2f2d25b3 100644 --- a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp +++ b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp @@ -102,9 +102,8 @@ const std::string Converter::kEtsiMessagePayloadOffsetParam{"etsi_message_payloa const int Converter::kEtsiMessagePayloadOffsetParamDefault{78}; const std::string Converter::kRos2UdpEtsiTypesParam{"ros2udp_etsi_types"}; const std::string Converter::kUdp2RosEtsiTypesParam{"udp2ros_etsi_types"}; -const std::vector kRos2UdpEtsiTypesParamSupportedOptions{"cam", "cam_ts", "cpm_ts", "denm", "denm_ts", "mapem_ts", "spatem_ts", "vam_ts"}; -const std::vector kUdp2RosEtsiTypesParamSupportedOptions = kRos2UdpEtsiTypesParamSupportedOptions; -const std::vector Converter::kRos2UdpEtsiTypesParamDefault = kRos2UdpEtsiTypesParamSupportedOptions; +const std::vector kEtsiTypesParamSupportedOptions{"cam", "cam_ts", "cpm_ts", "denm", "denm_ts", "mapem_ts", "spatem_ts", "vam_ts"}; +const std::vector Converter::kRos2UdpEtsiTypesParamDefault = kEtsiTypesParamSupportedOptions; const std::vector Converter::kUdp2RosEtsiTypesParamDefault{"cam", "cpm_ts", "denm", "mapem_ts", "spatem_ts", "vam_ts"}; const std::string Converter::kSubscriberQueueSizeParam{"subscriber_queue_size"}; const int Converter::kSubscriberQueueSizeParamDefault{10}; @@ -209,7 +208,7 @@ void Converter::loadParameters() { // check ros2udp_etsi_types for (auto it = ros2udp_etsi_types_.begin(); it != ros2udp_etsi_types_.end(); ) { - if (std::find(kRos2UdpEtsiTypesParamSupportedOptions.begin(), kRos2UdpEtsiTypesParamSupportedOptions.end(), *it) == kRos2UdpEtsiTypesParamSupportedOptions.end()) { + if (std::find(kEtsiTypesParamSupportedOptions.begin(), kEtsiTypesParamSupportedOptions.end(), *it) == kEtsiTypesParamSupportedOptions.end()) { ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', removing", it->c_str(), kRos2UdpEtsiTypesParam.c_str()); it = ros2udp_etsi_types_.erase(it); } else { @@ -234,7 +233,7 @@ void Converter::loadParameters() { // check udp2ros_etsi_types for (auto it = udp2ros_etsi_types_.begin(); it != udp2ros_etsi_types_.end(); ) { - if (std::find(kUdp2RosEtsiTypesParamSupportedOptions.begin(), kUdp2RosEtsiTypesParamSupportedOptions.end(), *it) == kUdp2RosEtsiTypesParamSupportedOptions.end()) { + if (std::find(kEtsiTypesParamSupportedOptions.begin(), kEtsiTypesParamSupportedOptions.end(), *it) == kEtsiTypesParamSupportedOptions.end()) { ROS12_LOG(WARN, "Invalid value '%s' for parameter '%s', removing", it->c_str(), kRos2UdpEtsiTypesParam.c_str()); it = udp2ros_etsi_types_.erase(it); } else {