29 #include <unordered_map>
43 #include <udp_msgs/UdpPacket.h>
44 #include <etsi_its_cam_msgs/CAM.h>
45 #include <etsi_its_cam_ts_msgs/CAM.h>
46 #include <etsi_its_cpm_ts_msgs/CollectivePerceptionMessage.h>
47 #include <etsi_its_denm_msgs/DENM.h>
48 #include <etsi_its_denm_ts_msgs/DENM.h>
49 #include <etsi_its_mapem_ts_msgs/MAPEM.h>
50 #include <etsi_its_mcm_uulm_msgs/MCM.h>
51 #include <etsi_its_spatem_ts_msgs/SPATEM.h>
52 #include <etsi_its_vam_ts_msgs/VAM.h>
54 #include <etsi_its_cam_msgs/msg/cam.hpp>
55 #include <etsi_its_cam_ts_msgs/msg/cam.hpp>
56 #include <etsi_its_cpm_ts_msgs/msg/collective_perception_message.hpp>
57 #include <etsi_its_denm_msgs/msg/denm.hpp>
58 #include <etsi_its_denm_ts_msgs/msg/denm.hpp>
59 #include <etsi_its_mapem_ts_msgs/msg/mapem.hpp>
60 #include <etsi_its_mcm_uulm_msgs/msg/mcm.hpp>
61 #include <etsi_its_spatem_ts_msgs/msg/spatem.hpp>
62 #include <etsi_its_vam_ts_msgs/msg/vam.hpp>
63 #include <rclcpp/rclcpp.hpp>
64 #include <udp_msgs/msg/udp_packet.hpp>
72 using namespace udp_msgs;
73 namespace cam_msgs = etsi_its_cam_msgs;
74 namespace cam_ts_msgs = etsi_its_cam_ts_msgs;
75 namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs;
76 namespace denm_msgs = etsi_its_denm_msgs;
77 namespace denm_ts_msgs = etsi_its_denm_ts_msgs;
78 namespace mapem_ts_msgs = etsi_its_mapem_ts_msgs;
79 namespace mcm_uulm_msgs = etsi_its_mcm_uulm_msgs;
80 namespace spatem_ts_msgs = etsi_its_spatem_ts_msgs;
81 namespace vam_ts_msgs = etsi_its_vam_ts_msgs;
83 using namespace udp_msgs::msg;
84 namespace cam_msgs = etsi_its_cam_msgs::msg;
85 namespace cam_ts_msgs = etsi_its_cam_ts_msgs::msg;
86 namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs::msg;
87 namespace denm_msgs = etsi_its_denm_msgs::msg;
88 namespace denm_ts_msgs = etsi_its_denm_ts_msgs::msg;
89 namespace mapem_ts_msgs = etsi_its_mapem_ts_msgs::msg;
90 namespace mcm_uulm_msgs = etsi_its_mcm_uulm_msgs::msg;
91 namespace spatem_ts_msgs = etsi_its_spatem_ts_msgs::msg;
92 namespace vam_ts_msgs = etsi_its_vam_ts_msgs::msg;
105 virtual void onInit();
107 explicit Converter(
const rclcpp::NodeOptions& options);
112 void loadParameters();
116 bool logLevelIsDebug();
118 template <
typename T_struct>
119 bool decodeBufferToStruct(
const uint8_t* buffer,
const int size,
const asn_TYPE_descriptor_t* type_descriptor, T_struct* asn1_struct);
121 template <
typename T_ros,
typename T_struct>
122 T_ros structToRosMessage(
const T_struct& asn1_struct,
const asn_TYPE_descriptor_t* type_descriptor, std::function<
void(
const T_struct&, T_ros&)> conversion_fn);
124 template <
typename T_ros,
typename T_struct>
125 bool decodeBufferToRosMessage(
const uint8_t* buffer,
const int size,
const asn_TYPE_descriptor_t* type_descriptor, std::function<
void(
const T_struct&, T_ros&)> conversion_fn, T_ros& msg);
127 UdpPacket bufferToUdpPacketMessage(
const uint8_t* buffer,
const int size,
const int btp_header_destination_port);
129 template <
typename T_ros,
typename T_struct>
130 T_struct rosMessageToStruct(
const T_ros& msg,
const asn_TYPE_descriptor_t* type_descriptor, std::function<
void(
const T_ros&, T_struct&)> conversion_fn);
132 template <
typename T_struct>
133 bool encodeStructToBuffer(
const T_struct& asn1_struct,
const asn_TYPE_descriptor_t* type_descriptor, uint8_t*& buffer,
int& size);
135 template <
typename T_ros,
typename T_struct>
136 bool encodeRosMessageToUdpPacketMessage(
const T_ros& msg, UdpPacket& udp_msg,
const asn_TYPE_descriptor_t* type_descriptor, std::function<
void(
const T_ros&, T_struct&)> conversion_fn,
const int btp_header_destination_port);
139 void udpCallback(
const UdpPacket::ConstPtr udp_msg);
141 void udpCallback(
const UdpPacket::UniquePtr udp_msg);
144 template <
typename T_ros,
typename T_struct>
146 void rosCallback(
const typename T_ros::ConstPtr msg,
148 void rosCallback(
const typename T_ros::UniquePtr msg,
150 const std::string& type,
const asn_TYPE_descriptor_t* type_descriptor, std::function<
void(
const T_ros&, T_struct&)> conversion_fn);
203 std::shared_ptr<ros::Subscriber> subscriber_udp_;
204 std::unordered_map<std::string, std::shared_ptr<ros::Publisher>> publishers_;
205 std::unordered_map<std::string, std::shared_ptr<ros::Subscriber>> subscribers_;
206 std::shared_ptr<ros::Publisher> publisher_udp_;
209 std::unordered_map<std::string, rclcpp::SubscriptionBase::SharedPtr>
subscribers_;