add param to set payload offset in converter node

ika-rwth-aachen · Dec 7, 2023 · 22f0241 · 22f0241
1 parent 079f751
commit 22f0241
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 4 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 has_btp_header:=true -p etsi_types:=[cam,denm]
+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
 
 # ROS
 roslaunch etsi_its_conversion converter.ros1.launch
 # or
-rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_types:=[cam,denm]
+rosrun nodelet nodelet standalone etsi_its_conversion/Converter _etsi_types:=[cam,denm] _has_btp_header:=true _incoming_payload_offset:=0
 ```
 
 ##### Subscribed Topics
@@ -177,6 +177,7 @@ 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) |
 | `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,3 +1,4 @@
+incoming_payload_offset: 0
 has_btp_header: true
 etsi_types:
   - cam

diff --git a/etsi_its_conversion/etsi_its_conversion/config/params.yml b/etsi_its_conversion/etsi_its_conversion/config/params.yml
@@ -1,5 +1,6 @@
 etsi_its_conversion:
   ros__parameters:
+    incoming_payload_offset: 0
     has_btp_header: true
     etsi_types:
       - cam

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,11 +92,14 @@ 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 kEtsiTypesParam;
     static const std::vector<std::string> kEtsiTypesParamDefault;
 
+    int incoming_payload_offset_;
     bool has_btp_header_;
     std::vector<std::string> etsi_types_;
 

diff --git a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp
@@ -68,6 +68,8 @@ 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::kEtsiTypesParam{"etsi_types"};
@@ -116,6 +118,20 @@ void Converter::loadParameters() {
   rcl_interfaces::msg::ParameterDescriptor param_desc;
 #endif
 
+  // load incoming_payload_offset
+#ifdef ROS1
+  if (!private_node_handle_.param<int>(kIncomingPayloadOffset, incoming_payload_offset_, kIncomingPayloadOffsetDefault)) {
+    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_)) {
+    RCLCPP_WARN(this->get_logger(),
+#endif
+      "Parameter '%s' is not set, defaulting to '%d'", kIncomingPayloadOffset.c_str(), kIncomingPayloadOffsetDefault);
+  }
+
   // load has_btp_header
 #ifdef ROS1
   if (!private_node_handle_.param<bool>(kHasBtpHeaderParam, has_btp_header_, kHasBtpHeaderParamDefault)) {
@@ -221,9 +237,9 @@ void Converter::udpCallback(const udp_msgs::msg::UdpPacket::UniquePtr udp_msg) {
 
   // decode BTP-Header if enabled
   std::string detected_etsi_type = etsi_types_[0];
-  int offset = 0;
+  int offset = incoming_payload_offset_;
   if (has_btp_header_) {
-    offset = 4;
+    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 (destination_port == kBtpHeaderDestinationPortCam) detected_etsi_type = "cam";
@@ -234,6 +250,7 @@ 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") {