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
20 #endif // AP_DDS_TIME_PUB_ENABLED
21 #if AP_DDS_NAVSATFIX_PUB_ENABLED
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
29 #endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
30 #if AP_DDS_IMU_PUB_ENABLED
32 #endif //AP_DDS_IMU_PUB_ENABLED
33 #if AP_DDS_LOCAL_POSE_PUB_ENABLED
35 #endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
36 #if AP_DDS_LOCAL_VEL_PUB_ENABLED
38 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
39 #if AP_DDS_AIRSPEED_PUB_ENABLED
41 #endif // AP_DDS_AIRSPEED_PUB_ENABLED
42 #if AP_DDS_GEOPOSE_PUB_ENABLED
44 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
45 #if AP_DDS_GOAL_PUB_ENABLED
47 #endif // AP_DDS_GOAL_PUB_ENABLED
48 #if AP_DDS_CLOCK_PUB_ENABLED
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
56 #endif // AP_DDS_STATUS_PUB_ENABLED
57 #if AP_DDS_JOY_SUB_ENABLED
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
65 #endif // AP_DDS_VEL_CTRL_ENABLED
66 #if AP_DDS_GLOBAL_POS_CTRL_ENABLED
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_",
90 .durability
= UXR_DURABILITY_VOLATILE
,
91 .reliability
= UXR_RELIABILITY_RELIABLE
,
92 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
108 .durability
= UXR_DURABILITY_VOLATILE
,
109 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
110 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
126 .durability
= UXR_DURABILITY_TRANSIENT_LOCAL
,
127 .reliability
= UXR_RELIABILITY_RELIABLE
,
128 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
144 .durability
= UXR_DURABILITY_VOLATILE
,
145 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
146 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
162 .durability
= UXR_DURABILITY_VOLATILE
,
163 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
164 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
180 .durability
= UXR_DURABILITY_VOLATILE
,
181 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
182 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
198 .durability
= UXR_DURABILITY_VOLATILE
,
199 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
200 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
216 .durability
= UXR_DURABILITY_VOLATILE
,
217 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
218 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
234 .durability
= UXR_DURABILITY_VOLATILE
,
235 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
236 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
252 .durability
= UXR_DURABILITY_TRANSIENT_LOCAL
,
253 .reliability
= UXR_RELIABILITY_RELIABLE
,
254 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
270 .durability
= UXR_DURABILITY_VOLATILE
,
271 .reliability
= UXR_RELIABILITY_RELIABLE
,
272 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
288 .durability
= UXR_DURABILITY_VOLATILE
,
289 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
290 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
306 .durability
= UXR_DURABILITY_TRANSIENT_LOCAL
,
307 .reliability
= UXR_RELIABILITY_RELIABLE
,
308 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
324 .durability
= UXR_DURABILITY_VOLATILE
,
325 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
326 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
342 .durability
= UXR_DURABILITY_VOLATILE
,
343 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
344 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
360 .durability
= UXR_DURABILITY_VOLATILE
,
361 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
362 .history
= UXR_HISTORY_KEEP_LAST
,
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_",
378 .durability
= UXR_DURABILITY_VOLATILE
,
379 .reliability
= UXR_RELIABILITY_BEST_EFFORT
,
380 .history
= UXR_HISTORY_KEEP_LAST
,
384 #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED