enable to configure offset before btp destination port and payload in…

…dividually
ika-rwth-aachen · Dec 20, 2023 · 024c644 · 024c644
1 parent 22f0241
commit 024c644
Show file tree

Hide file tree

Showing 5 changed files with 62 additions and 43 deletions.
diff --git a/README.md b/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_types:=[cam,denm] -p has_btp_header:=true -p incoming_payload_offset:=0
+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:=0 -p etsi_message_payload_offset:=4
 
 # ROS
 roslaunch etsi_its_conversion converter.ros1.launch
 # or
-rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_types:=[cam,denm] _has_btp_header:=true _incoming_payload_offset:=0
+rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_types:=[cam,denm] _has_btp_destination_port:=true _btp_destination_port_offset:=0 _etsi_message_payload_offset:=4
 ```
 
 ##### Subscribed Topics
@@ -177,8 +177,9 @@ rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_types:=[ca
 
 | Parameter | Type | Description | Options |
 | --- | --- | --- | --- |
-| `incoming_payload_offset` | `int` | number of bytes until actual message payload starts in incoming UDP message (optionally including BTP header, see `has_btp_header`) |
-| `has_btp_header` | `bool` | whether incoming/outgoing UDP messages include a [4-byte BTP header](https://www.etsi.org/deliver/etsi_en/302600_302699/3026360501/02.01.00_20/en_3026360501v020100a.pdf) |
+| `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` |
 
 

diff --git a/etsi_its_conversion/etsi_its_conversion/config/params.ros1.yml b/etsi_its_conversion/etsi_its_conversion/config/params.ros1.yml
@@ -1,5 +1,6 @@
-incoming_payload_offset: 0
-has_btp_header: true
+has_btp_destination_port: true
+btp_destination_port_offset: 0
+etsi_message_payload_offset: 4
 etsi_types:
   - cam
   - denm
diff --git a/etsi_its_conversion/etsi_its_conversion/config/params.yml b/etsi_its_conversion/etsi_its_conversion/config/params.yml
@@ -1,7 +1,8 @@
 etsi_its_conversion:
   ros__parameters:
-    incoming_payload_offset: 0
-    has_btp_header: true
+    has_btp_destination_port: true
+    btp_destination_port_offset: 0
+    etsi_message_payload_offset: 4
     etsi_types:
       - cam
       - denm
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
@@ -92,15 +92,18 @@ class Converter : public rclcpp::Node {
     static const std::string kInputTopicDenm;
     static const std::string kOutputTopicDenm;
 
-    static const std::string kIncomingPayloadOffset;
-    static const int kIncomingPayloadOffsetDefault;
-    static const std::string kHasBtpHeaderParam;
-    static const bool kHasBtpHeaderParamDefault;
+    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;
 
-    int incoming_payload_offset_;
-    bool has_btp_header_;
+    bool has_btp_destination_port_;
+    int btp_destination_port_offset_;
+    int etsi_message_payload_offset_;
     std::vector<std::string> etsi_types_;
 
 #ifdef ROS1

diff --git a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp
@@ -68,10 +68,12 @@ const std::string Converter::kInputTopicDenm{"~/denm/in"};
 const std::string Converter::kOutputTopicDenm{"~/denm/out"};
 #endif
 
-const std::string Converter::kIncomingPayloadOffset{"incoming_payload_offset"};
-const int Converter::kIncomingPayloadOffsetDefault{0};
-const std::string Converter::kHasBtpHeaderParam{"has_btp_header"};
-const bool Converter::kHasBtpHeaderParamDefault{true};
+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{0};
+const std::string Converter::kEtsiMessagePayloadOffsetParam{"etsi_message_payload_offset"};
+const int Converter::kEtsiMessagePayloadOffsetParamDefault{4};
 const std::string Converter::kEtsiTypesParam{"etsi_types"};
 const std::vector<std::string> Converter::kEtsiTypesParamDefault{"cam", "denm"};
 
@@ -118,32 +120,46 @@ void Converter::loadParameters() {
   rcl_interfaces::msg::ParameterDescriptor param_desc;
 #endif
 
-  // load incoming_payload_offset
+  // load has_btp_destination_port
 #ifdef ROS1
-  if (!private_node_handle_.param<int>(kIncomingPayloadOffset, incoming_payload_offset_, kIncomingPayloadOffsetDefault)) {
+  if (!private_node_handle_.param<bool>(kHasBtpDestinationPortParam, has_btp_destination_port_, kHasBtpDestinationPortParamDefault)) {
+    NODELET_WARN(
+#else
+  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(kIncomingPayloadOffset, kIncomingPayloadOffsetDefault, param_desc);
-  if (!this->get_parameter(kIncomingPayloadOffset, incoming_payload_offset_)) {
+  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'", kIncomingPayloadOffset.c_str(), kIncomingPayloadOffsetDefault);
+      "Parameter '%s' is not set, defaulting to '%d'", kBtpDestinationPortOffsetParam.c_str(), kBtpDestinationPortOffsetParamDefault);
   }
 
-  // load has_btp_header
+  // load etsi_message_payload_offset
 #ifdef ROS1
-  if (!private_node_handle_.param<bool>(kHasBtpHeaderParam, has_btp_header_, kHasBtpHeaderParamDefault)) {
+  if (!private_node_handle_.param<int>(kEtsiMessagePayloadOffsetParam, etsi_message_payload_offset_, kEtsiMessagePayloadOffsetParamDefault)) {
     NODELET_WARN(
 #else
   param_desc = rcl_interfaces::msg::ParameterDescriptor();
-  param_desc.description = "whether incoming/outgoing UDP messages include a 4-byte BTP header";
-  this->declare_parameter(kHasBtpHeaderParam, kHasBtpHeaderParamDefault, param_desc);
-  if (!this->get_parameter(kHasBtpHeaderParam, has_btp_header_)) {
+  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 '%s'", kHasBtpHeaderParam.c_str(), kHasBtpHeaderParamDefault ? "true" : "false");
+      "Parameter '%s' is not set, defaulting to '%d'", kEtsiMessagePayloadOffsetParam.c_str(), kEtsiMessagePayloadOffsetParamDefault);
   }
 
   // load etsi_types
@@ -173,13 +189,13 @@ void Converter::loadParameters() {
 #endif
         "Invalid value '%s' for parameter '%s', skipping", e.c_str(), kEtsiTypesParam.c_str());
   }
-  if (!has_btp_header_ && etsi_types_.size() > 1) {
+  if (!has_btp_destination_port_ && etsi_types_.size() > 1) {
 #ifdef ROS1
     NODELET_WARN(
 #else
     RCLCPP_WARN(this->get_logger(),
 #endif
-      "Parameter '%s' is set to 'false', but multiple 'etsi_types' are configured, dropping all but the first", kHasBtpHeaderParam.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);
   }
 }
@@ -235,13 +251,11 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
 #endif
     "Received bitstring");
 
-  // decode BTP-Header if enabled
+  // auto-detect ETSI message type if BTP destination port is present
   std::string detected_etsi_type = etsi_types_[0];
-  int offset = incoming_payload_offset_;
-  if (has_btp_header_) {
-    offset += 4;
-    const uint16_t* btp_header = reinterpret_cast<const uint16_t*>(&udp_msg->data[0]);
-    uint16_t destination_port = ntohs(btp_header[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";
@@ -250,13 +264,12 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
     else if (destination_port == kBtpHeaderDestinationPortCpm) detected_etsi_type = "cpm";
     else detected_etsi_type = "unknown";
   }
-  offset = std::min(0, std::max(offset, static_cast<int>(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_], udp_msg->data.size() - etsi_message_payload_offset_);
     if (ret.code != RC_OK) {
 #ifdef ROS1
       NODELET_ERROR(
@@ -290,7 +303,7 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
 
     // 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_], udp_msg->data.size() - etsi_message_payload_offset_);
     if (ret.code != RC_OK) {
 #ifdef ROS1
       NODELET_ERROR(
@@ -380,8 +393,8 @@ void Converter::rosCallbackCam(const etsi_its_cam_msgs::msg::CAM::UniquePtr msg)
 #else
   udp_msgs::msg::UdpPacket udp_msg;
 #endif
-  if (has_btp_header_) {
-    // 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};
@@ -451,7 +464,7 @@ void Converter::rosCallbackDenm(const etsi_its_denm_msgs::msg::DENM::UniquePtr m
 #else
   udp_msgs::msg::UdpPacket udp_msg;
 #endif
-  if (has_btp_header_) {
+  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;