Converter.hpp
Go to the documentation of this file.
1 
25 #pragma once
26 
27 #include <memory>
28 #include <string>
29 #include <unordered_map>
30 
40 #ifdef ROS1
41 #include <nodelet/nodelet.h>
42 #include <ros/ros.h>
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>
53 #else
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>
65 #endif
66 
67 
69 
70 
71 #ifdef ROS1
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;
82 #else
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;
93 #endif
94 
95 
96 #ifdef ROS1
97 class Converter : public nodelet::Nodelet {
98 #else
99 class Converter : public rclcpp::Node {
100 #endif
101 
102  public:
103 
104 #ifdef ROS1
105  virtual void onInit();
106 #else
107  explicit Converter(const rclcpp::NodeOptions& options);
108 #endif
109 
110  protected:
111 
112  void loadParameters();
113 
114  void setup();
115 
116  bool logLevelIsDebug();
117 
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);
120 
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);
123 
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);
126 
127  UdpPacket bufferToUdpPacketMessage(const uint8_t* buffer, const int size, const int btp_header_destination_port);
128 
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);
131 
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);
134 
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);
137 
138 #ifdef ROS1
139  void udpCallback(const UdpPacket::ConstPtr udp_msg);
140 #else
141  void udpCallback(const UdpPacket::UniquePtr udp_msg);
142 #endif
143 
144  template <typename T_ros, typename T_struct>
145 #ifdef ROS1
146  void rosCallback(const typename T_ros::ConstPtr msg,
147 #else
148  void rosCallback(const typename T_ros::UniquePtr msg,
149 #endif
150  const std::string& type, const asn_TYPE_descriptor_t* type_descriptor, std::function<void(const T_ros&, T_struct&)> conversion_fn);
151 
152  protected:
153 
154  static const std::string kInputTopicUdp;
155  static const std::string kOutputTopicUdp;
156  static const std::string kInputTopicCam;
157  static const std::string kOutputTopicCam;
158  static const std::string kInputTopicCamTs;
159  static const std::string kOutputTopicCamTs;
160  static const std::string kInputTopicCpmTs;
161  static const std::string kOutputTopicCpmTs;
162  static const std::string kInputTopicDenm;
163  static const std::string kOutputTopicDenm;
164  static const std::string kInputTopicDenmTs;
165  static const std::string kOutputTopicDenmTs;
166  static const std::string kInputTopicMapemTs;
167  static const std::string kOutputTopicMapemTs;
168  static const std::string kInputTopicMcmUulm;
169  static const std::string kOutputTopicMcmUulm;
170  static const std::string kInputTopicSpatemTs;
171  static const std::string kOutputTopicSpatemTs;
172  static const std::string kInputTopicVamTs;
173  static const std::string kOutputTopicVamTs;
174 
175  static const std::string kHasBtpDestinationPortParam;
177  static const std::string kBtpDestinationPortOffsetParam;
179  static const std::string kEtsiMessagePayloadOffsetParam;
181  static const std::string kRos2UdpEtsiTypesParam;
182  static const std::string kUdp2RosEtsiTypesParam;
183  static const std::vector<std::string> kEtsiTypesParamSupportedOptions;
184  static const std::vector<std::string> kRos2UdpEtsiTypesParamDefault;
185  static const std::vector<std::string> kUdp2RosEtsiTypesParamDefault;
186  static const std::string kSubscriberQueueSizeParam;
188  static const std::string kPublisherQueueSizeParam;
190  static const std::string kCheckConstraintsBeforeEncodingParam;
192 
196  std::vector<std::string> ros2udp_etsi_types_;
197  std::vector<std::string> udp2ros_etsi_types_;
200 
201 #ifdef ROS1
202  ros::NodeHandle private_node_handle_;
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_;
207 #else
208  rclcpp::Subscription<UdpPacket>::SharedPtr subscriber_udp_;
209  std::unordered_map<std::string, rclcpp::SubscriptionBase::SharedPtr> subscribers_;
210  rclcpp::Publisher<cam_msgs::CAM>::SharedPtr publisher_cam_;
211  rclcpp::Publisher<cam_ts_msgs::CAM>::SharedPtr publisher_cam_ts_;
212  rclcpp::Publisher<cpm_ts_msgs::CollectivePerceptionMessage>::SharedPtr publisher_cpm_ts_;
213  rclcpp::Publisher<denm_msgs::DENM>::SharedPtr publisher_denm_;
214  rclcpp::Publisher<denm_ts_msgs::DENM>::SharedPtr publisher_denm_ts_;
215  rclcpp::Publisher<mapem_ts_msgs::MAPEM>::SharedPtr publisher_mapem_ts_;
216  rclcpp::Publisher<mcm_uulm_msgs::MCM>::SharedPtr publisher_mcm_uulm_;
217  rclcpp::Publisher<spatem_ts_msgs::SPATEM>::SharedPtr publisher_spatem_ts_;
218  rclcpp::Publisher<vam_ts_msgs::VAM>::SharedPtr publisher_vam_ts_;
219  rclcpp::Publisher<UdpPacket>::SharedPtr publisher_udp_;
220 #endif
221 
222 };
223 
224 
225 }
setup
convertSPATEM.h
etsi_its_conversion::Converter::kInputTopicCpmTs
static const std::string kInputTopicCpmTs
Definition: Converter.hpp:160
etsi_its_conversion::Converter::subscribers_
std::unordered_map< std::string, rclcpp::SubscriptionBase::SharedPtr > subscribers_
Definition: Converter.hpp:209
etsi_its_conversion::Converter::kOutputTopicMcmUulm
static const std::string kOutputTopicMcmUulm
Definition: Converter.hpp:169
etsi_its_conversion::Converter::has_btp_destination_port_
bool has_btp_destination_port_
Definition: Converter.hpp:193
etsi_its_conversion::Converter::kOutputTopicCamTs
static const std::string kOutputTopicCamTs
Definition: Converter.hpp:159
etsi_its_conversion::Converter::kEtsiMessagePayloadOffsetParamDefault
static const int kEtsiMessagePayloadOffsetParamDefault
Definition: Converter.hpp:180
etsi_its_conversion::Converter::kBtpDestinationPortOffsetParamDefault
static const int kBtpDestinationPortOffsetParamDefault
Definition: Converter.hpp:178
etsi_its_conversion::Converter::kEtsiTypesParamSupportedOptions
static const std::vector< std::string > kEtsiTypesParamSupportedOptions
Definition: Converter.hpp:183
etsi_its_conversion::Converter::kBtpDestinationPortOffsetParam
static const std::string kBtpDestinationPortOffsetParam
Definition: Converter.hpp:177
ros.h
convertMAPEM.h
etsi_its_conversion::Converter::kRos2UdpEtsiTypesParamDefault
static const std::vector< std::string > kRos2UdpEtsiTypesParamDefault
Definition: Converter.hpp:184
etsi_its_conversion::Converter::publisher_cam_
rclcpp::Publisher< cam_msgs::CAM >::SharedPtr publisher_cam_
Definition: Converter.hpp:210
etsi_its_conversion::Converter::kOutputTopicDenmTs
static const std::string kOutputTopicDenmTs
Definition: Converter.hpp:165
etsi_its_conversion::Converter::publisher_spatem_ts_
rclcpp::Publisher< spatem_ts_msgs::SPATEM >::SharedPtr publisher_spatem_ts_
Definition: Converter.hpp:217
etsi_its_conversion::Converter
Definition: Converter.hpp:99
etsi_its_conversion::Converter::publisher_udp_
rclcpp::Publisher< UdpPacket >::SharedPtr publisher_udp_
Definition: Converter.hpp:219
etsi_its_conversion::Converter::kPublisherQueueSizeParamDefault
static const int kPublisherQueueSizeParamDefault
Definition: Converter.hpp:189
etsi_its_conversion::Converter::publisher_denm_ts_
rclcpp::Publisher< denm_ts_msgs::DENM >::SharedPtr publisher_denm_ts_
Definition: Converter.hpp:214
convertCollectivePerceptionMessage.h
etsi_its_conversion::Converter::kInputTopicMcmUulm
static const std::string kInputTopicMcmUulm
Definition: Converter.hpp:168
etsi_its_conversion::Converter::etsi_message_payload_offset_
int etsi_message_payload_offset_
Definition: Converter.hpp:195
convertVAM.h
etsi_its_conversion::Converter::kRos2UdpEtsiTypesParam
static const std::string kRos2UdpEtsiTypesParam
Definition: Converter.hpp:181
etsi_its_conversion::Converter::publisher_cam_ts_
rclcpp::Publisher< cam_ts_msgs::CAM >::SharedPtr publisher_cam_ts_
Definition: Converter.hpp:211
asn_TYPE_descriptor_s
etsi_its_conversion::Converter::kOutputTopicUdp
static const std::string kOutputTopicUdp
Definition: Converter.hpp:155
etsi_its_conversion::Converter::subscriber_udp_
rclcpp::Subscription< UdpPacket >::SharedPtr subscriber_udp_
Definition: Converter.hpp:208
convertMCM.h
etsi_its_conversion::Converter::kSubscriberQueueSizeParam
static const std::string kSubscriberQueueSizeParam
Definition: Converter.hpp:186
etsi_its_conversion::Converter::kInputTopicUdp
static const std::string kInputTopicUdp
Definition: Converter.hpp:154
etsi_its_conversion::Converter::kCheckConstraintsBeforeEncodingParam
static const std::string kCheckConstraintsBeforeEncodingParam
Definition: Converter.hpp:190
etsi_its_conversion::Converter::kPublisherQueueSizeParam
static const std::string kPublisherQueueSizeParam
Definition: Converter.hpp:188
etsi_its_conversion::Converter::btp_destination_port_offset_
int btp_destination_port_offset_
Definition: Converter.hpp:194
etsi_its_conversion::Converter::kUdp2RosEtsiTypesParamDefault
static const std::vector< std::string > kUdp2RosEtsiTypesParamDefault
Definition: Converter.hpp:185
etsi_its_conversion::Converter::kOutputTopicCpmTs
static const std::string kOutputTopicCpmTs
Definition: Converter.hpp:161
etsi_its_conversion::Converter::kEtsiMessagePayloadOffsetParam
static const std::string kEtsiMessagePayloadOffsetParam
Definition: Converter.hpp:179
etsi_its_conversion::Converter::kOutputTopicSpatemTs
static const std::string kOutputTopicSpatemTs
Definition: Converter.hpp:171
etsi_its_conversion::Converter::ros2udp_etsi_types_
std::vector< std::string > ros2udp_etsi_types_
Definition: Converter.hpp:196
etsi_its_conversion::Converter::publisher_cpm_ts_
rclcpp::Publisher< cpm_ts_msgs::CollectivePerceptionMessage >::SharedPtr publisher_cpm_ts_
Definition: Converter.hpp:212
etsi_its_conversion::Converter::kHasBtpDestinationPortParam
static const std::string kHasBtpDestinationPortParam
Definition: Converter.hpp:175
etsi_its_conversion::Converter::kCheckConstraintsBeforeEncodingParamDefault
static const bool kCheckConstraintsBeforeEncodingParamDefault
Definition: Converter.hpp:191
etsi_its_conversion
Definition: Converter.hpp:68
etsi_its_conversion::Converter::publisher_mapem_ts_
rclcpp::Publisher< mapem_ts_msgs::MAPEM >::SharedPtr publisher_mapem_ts_
Definition: Converter.hpp:215
nodelet::Nodelet
etsi_its_conversion::Converter::kOutputTopicCam
static const std::string kOutputTopicCam
Definition: Converter.hpp:157
convertDENM.h
etsi_its_conversion::Converter::publisher_queue_size_
int publisher_queue_size_
Definition: Converter.hpp:199
etsi_its_conversion::Converter::kOutputTopicVamTs
static const std::string kOutputTopicVamTs
Definition: Converter.hpp:173
nodelet.h
etsi_its_conversion::Converter::publisher_denm_
rclcpp::Publisher< denm_msgs::DENM >::SharedPtr publisher_denm_
Definition: Converter.hpp:213
etsi_its_conversion::Converter::subscriber_queue_size_
int subscriber_queue_size_
Definition: Converter.hpp:198
etsi_its_conversion::Converter::kInputTopicDenm
static const std::string kInputTopicDenm
Definition: Converter.hpp:162
etsi_its_conversion::Converter::kInputTopicMapemTs
static const std::string kInputTopicMapemTs
Definition: Converter.hpp:166
etsi_its_conversion::Converter::publisher_mcm_uulm_
rclcpp::Publisher< mcm_uulm_msgs::MCM >::SharedPtr publisher_mcm_uulm_
Definition: Converter.hpp:216
etsi_its_conversion::Converter::kHasBtpDestinationPortParamDefault
static const bool kHasBtpDestinationPortParamDefault
Definition: Converter.hpp:176
etsi_its_conversion::Converter::kInputTopicCamTs
static const std::string kInputTopicCamTs
Definition: Converter.hpp:158
etsi_its_conversion::Converter::kInputTopicVamTs
static const std::string kInputTopicVamTs
Definition: Converter.hpp:172
etsi_its_conversion::Converter::kSubscriberQueueSizeParamDefault
static const int kSubscriberQueueSizeParamDefault
Definition: Converter.hpp:187
etsi_its_conversion::Converter::udp2ros_etsi_types_
std::vector< std::string > udp2ros_etsi_types_
Definition: Converter.hpp:197
etsi_its_conversion::Converter::kInputTopicCam
static const std::string kInputTopicCam
Definition: Converter.hpp:156
etsi_its_conversion::Converter::publisher_vam_ts_
rclcpp::Publisher< vam_ts_msgs::VAM >::SharedPtr publisher_vam_ts_
Definition: Converter.hpp:218
etsi_its_conversion::Converter::kOutputTopicDenm
static const std::string kOutputTopicDenm
Definition: Converter.hpp:163
etsi_its_conversion::Converter::kUdp2RosEtsiTypesParam
static const std::string kUdp2RosEtsiTypesParam
Definition: Converter.hpp:182
convertCAM.h
ros::NodeHandle
etsi_its_conversion::Converter::kOutputTopicMapemTs
static const std::string kOutputTopicMapemTs
Definition: Converter.hpp:167
etsi_its_conversion::Converter::kInputTopicDenmTs
static const std::string kInputTopicDenmTs
Definition: Converter.hpp:164
etsi_its_conversion::Converter::kInputTopicSpatemTs
static const std::string kInputTopicSpatemTs
Definition: Converter.hpp:170


etsi_its_conversion
Author(s): Jean-Pierre Busch , Guido Küppers , Lennart Reiher
autogenerated on Sun May 18 2025 02:32:28