Skip to content

Commit

Permalink
add support vor CAM TS to conversion node (handle protocol version)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jpbusch
jpbusch committed Jul 11, 2024
1 parent ac1486f commit 5b8c0c5
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 11 deletions.
8 changes: 8 additions & 0 deletions etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,17 +29,20 @@ SOFTWARE.
#include <unordered_map>

#include <etsi_its_cam_conversion/convertCAM.h>
#include <etsi_its_cam_ts_conversion/convertCAM.h>
#include <etsi_its_cpm_ts_conversion/convertCollectivePerceptionMessage.h>
#include <etsi_its_denm_conversion/convertDENM.h>
#ifdef ROS1
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <udp_msgs/UdpPacket.h>
#include <etsi_its_cam_msgs/CAM.h>
#include <etsi_its_cam_ts_msgs/CAM.h>
#include <etsi_its_cpm_ts_msgs/CollectivePerceptionMessage.h>
#include <etsi_its_denm_msgs/DENM.h>
#else
#include <etsi_its_cam_msgs/msg/cam.hpp>
#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
#include <etsi_its_cpm_ts_msgs/msg/collective_perception_message.hpp>
#include <etsi_its_denm_msgs/msg/denm.hpp>
#include <rclcpp/rclcpp.hpp>
Expand All @@ -53,11 +56,13 @@ namespace etsi_its_conversion {
#ifdef ROS1
using namespace udp_msgs;
namespace cam_msgs = etsi_its_cam_msgs;
namespace cam_ts_msgs = etsi_its_cam_ts_msgs;
namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs;
namespace denm_msgs = etsi_its_denm_msgs;
#else
using namespace udp_msgs::msg;
namespace cam_msgs = etsi_its_cam_msgs::msg;
namespace cam_ts_msgs = etsi_its_cam_ts_msgs::msg;
namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs::msg;
namespace denm_msgs = etsi_its_denm_msgs::msg;
#endif
Expand Down Expand Up @@ -125,6 +130,8 @@ class Converter : public rclcpp::Node {
static const std::string kOutputTopicUdp;
static const std::string kInputTopicCam;
static const std::string kOutputTopicCam;
static const std::string kInputTopicCamTs;
static const std::string kOutputTopicCamTs;
static const std::string kInputTopicCpmTs;
static const std::string kOutputTopicCpmTs;
static const std::string kInputTopicDenm;
Expand Down Expand Up @@ -160,6 +167,7 @@ class Converter : public rclcpp::Node {
rclcpp::Subscription<UdpPacket>::SharedPtr subscriber_udp_;
std::unordered_map<std::string, rclcpp::SubscriptionBase::SharedPtr> subscribers_;
rclcpp::Publisher<cam_msgs::CAM>::SharedPtr publisher_cam_;
rclcpp::Publisher<cam_ts_msgs::CAM>::SharedPtr publisher_cam_ts_;
rclcpp::Publisher<cpm_ts_msgs::CollectivePerceptionMessage>::SharedPtr publisher_cpm_ts_;
rclcpp::Publisher<denm_msgs::DENM>::SharedPtr publisher_denm_;
rclcpp::Publisher<UdpPacket>::SharedPtr publisher_udp_;
Expand Down
39 changes: 28 additions & 11 deletions etsi_its_conversion/etsi_its_conversion/src/Converter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,18 +55,22 @@ const int kBtpHeaderDestinationPortCpmTs{2009};

#ifdef ROS1
const std::string Converter::kInputTopicUdp{"udp/in"};
const std::string Converter::kOutputTopicUdp{"udp/in"};
const std::string Converter::kOutputTopicUdp{"udp/out"};
const std::string Converter::kInputTopicCam{"cam/in"};
const std::string Converter::kOutputTopicCam{"cam/out"};
const std::string Converter::kInputTopicCamTs{"cam_ts/in"};
const std::string Converter::kOutputTopicCamTs{"cam_ts/out"};
const std::string Converter::kInputTopicCpmTs{"cpm_ts/in"};
const std::string Converter::kOutputTopicCpmTs{"cpm_ts/out"};
const std::string Converter::kInputTopicDenm{"denm/in"};
const std::string Converter::kOutputTopicDenm{"denm/out"};
#else
const std::string Converter::kInputTopicUdp{"~/udp/in"};
const std::string Converter::kOutputTopicUdp{"~/udp/in"};
const std::string Converter::kOutputTopicUdp{"~/udp/out"};
const std::string Converter::kInputTopicCam{"~/cam/in"};
const std::string Converter::kOutputTopicCam{"~/cam/out"};
const std::string Converter::kInputTopicCamTs{"~/cam_ts/in"};
const std::string Converter::kOutputTopicCamTs{"~/cam_ts/out"};
const std::string Converter::kInputTopicCpmTs{"~/cpm_ts/in"};
const std::string Converter::kOutputTopicCpmTs{"~/cpm_ts/out"};
const std::string Converter::kInputTopicDenm{"~/denm/in"};
Expand Down Expand Up @@ -414,22 +418,35 @@ void Converter::udpCallback(const UdpPacket::UniquePtr udp_msg) {
else detected_etsi_type = "unknown";
}

const uint8_t* protocol_version = reinterpret_cast<const uint8_t*>(&udp_msg->data[etsi_message_payload_offset_]);

int msg_size = udp_msg->data.size() - etsi_message_payload_offset_;
ROS12_LOG(INFO, "Received ETSI message of type '%s' as bitstring (message size: %d | total payload size: %ld)", detected_etsi_type.c_str(), msg_size, udp_msg->data.size());
ROS12_LOG(INFO, "Received ETSI message of type '%s' (protocolVersion: %d) as bitstring (message size: %d | total payload size: %ld)", detected_etsi_type.c_str(), *protocol_version , msg_size, udp_msg->data.size());

if (detected_etsi_type == "cam") {

// decode buffer to ROS msg
cam_msgs::CAM msg;
bool success = this->decodeBufferToRosMessage(&udp_msg->data[etsi_message_payload_offset_], msg_size, &asn_DEF_cam_CAM, std::function<void(const cam_CAM_t&, cam_msgs::CAM&)>(etsi_its_cam_conversion::toRos_CAM), msg);
if (!success) return;

// publish msg
if (*protocol_version == 2) { // CAM EN v1.4.1
cam_msgs::CAM msg;
bool success = this->decodeBufferToRosMessage(&udp_msg->data[etsi_message_payload_offset_], msg_size, &asn_DEF_cam_CAM, std::function<void(const cam_CAM_t&, cam_msgs::CAM&)>(etsi_its_cam_conversion::toRos_CAM), msg);
if (!success) return;
#ifdef ROS1
publishers_["cam"]->publish(msg);
#else
publisher_cam_->publish(msg);
#endif
} else if (*protocol_version == 3) { // CAM TS v2.1.1
cam_ts_msgs::CAM msg;
bool success = this->decodeBufferToRosMessage(&udp_msg->data[etsi_message_payload_offset_], msg_size, &asn_DEF_cam_ts_CAM, std::function<void(const cam_ts_CAM_t&, cam_ts_msgs::CAM&)>(etsi_its_cam_ts_conversion::toRos_CAM), msg);
if (!success) return;
#ifdef ROS1
publishers_["cam"]->publish(msg);
publishers_["cam_ts"]->publish(msg);
#else
publisher_cam_->publish(msg);
publisher_cam_ts_->publish(msg);
#endif
} else {
ROS12_LOG(ERROR, "Detected ETSI message type 'cam' with unknown protocol version %d, dropping message", *protocol_version);
return;
}

} else if (detected_etsi_type == "cpm_ts") {

Expand Down

0 comments on commit 5b8c0c5

Please sign in to comment.