Copter: free allocations in case of allocation failure
[ardupilot.git] / libraries / AP_DDS / AP_DDS_Topic_Table.h
blobd471a25437586837bd090de16313dbc1197cc4b5
1 #include "builtin_interfaces/msg/Time.h"
2 #include "sensor_msgs/msg/NavSatFix.h"
3 #include "tf2_msgs/msg/TFMessage.h"
4 #include "sensor_msgs/msg/BatteryState.h"
5 #include "geographic_msgs/msg/GeoPoseStamped.h"
6 #include "geometry_msgs/msg/Vector3Stamped.h"
7 #if AP_DDS_IMU_PUB_ENABLED
8 #include "sensor_msgs/msg/Imu.h"
9 #endif //AP_DDS_IMU_PUB_ENABLED
11 #include "uxr/client/client.h"
13 // Code generated table based on the enabled topics.
14 // Mavgen is using python, loops are not readable.
15 // Can use jinja to template (like Flask)
17 enum class TopicIndex: uint8_t {
18 #if AP_DDS_TIME_PUB_ENABLED
19 TIME_PUB,
20 #endif // AP_DDS_TIME_PUB_ENABLED
21 #if AP_DDS_NAVSATFIX_PUB_ENABLED
22 NAV_SAT_FIX_PUB,
23 #endif // AP_DDS_NAVSATFIX_PUB_ENABLED
24 #if AP_DDS_STATIC_TF_PUB_ENABLED
25 STATIC_TRANSFORMS_PUB,
26 #endif // AP_DDS_STATIC_TF_PUB_ENABLED
27 #if AP_DDS_BATTERY_STATE_PUB_ENABLED
28 BATTERY_STATE_PUB,
29 #endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
30 #if AP_DDS_IMU_PUB_ENABLED
31 IMU_PUB,
32 #endif //AP_DDS_IMU_PUB_ENABLED
33 #if AP_DDS_LOCAL_POSE_PUB_ENABLED
34 LOCAL_POSE_PUB,
35 #endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
36 #if AP_DDS_LOCAL_VEL_PUB_ENABLED
37 LOCAL_VELOCITY_PUB,
38 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
39 #if AP_DDS_AIRSPEED_PUB_ENABLED
40 LOCAL_AIRSPEED_PUB,
41 #endif // AP_DDS_AIRSPEED_PUB_ENABLED
42 #if AP_DDS_GEOPOSE_PUB_ENABLED
43 GEOPOSE_PUB,
44 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
45 #if AP_DDS_GOAL_PUB_ENABLED
46 GOAL_PUB,
47 #endif // AP_DDS_GOAL_PUB_ENABLED
48 #if AP_DDS_CLOCK_PUB_ENABLED
49 CLOCK_PUB,
50 #endif // AP_DDS_CLOCK_PUB_ENABLED
51 #if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
52 GPS_GLOBAL_ORIGIN_PUB,
53 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
54 #if AP_DDS_STATUS_PUB_ENABLED
55 STATUS_PUB,
56 #endif // AP_DDS_STATUS_PUB_ENABLED
57 #if AP_DDS_JOY_SUB_ENABLED
58 JOY_SUB,
59 #endif // AP_DDS_JOY_SUB_ENABLED
60 #if AP_DDS_DYNAMIC_TF_SUB_ENABLED
61 DYNAMIC_TRANSFORMS_SUB,
62 #endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
63 #if AP_DDS_VEL_CTRL_ENABLED
64 VELOCITY_CONTROL_SUB,
65 #endif // AP_DDS_VEL_CTRL_ENABLED
66 #if AP_DDS_GLOBAL_POS_CTRL_ENABLED
67 GLOBAL_POSITION_SUB,
68 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
71 static inline constexpr uint8_t to_underlying(const TopicIndex index)
73 static_assert(sizeof(index) == sizeof(uint8_t));
74 return static_cast<uint8_t>(index);
78 constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
79 #if AP_DDS_TIME_PUB_ENABLED
81 .topic_id = to_underlying(TopicIndex::TIME_PUB),
82 .pub_id = to_underlying(TopicIndex::TIME_PUB),
83 .sub_id = to_underlying(TopicIndex::TIME_PUB),
84 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAWRITER_ID},
85 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAREADER_ID},
86 .topic_rw = Topic_rw::DataWriter,
87 .topic_name = "rt/ap/time",
88 .type_name = "builtin_interfaces::msg::dds_::Time_",
89 .qos = {
90 .durability = UXR_DURABILITY_VOLATILE,
91 .reliability = UXR_RELIABILITY_RELIABLE,
92 .history = UXR_HISTORY_KEEP_LAST,
93 .depth = 20,
96 #endif // AP_DDS_TIME_PUB_ENABLED
97 #if AP_DDS_NAVSATFIX_PUB_ENABLED
99 .topic_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB),
100 .pub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB),
101 .sub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB),
102 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAWRITER_ID},
103 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAREADER_ID},
104 .topic_rw = Topic_rw::DataWriter,
105 .topic_name = "rt/ap/navsat",
106 .type_name = "sensor_msgs::msg::dds_::NavSatFix_",
107 .qos = {
108 .durability = UXR_DURABILITY_VOLATILE,
109 .reliability = UXR_RELIABILITY_BEST_EFFORT,
110 .history = UXR_HISTORY_KEEP_LAST,
111 .depth = 5,
114 #endif // AP_DDS_NAVSATFIX_PUB_ENABLED
115 #if AP_DDS_STATIC_TF_PUB_ENABLED
117 .topic_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB),
118 .pub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB),
119 .sub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB),
120 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAWRITER_ID},
121 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAREADER_ID},
122 .topic_rw = Topic_rw::DataWriter,
123 .topic_name = "rt/ap/tf_static",
124 .type_name = "tf2_msgs::msg::dds_::TFMessage_",
125 .qos = {
126 .durability = UXR_DURABILITY_TRANSIENT_LOCAL,
127 .reliability = UXR_RELIABILITY_RELIABLE,
128 .history = UXR_HISTORY_KEEP_LAST,
129 .depth = 1,
132 #endif // AP_DDS_STATIC_TF_PUB_ENABLED
133 #if AP_DDS_BATTERY_STATE_PUB_ENABLED
135 .topic_id = to_underlying(TopicIndex::BATTERY_STATE_PUB),
136 .pub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB),
137 .sub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB),
138 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAWRITER_ID},
139 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAREADER_ID},
140 .topic_rw = Topic_rw::DataWriter,
141 .topic_name = "rt/ap/battery",
142 .type_name = "sensor_msgs::msg::dds_::BatteryState_",
143 .qos = {
144 .durability = UXR_DURABILITY_VOLATILE,
145 .reliability = UXR_RELIABILITY_BEST_EFFORT,
146 .history = UXR_HISTORY_KEEP_LAST,
147 .depth = 5,
150 #endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
151 #if AP_DDS_IMU_PUB_ENABLED
153 .topic_id = to_underlying(TopicIndex::IMU_PUB),
154 .pub_id = to_underlying(TopicIndex::IMU_PUB),
155 .sub_id = to_underlying(TopicIndex::IMU_PUB),
156 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAWRITER_ID},
157 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAREADER_ID},
158 .topic_rw = Topic_rw::DataWriter,
159 .topic_name = "rt/ap/imu/experimental/data",
160 .type_name = "sensor_msgs::msg::dds_::Imu_",
161 .qos = {
162 .durability = UXR_DURABILITY_VOLATILE,
163 .reliability = UXR_RELIABILITY_BEST_EFFORT,
164 .history = UXR_HISTORY_KEEP_LAST,
165 .depth = 5,
168 #endif //AP_DDS_IMU_PUB_ENABLED
169 #if AP_DDS_LOCAL_POSE_PUB_ENABLED
171 .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
172 .pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
173 .sub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
174 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAWRITER_ID},
175 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAREADER_ID},
176 .topic_rw = Topic_rw::DataWriter,
177 .topic_name = "rt/ap/pose/filtered",
178 .type_name = "geometry_msgs::msg::dds_::PoseStamped_",
179 .qos = {
180 .durability = UXR_DURABILITY_VOLATILE,
181 .reliability = UXR_RELIABILITY_BEST_EFFORT,
182 .history = UXR_HISTORY_KEEP_LAST,
183 .depth = 5,
186 #endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
187 #if AP_DDS_LOCAL_VEL_PUB_ENABLED
189 .topic_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB),
190 .pub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB),
191 .sub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB),
192 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAWRITER_ID},
193 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAREADER_ID},
194 .topic_rw = Topic_rw::DataWriter,
195 .topic_name = "rt/ap/twist/filtered",
196 .type_name = "geometry_msgs::msg::dds_::TwistStamped_",
197 .qos = {
198 .durability = UXR_DURABILITY_VOLATILE,
199 .reliability = UXR_RELIABILITY_BEST_EFFORT,
200 .history = UXR_HISTORY_KEEP_LAST,
201 .depth = 5,
204 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
205 #if AP_DDS_AIRSPEED_PUB_ENABLED
207 .topic_id = to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB),
208 .pub_id = to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB),
209 .sub_id = to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB),
210 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB), .type=UXR_DATAWRITER_ID},
211 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB), .type=UXR_DATAREADER_ID},
212 .topic_rw = Topic_rw::DataWriter,
213 .topic_name = "rt/ap/airspeed",
214 .type_name = "geometry_msgs::msg::dds_::Vector3Stamped_",
215 .qos = {
216 .durability = UXR_DURABILITY_VOLATILE,
217 .reliability = UXR_RELIABILITY_BEST_EFFORT,
218 .history = UXR_HISTORY_KEEP_LAST,
219 .depth = 5,
222 #endif // AP_DDS_AIRSPEED_PUB_ENABLED
223 #if AP_DDS_GEOPOSE_PUB_ENABLED
225 .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB),
226 .pub_id = to_underlying(TopicIndex::GEOPOSE_PUB),
227 .sub_id = to_underlying(TopicIndex::GEOPOSE_PUB),
228 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAWRITER_ID},
229 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAREADER_ID},
230 .topic_rw = Topic_rw::DataWriter,
231 .topic_name = "rt/ap/geopose/filtered",
232 .type_name = "geographic_msgs::msg::dds_::GeoPoseStamped_",
233 .qos = {
234 .durability = UXR_DURABILITY_VOLATILE,
235 .reliability = UXR_RELIABILITY_BEST_EFFORT,
236 .history = UXR_HISTORY_KEEP_LAST,
237 .depth = 5,
240 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
241 #if AP_DDS_GOAL_PUB_ENABLED
243 .topic_id = to_underlying(TopicIndex::GOAL_PUB),
244 .pub_id = to_underlying(TopicIndex::GOAL_PUB),
245 .sub_id = to_underlying(TopicIndex::GOAL_PUB),
246 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAWRITER_ID},
247 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAREADER_ID},
248 .topic_rw = Topic_rw::DataWriter,
249 .topic_name = "rt/ap/goal_lla",
250 .type_name = "geographic_msgs::msg::dds_::GeoPointStamped_",
251 .qos = {
252 .durability = UXR_DURABILITY_TRANSIENT_LOCAL,
253 .reliability = UXR_RELIABILITY_RELIABLE,
254 .history = UXR_HISTORY_KEEP_LAST,
255 .depth = 1,
258 #endif // AP_DDS_GOAL_PUB_ENABLED
259 #if AP_DDS_CLOCK_PUB_ENABLED
261 .topic_id = to_underlying(TopicIndex::CLOCK_PUB),
262 .pub_id = to_underlying(TopicIndex::CLOCK_PUB),
263 .sub_id = to_underlying(TopicIndex::CLOCK_PUB),
264 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAWRITER_ID},
265 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAREADER_ID},
266 .topic_rw = Topic_rw::DataWriter,
267 .topic_name = "rt/ap/clock",
268 .type_name = "rosgraph_msgs::msg::dds_::Clock_",
269 .qos = {
270 .durability = UXR_DURABILITY_VOLATILE,
271 .reliability = UXR_RELIABILITY_RELIABLE,
272 .history = UXR_HISTORY_KEEP_LAST,
273 .depth = 20,
276 #endif // AP_DDS_CLOCK_PUB_ENABLED
277 #if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
279 .topic_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
280 .pub_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
281 .sub_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
282 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), .type=UXR_DATAWRITER_ID},
283 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), .type=UXR_DATAREADER_ID},
284 .topic_rw = Topic_rw::DataWriter,
285 .topic_name = "rt/ap/gps_global_origin/filtered",
286 .type_name = "geographic_msgs::msg::dds_::GeoPointStamped_",
287 .qos = {
288 .durability = UXR_DURABILITY_VOLATILE,
289 .reliability = UXR_RELIABILITY_BEST_EFFORT,
290 .history = UXR_HISTORY_KEEP_LAST,
291 .depth = 5,
294 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
295 #if AP_DDS_STATUS_PUB_ENABLED
297 .topic_id = to_underlying(TopicIndex::STATUS_PUB),
298 .pub_id = to_underlying(TopicIndex::STATUS_PUB),
299 .sub_id = to_underlying(TopicIndex::STATUS_PUB),
300 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAWRITER_ID},
301 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAREADER_ID},
302 .topic_rw = Topic_rw::DataWriter,
303 .topic_name = "rt/ap/status",
304 .type_name = "ardupilot_msgs::msg::dds_::Status_",
305 .qos = {
306 .durability = UXR_DURABILITY_TRANSIENT_LOCAL,
307 .reliability = UXR_RELIABILITY_RELIABLE,
308 .history = UXR_HISTORY_KEEP_LAST,
309 .depth = 1,
312 #endif // AP_DDS_STATUS_PUB_ENABLED
313 #if AP_DDS_JOY_SUB_ENABLED
315 .topic_id = to_underlying(TopicIndex::JOY_SUB),
316 .pub_id = to_underlying(TopicIndex::JOY_SUB),
317 .sub_id = to_underlying(TopicIndex::JOY_SUB),
318 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAWRITER_ID},
319 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAREADER_ID},
320 .topic_rw = Topic_rw::DataReader,
321 .topic_name = "rt/ap/joy",
322 .type_name = "sensor_msgs::msg::dds_::Joy_",
323 .qos = {
324 .durability = UXR_DURABILITY_VOLATILE,
325 .reliability = UXR_RELIABILITY_BEST_EFFORT,
326 .history = UXR_HISTORY_KEEP_LAST,
327 .depth = 5,
330 #endif // AP_DDS_JOY_SUB_ENABLED
331 #if AP_DDS_DYNAMIC_TF_SUB_ENABLED
333 .topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
334 .pub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
335 .sub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
336 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAWRITER_ID},
337 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAREADER_ID},
338 .topic_rw = Topic_rw::DataReader,
339 .topic_name = "rt/ap/tf",
340 .type_name = "tf2_msgs::msg::dds_::TFMessage_",
341 .qos = {
342 .durability = UXR_DURABILITY_VOLATILE,
343 .reliability = UXR_RELIABILITY_BEST_EFFORT,
344 .history = UXR_HISTORY_KEEP_LAST,
345 .depth = 5,
348 #endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
349 #if AP_DDS_VEL_CTRL_ENABLED
351 .topic_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB),
352 .pub_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB),
353 .sub_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB),
354 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .type=UXR_DATAWRITER_ID},
355 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .type=UXR_DATAREADER_ID},
356 .topic_rw = Topic_rw::DataReader,
357 .topic_name = "rt/ap/cmd_vel",
358 .type_name = "geometry_msgs::msg::dds_::TwistStamped_",
359 .qos = {
360 .durability = UXR_DURABILITY_VOLATILE,
361 .reliability = UXR_RELIABILITY_BEST_EFFORT,
362 .history = UXR_HISTORY_KEEP_LAST,
363 .depth = 5,
366 #endif // AP_DDS_VEL_CTRL_ENABLED
367 #if AP_DDS_GLOBAL_POS_CTRL_ENABLED
369 .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
370 .pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
371 .sub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
372 .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAWRITER_ID},
373 .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAREADER_ID},
374 .topic_rw = Topic_rw::DataReader,
375 .topic_name = "rt/ap/cmd_gps_pose",
376 .type_name = "ardupilot_msgs::msg::dds_::GlobalPosition_",
377 .qos = {
378 .durability = UXR_DURABILITY_VOLATILE,
379 .reliability = UXR_RELIABILITY_BEST_EFFORT,
380 .history = UXR_HISTORY_KEEP_LAST,
381 .depth = 5,
384 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED