diff --git a/README.md b/README.md index b228a12c7..980688d1b 100644 --- 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 index 3b02b21f6..44b7e830b 100644 --- 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 index 1e8659b79..f94fd0c0c 100644 --- 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 index 3676a199a..c8ce8638f 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 @@ -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 kEtsiTypesParamDefault; + int incoming_payload_offset_; bool has_btp_header_; std::vector etsi_types_; diff --git a/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp b/etsi_its_conversion/etsi_its_conversion/src/Converter.cpp index 0718a2111..522b7a275 100644 --- 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(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(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(&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(udp_msg->data.size()))); if (detected_etsi_type == "cam") {