Improve configuration of conversion node

README.md

@@ -149,12 +149,12 @@ The conversion node bridges all ETSI ITS message types at the same time in both
 # ROS 2
 ros2 launch etsi_its_conversion converter.launch.py
 # or
-ros2 run etsi_its_conversion etsi_its_conversion_node --ros-args -p etsi_type:=auto
+ros2 run etsi_its_conversion etsi_its_conversion_node --ros-args -p etsi_types:=[cam,denm] -p has_btp_destination_port:=true -p btp_destination_port_offset:=8 -p etsi_message_payload_offset:=78
 
 # ROS
 roslaunch etsi_its_conversion converter.ros1.launch
 # or
-rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_type:=auto
+rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_types:=[cam,denm] _has_btp_destination_port:=true _btp_destination_port_offset:=8 _etsi_message_payload_offset:=78
 ```
 
 ##### Subscribed Topics
@@ -177,7 +177,10 @@ rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_type:=auto
 
 | Parameter | Type | Description | Options |
 | --- | --- | --- | --- |
-| `etsi_type` | `string` | if set to `auto`, in- and outgoing UDP payloads are expected to include a [4-byte BTP header](https://www.etsi.org/deliver/etsi_en/302600_302699/3026360501/02.01.00_20/en_3026360501v020100a.pdf) | `auto`, `cam`, `denm` |
+| `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` |
+| `etsi_message_payload_offset` | `int` | number of bytes before actual ETSI message payload |
+| `etsi_types` | `string[]` | list of ETSI types to convert | `cam`, `denm` |
 
 
 #### Automated Generation

etsi_its_conversion/etsi_its_conversion/config/params.ros1.yml

@@ -1 +1,6 @@
-etsi_type: auto
+has_btp_destination_port: true
+btp_destination_port_offset: 8  # default for Cohda Wireless MK-5 exampleETSI application
+etsi_message_payload_offset: 78 # default for Cohda Wireless MK-5 exampleETSI application
+etsi_types:
+  - cam
+  - denm

etsi_its_conversion/etsi_its_conversion/config/params.yml

@@ -1,3 +1,8 @@
 etsi_its_conversion:
   ros__parameters:
-    etsi_type: auto
+    has_btp_destination_port: true
+    btp_destination_port_offset: 8  # default for Cohda Wireless MK-5 exampleETSI application
+    etsi_message_payload_offset: 78 # default for Cohda Wireless MK-5 exampleETSI application
+    etsi_types:
+      - cam
+      - denm

etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp

@@ -92,10 +92,19 @@ class Converter : public rclcpp::Node {
     static const std::string kInputTopicDenm;
     static const std::string kOutputTopicDenm;
 
-    static const std::string kEtsiTypeParam;
-    static const std::string kEtsiTypeParamDefault;
-
-    std::string etsi_type_;
+    static const std::string kHasBtpDestinationPortParam;
+    static const bool kHasBtpDestinationPortParamDefault;
+    static const std::string kBtpDestinationPortOffsetParam;
+    static const int kBtpDestinationPortOffsetParamDefault;
+    static const std::string kEtsiMessagePayloadOffsetParam;
+    static const int kEtsiMessagePayloadOffsetParamDefault;
+    static const std::string kEtsiTypesParam;
+    static const std::vector<std::string> kEtsiTypesParamDefault;
+
+    bool has_btp_destination_port_;
+    int btp_destination_port_offset_;
+    int etsi_message_payload_offset_;
+    std::vector<std::string> etsi_types_;
 
 #ifdef ROS1
     ros::NodeHandle private_node_handle_;

etsi_its_conversion/etsi_its_conversion/src/Converter.cpp

@@ -68,8 +68,14 @@ const std::string Converter::kInputTopicDenm{"~/denm/in"};
 const std::string Converter::kOutputTopicDenm{"~/denm/out"};
 #endif
 
-const std::string Converter::kEtsiTypeParam{"etsi_type"};
-const std::string Converter::kEtsiTypeParamDefault{"auto"};
+const std::string Converter::kHasBtpDestinationPortParam{"has_btp_destination_port"};
+const bool Converter::kHasBtpDestinationPortParamDefault{true};
+const std::string Converter::kBtpDestinationPortOffsetParam{"btp_destination_port_offset"};
+const int Converter::kBtpDestinationPortOffsetParamDefault{8};
+const std::string Converter::kEtsiMessagePayloadOffsetParam{"etsi_message_payload_offset"};
+const int Converter::kEtsiMessagePayloadOffsetParamDefault{78};
+const std::string Converter::kEtsiTypesParam{"etsi_types"};
+const std::vector<std::string> Converter::kEtsiTypesParamDefault{"cam", "denm"};
 
 
 bool Converter::logLevelIsDebug() {
@@ -110,32 +116,87 @@ Converter::Converter(const rclcpp::NodeOptions& options) : Node("converter", opt
 
 void Converter::loadParameters() {
 
-  std::vector<std::string> known_etsi_types = {kEtsiTypeParamDefault, "cam", "denm"};
+#ifndef ROS1
+  rcl_interfaces::msg::ParameterDescriptor param_desc;
+#endif
 
+  // load has_btp_destination_port
 #ifdef ROS1
-  if (!private_node_handle_.param<std::string>(kEtsiTypeParam, etsi_type_, kEtsiTypeParamDefault)) {
+  if (!private_node_handle_.param<bool>(kHasBtpDestinationPortParam, has_btp_destination_port_, kHasBtpDestinationPortParamDefault)) {
     NODELET_WARN(
 #else
-  rcl_interfaces::msg::ParameterDescriptor param_desc;
+  param_desc = rcl_interfaces::msg::ParameterDescriptor();
+  param_desc.description = "whether incoming/outgoing UDP messages include a 4-byte BTP header";
+  this->declare_parameter(kHasBtpDestinationPortParam, kHasBtpDestinationPortParamDefault, param_desc);
+  if (!this->get_parameter(kHasBtpDestinationPortParam, has_btp_destination_port_)) {
+    RCLCPP_WARN(this->get_logger(),
+#endif
+      "Parameter '%s' is not set, defaulting to '%s'", kHasBtpDestinationPortParam.c_str(), kHasBtpDestinationPortParamDefault ? "true" : "false");
+  }
+
+  // load btp_destination_port_offset
+#ifdef ROS1
+  if (!private_node_handle_.param<int>(kBtpDestinationPortOffsetParam, btp_destination_port_offset_, kBtpDestinationPortOffsetParamDefault)) {
+    NODELET_WARN(
+#else
+  param_desc = rcl_interfaces::msg::ParameterDescriptor();
+  param_desc.description = "number of bytes until actual message payload starts in incoming UDP message (optionally including BTP header)";
+  this->declare_parameter(kBtpDestinationPortOffsetParam, kBtpDestinationPortOffsetParamDefault, param_desc);
+  if (!this->get_parameter(kBtpDestinationPortOffsetParam, btp_destination_port_offset_)) {
+    RCLCPP_WARN(this->get_logger(),
+#endif
+      "Parameter '%s' is not set, defaulting to '%d'", kBtpDestinationPortOffsetParam.c_str(), kBtpDestinationPortOffsetParamDefault);
+  }
+
+  // load etsi_message_payload_offset
+#ifdef ROS1
+  if (!private_node_handle_.param<int>(kEtsiMessagePayloadOffsetParam, etsi_message_payload_offset_, kEtsiMessagePayloadOffsetParamDefault)) {
+    NODELET_WARN(
+#else
+  param_desc = rcl_interfaces::msg::ParameterDescriptor();
+  param_desc.description = "number of bytes until actual message payload starts in incoming UDP message (optionally including BTP header)";
+  this->declare_parameter(kEtsiMessagePayloadOffsetParam, kEtsiMessagePayloadOffsetParamDefault, param_desc);
+  if (!this->get_parameter(kEtsiMessagePayloadOffsetParam, etsi_message_payload_offset_)) {
+    RCLCPP_WARN(this->get_logger(),
+#endif
+      "Parameter '%s' is not set, defaulting to '%d'", kEtsiMessagePayloadOffsetParam.c_str(), kEtsiMessagePayloadOffsetParamDefault);
+  }
+
+  // load etsi_types
+#ifdef ROS1
+  if (!private_node_handle_.param<std::vector<std::string>>(kEtsiTypesParam, etsi_types_, kEtsiTypesParamDefault)) {
+    NODELET_WARN(
+#else
+  param_desc = rcl_interfaces::msg::ParameterDescriptor();
   std::stringstream ss;
-  ss << "ETSI type to convert, one of ";
-  for (const auto& e : known_etsi_types) ss << e << ", ";
+  ss << "list of ETSI types to convert, one of ";
+  for (const auto& e : kEtsiTypesParamDefault) ss << e << ", ";
   param_desc.description = ss.str();
-  this->declare_parameter(kEtsiTypeParam, kEtsiTypeParamDefault, param_desc);
-
-  if (!this->get_parameter(kEtsiTypeParam, etsi_type_)) {
+  this->declare_parameter(kEtsiTypesParam, kEtsiTypesParamDefault, param_desc);
+  if (!this->get_parameter(kEtsiTypesParam, etsi_types_)) {
     RCLCPP_WARN(this->get_logger(),
 #endif
-      "Parameter '%s' is not set, defaulting to '%s'", kEtsiTypeParam.c_str(), kEtsiTypeParamDefault.c_str());
+      "Parameter '%s' is not set, defaulting to all", kEtsiTypesParam.c_str());
   }
 
-  if (std::find(known_etsi_types.begin(), known_etsi_types.end(), etsi_type_) == known_etsi_types.end()) {
+  // check etsi_types
+  for (const auto& e : etsi_types_) {
+    if (std::find(kEtsiTypesParamDefault.begin(), kEtsiTypesParamDefault.end(), e) == kEtsiTypesParamDefault.end())
+#ifdef ROS1
+      NODELET_WARN(
+#else
+      RCLCPP_WARN(this->get_logger(),
+#endif
+        "Invalid value '%s' for parameter '%s', skipping", e.c_str(), kEtsiTypesParam.c_str());
+  }
+  if (!has_btp_destination_port_ && etsi_types_.size() > 1) {
 #ifdef ROS1
     NODELET_WARN(
 #else
     RCLCPP_WARN(this->get_logger(),
 #endif
-      "Invalid value for parameter '%s', defaulting to '%s'", kEtsiTypeParam.c_str(), kEtsiTypeParamDefault.c_str());
+      "Parameter '%s' is set to 'false', but multiple 'etsi_types' are configured, dropping all but the first", kHasBtpDestinationPortParam.c_str());
+    etsi_types_.resize(1);
   }
 }
 
@@ -148,23 +209,31 @@ void Converter::setup() {
   publishers_["cam"] = private_node_handle_.advertise<etsi_its_cam_msgs::CAM>(kOutputTopicCam, 1);
   publishers_["denm"] = private_node_handle_.advertise<etsi_its_denm_msgs::DENM>(kOutputTopicDenm, 1);
   subscriber_udp_ = private_node_handle_.subscribe(kInputTopicUdp, 1, &Converter::udpCallback, this);
-  subscribers_["cam"] = private_node_handle_.subscribe(kInputTopicCam, 1, &Converter::rosCallbackCam, this);
-  subscribers_["denm"] = private_node_handle_.subscribe(kInputTopicDenm, 1, &Converter::rosCallbackDenm, this);
-  NODELET_INFO("Converting UDP messages of type '%s' on '%s' to native ROS messages on '%s'", etsi_type_.c_str(), subscriber_udp_.getTopic().c_str(), publishers_["cam"].getTopic().c_str());
-  NODELET_INFO("Converting native ROS CAM messages on '%s' to UDP messages on '%s'", subscribers_["cam"].getTopic().c_str(), publisher_udp_.getTopic().c_str());
-  NODELET_INFO("Converting UDP messages of type '%s' on '%s' to native ROS messages on '%s'", etsi_type_.c_str(), subscriber_udp_.getTopic().c_str(), publishers_["denm"].getTopic().c_str());
-  NODELET_INFO("Converting native ROS DENM messages on '%s' to UDP messages on '%s'", subscribers_["denm"].getTopic().c_str(), publisher_udp_.getTopic().c_str());
+  if (std::find(etsi_types_.begin(), etsi_types_.end(), "cam") != etsi_types_.end()) {
+    subscribers_["cam"] = private_node_handle_.subscribe(kInputTopicCam, 1, &Converter::rosCallbackCam, this);
+    NODELET_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());
+    NODELET_INFO("Converting native ROS CAMs on '%s' to UDP messages on '%s'", subscribers_["cam"].getTopic().c_str(), publisher_udp_.getTopic().c_str());
+  }
+  if (std::find(etsi_types_.begin(), etsi_types_.end(), "denm") != etsi_types_.end()) {
+    subscribers_["denm"] = private_node_handle_.subscribe(kInputTopicDenm, 1, &Converter::rosCallbackDenm, this);
+    NODELET_INFO("Converting UDP messages of type DENM on '%s' to native ROS messages on '%s'", subscriber_udp_.getTopic().c_str(), publishers_["denm"].getTopic().c_str());
+    NODELET_INFO("Converting native ROS DENMs on '%s' to UDP messages on '%s'", subscribers_["denm"].getTopic().c_str(), publisher_udp_.getTopic().c_str());
+  }
 #else
   publisher_udp_ = this->create_publisher<udp_msgs::msg::UdpPacket>(kOutputTopicUdp, 1);
   publishers_cam_["cam"] = this->create_publisher<etsi_its_cam_msgs::msg::CAM>(kOutputTopicCam, 1);
   publishers_denm_["denm"] = this->create_publisher<etsi_its_denm_msgs::msg::DENM>(kOutputTopicDenm, 1);
   subscriber_udp_ = this->create_subscription<udp_msgs::msg::UdpPacket>(kInputTopicUdp, 1, std::bind(&Converter::udpCallback, this, std::placeholders::_1));
-  subscribers_cam_["cam"] = this->create_subscription<etsi_its_cam_msgs::msg::CAM>(kInputTopicCam, 1, std::bind(&Converter::rosCallbackCam, this, std::placeholders::_1));
-  subscribers_denm_["denm"] = this->create_subscription<etsi_its_denm_msgs::msg::DENM>(kInputTopicDenm, 1, std::bind(&Converter::rosCallbackDenm, this, std::placeholders::_1));
-  RCLCPP_INFO(this->get_logger(), "Converting UDP messages of type '%s' on '%s' to native ROS messages on '%s'", etsi_type_.c_str(), subscriber_udp_->get_topic_name(), publishers_cam_["cam"]->get_topic_name());
-  RCLCPP_INFO(this->get_logger(), "Converting native ROS CAM messages on '%s' to UDP messages on '%s'", subscribers_cam_["cam"]->get_topic_name(), publisher_udp_->get_topic_name());
-  RCLCPP_INFO(this->get_logger(), "Converting UDP messages of type '%s' on '%s' to native ROS messages on '%s'", etsi_type_.c_str(), subscriber_udp_->get_topic_name(), publishers_denm_["denm"]->get_topic_name());
-  RCLCPP_INFO(this->get_logger(), "Converting native ROS DENM messages on '%s' to UDP messages on '%s'", subscribers_denm_["denm"]->get_topic_name(), publisher_udp_->get_topic_name());
+  if (std::find(etsi_types_.begin(), etsi_types_.end(), "cam") != etsi_types_.end()) {
+    subscribers_cam_["cam"] = this->create_subscription<etsi_its_cam_msgs::msg::CAM>(kInputTopicCam, 1, std::bind(&Converter::rosCallbackCam, this, std::placeholders::_1));
+    RCLCPP_INFO(this->get_logger(), "Converting UDP messages of type CAM on '%s' to native ROS messages on '%s'", subscriber_udp_->get_topic_name(), publishers_cam_["cam"]->get_topic_name());
+    RCLCPP_INFO(this->get_logger(), "Converting native ROS CAMs on '%s' to UDP messages on '%s'", subscribers_cam_["cam"]->get_topic_name(), publisher_udp_->get_topic_name());
+  }
+  if (std::find(etsi_types_.begin(), etsi_types_.end(), "denm") != etsi_types_.end()) {
+    subscribers_denm_["denm"] = this->create_subscription<etsi_its_denm_msgs::msg::DENM>(kInputTopicDenm, 1, std::bind(&Converter::rosCallbackDenm, this, std::placeholders::_1));
+    RCLCPP_INFO(this->get_logger(), "Converting UDP messages of type DENM on '%s' to native ROS messages on '%s'", subscriber_udp_->get_topic_name(), publishers_denm_["denm"]->get_topic_name());
+    RCLCPP_INFO(this->get_logger(), "Converting native ROS DENMs on '%s' to UDP messages on '%s'", subscribers_denm_["denm"]->get_topic_name(), publisher_udp_->get_topic_name());
+  }
 #endif
 }
 
@@ -182,13 +251,11 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
 #endif
     "Received bitstring");
 
-  // decode BTP-Header, if type detection is enabled
-  std::string detected_etsi_type = etsi_type_;
-  int offset = 0;
-  if (etsi_type_ == "auto") {
-    offset = 4;
-    const uint16_t* btp_header = reinterpret_cast<const uint16_t*>(&udp_msg->data[0]);
-    uint16_t destination_port = ntohs(btp_header[0]);
+  // auto-detect ETSI message type if BTP destination port is present
+  std::string detected_etsi_type = etsi_types_[0];
+  if (has_btp_destination_port_) {
+    const uint16_t* btp_destination_port = reinterpret_cast<const uint16_t*>(&udp_msg->data[btp_destination_port_offset_]);
+    uint16_t destination_port = ntohs(*btp_destination_port);
     if (destination_port == kBtpHeaderDestinationPortCam) detected_etsi_type = "cam";
     else if (destination_port == kBtpHeaderDestinationPortDenm) detected_etsi_type = "denm";
     else if (destination_port == kBtpHeaderDestinationPortMap) detected_etsi_type = "map";
@@ -198,11 +265,19 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
     else detected_etsi_type = "unknown";
   }
 
+  int msg_size = udp_msg->data.size() - etsi_message_payload_offset_;
+#ifdef ROS1
+  NODELET_INFO(
+#else
+  RCLCPP_INFO(this->get_logger(),
+#endif
+    "Received ETSI message of type '%s' (message size: %d | total payload size: %ld)", detected_etsi_type.c_str(), msg_size, udp_msg->data.size());
+
   if (detected_etsi_type == "cam") {
 
     // decode ASN1 bitstring to struct
     CAM_t* asn1_struct = nullptr;
-    asn_dec_rval_t ret = asn_decode(0, ATS_UNALIGNED_BASIC_PER, &asn_DEF_CAM, (void **)&asn1_struct, &udp_msg->data[offset], udp_msg->data.size() - offset);
+    asn_dec_rval_t ret = asn_decode(0, ATS_UNALIGNED_BASIC_PER, &asn_DEF_CAM, (void **)&asn1_struct, &udp_msg->data[etsi_message_payload_offset_], msg_size);
     if (ret.code != RC_OK) {
 #ifdef ROS1
       NODELET_ERROR(
@@ -225,18 +300,15 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
     // publish msg
 #ifdef ROS1
     publishers_["cam"].publish(msg);
-    NODELET_DEBUG(
 #else
     publishers_cam_["cam"]->publish(msg);
-    RCLCPP_DEBUG(this->get_logger(),
 #endif
-      "Published CAM");
 
   } else if (detected_etsi_type == "denm") {
 
     // decode ASN1 bitstring to struct
     DENM_t* asn1_struct = nullptr;
-    asn_dec_rval_t ret = asn_decode(0, ATS_UNALIGNED_BASIC_PER, &asn_DEF_DENM, (void **)&asn1_struct, &udp_msg->data[offset], udp_msg->data.size() - offset);
+    asn_dec_rval_t ret = asn_decode(0, ATS_UNALIGNED_BASIC_PER, &asn_DEF_DENM, (void **)&asn1_struct, &udp_msg->data[etsi_message_payload_offset_], msg_size);
     if (ret.code != RC_OK) {
 #ifdef ROS1
       NODELET_ERROR(
@@ -259,12 +331,9 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
     // publish msg
 #ifdef ROS1
     publishers_["denm"].publish(msg);
-    NODELET_DEBUG(
 #else
     publishers_denm_["denm"]->publish(msg);
-    RCLCPP_DEBUG(this->get_logger(),
 #endif
-      "Published DENM");
 
   } else {
 #ifdef ROS1
@@ -275,6 +344,13 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
       "Detected ETSI message type '%s' not yet supported, dropping message", detected_etsi_type.c_str());
     return;
   }
+
+#ifdef ROS1
+    NODELET_INFO(
+#else
+    RCLCPP_INFO(this->get_logger(),
+#endif
+      "Published ETSI message of type '%s' as ROS message", detected_etsi_type.c_str());
 }
 
 
@@ -326,8 +402,8 @@ void Converter::rosCallbackCam(const etsi_its_cam_msgs::msg::CAM::UniquePtr msg)
 #else
   udp_msgs::msg::UdpPacket udp_msg;
 #endif
-  if (etsi_type_ == "auto") {
-    // add BTP-Header, if type detection is enabled
+  if (has_btp_destination_port_) {
+    // add BTP destination port and destination port info
     uint16_t destination_port = htons(kBtpHeaderDestinationPortCam);
     uint16_t destination_port_info = 0;
     uint16_t* btp_header = new uint16_t[2] {destination_port, destination_port_info};
@@ -397,7 +473,7 @@ void Converter::rosCallbackDenm(const etsi_its_denm_msgs::msg::DENM::UniquePtr m
 #else
   udp_msgs::msg::UdpPacket udp_msg;
 #endif
-  if (etsi_type_ == "auto") {
+  if (has_btp_destination_port_) {
     // add BTP-Header, if type detection is enabled
     uint16_t destination_port = htons(kBtpHeaderDestinationPortDenm);
     uint16_t destination_port_info = 0;