Skip to content

Commit ed0f429

Browse files
fgarciacardenasfgarciacardenas
authored
Added QoS to ROS2 (#81)
1 parent 2b34a6d commit ed0f429

File tree

1 file changed

+36
-36
lines changed

1 file changed

+36
-36
lines changed

‎fixposition_driver_ros2/src/fixposition_driver_node.cpp‎

Lines changed: 36 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -98,10 +98,10 @@ bool FixpositionDriverNode::StartNode() {
9898

9999
// FP_A-ODOMETRY
100100
if (params_.MessageEnabled(fpa::FpaOdometryPayload::MSG_NAME)) {
101-
_PUB(fpa_odometry_pub_, fpmsgs::FpaOdometry, output_ns + "/fpa/odometry", 5);
102-
_PUB(odometry_ecef_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_ecef", 5);
103-
_PUB(odometry_llh_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/odometry_llh", 5);
104-
_PUB(poiimu_pub_, sensor_msgs::msg::Imu, output_ns + "/poiimu", 5);
101+
_PUB(fpa_odometry_pub_, fpmsgs::FpaOdometry, output_ns + "/fpa/odometry", qos_settings_);
102+
_PUB(odometry_ecef_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_ecef", qos_settings_);
103+
_PUB(odometry_llh_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/odometry_llh", qos_settings_);
104+
_PUB(poiimu_pub_, sensor_msgs::msg::Imu, output_ns + "/poiimu", qos_settings_);
105105
driver_.AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
106106
auto odometry_payload = dynamic_cast<const fpa::FpaOdometryPayload&>(payload);
107107
PublishFpaOdometry(odometry_payload, fpa_odometry_pub_);
@@ -117,8 +117,8 @@ bool FixpositionDriverNode::StartNode() {
117117

118118
// FP_A-ODOMSH
119119
if (params_.MessageEnabled(fpa::FpaOdomshPayload::MSG_NAME)) {
120-
_PUB(fpa_odomsh_pub_, fpmsgs::FpaOdomsh, output_ns + "/fpa/odomsh", 5);
121-
_PUB(odometry_smooth_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_smooth", 5);
120+
_PUB(fpa_odomsh_pub_, fpmsgs::FpaOdomsh, output_ns + "/fpa/odomsh", qos_settings_);
121+
_PUB(odometry_smooth_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_smooth", qos_settings_);
122122
driver_.AddFpaObserver(fpa::FpaOdomshPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
123123
auto odomsh_payload = dynamic_cast<const fpa::FpaOdomshPayload&>(payload);
124124
PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_);
@@ -132,9 +132,9 @@ bool FixpositionDriverNode::StartNode() {
132132

133133
// FP_A-ODOMENU
134134
if (params_.MessageEnabled(fpa::FpaOdomenuPayload::MSG_NAME)) {
135-
_PUB(fpa_odomenu_pub_, fpmsgs::FpaOdomenu, output_ns + "/fpa/odomenu", 5);
136-
_PUB(odometry_enu_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_enu", 5);
137-
_PUB(eul_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/ypr", 5);
135+
_PUB(fpa_odomenu_pub_, fpmsgs::FpaOdomenu, output_ns + "/fpa/odomenu", qos_settings_);
136+
_PUB(odometry_enu_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_enu", qos_settings_);
137+
_PUB(eul_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/ypr", qos_settings_);
138138
driver_.AddFpaObserver(fpa::FpaOdomenuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
139139
auto odomenu_payload = dynamic_cast<const fpa::FpaOdomenuPayload&>(payload);
140140
PublishFpaOdomenu(odomenu_payload, fpa_odomenu_pub_);
@@ -149,7 +149,7 @@ bool FixpositionDriverNode::StartNode() {
149149

150150
// FP_A-ODOMSTATUS
151151
if (params_.MessageEnabled(fpa::FpaOdomstatusPayload::MSG_NAME)) {
152-
_PUB(fpa_odomstatus_pub_, fpmsgs::FpaOdomstatus, output_ns + "/fpa/odomstatus", 5);
152+
_PUB(fpa_odomstatus_pub_, fpmsgs::FpaOdomstatus, output_ns + "/fpa/odomstatus", qos_settings_);
153153
driver_.AddFpaObserver(fpa::FpaOdomstatusPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
154154
auto odomstatus_payload = dynamic_cast<const fpa::FpaOdomstatusPayload&>(payload);
155155
PublishFpaOdomstatus(odomstatus_payload, fpa_odomstatus_pub_);
@@ -159,7 +159,7 @@ bool FixpositionDriverNode::StartNode() {
159159

160160
// FP_A-EOE
161161
if (params_.MessageEnabled(fpa::FpaEoePayload::MSG_NAME)) {
162-
_PUB(fpa_eoe_pub_, fpmsgs::FpaEoe, output_ns + "/fpa/eoe", 5);
162+
_PUB(fpa_eoe_pub_, fpmsgs::FpaEoe, output_ns + "/fpa/eoe", qos_settings_);
163163
driver_.AddFpaObserver(fpa::FpaEoePayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
164164
auto eoe_payload = dynamic_cast<const fpa::FpaEoePayload&>(payload);
165165
(void)eoe_payload;
@@ -182,7 +182,7 @@ bool FixpositionDriverNode::StartNode() {
182182

183183
// FP_A-TF
184184
if (params_.MessageEnabled(fpa::FpaTfPayload::MSG_NAME)) {
185-
_PUB(eul_imu_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/imu_ypr", 5);
185+
_PUB(eul_imu_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/imu_ypr", qos_settings_);
186186
driver_.AddFpaObserver(fpa::FpaTfPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
187187
(void)payload;
188188
TfData tf;
@@ -193,31 +193,31 @@ bool FixpositionDriverNode::StartNode() {
193193

194194
// FP_A-LLH
195195
if (params_.MessageEnabled(fpa::FpaLlhPayload::MSG_NAME)) {
196-
_PUB(fpa_llh_pub_, fpmsgs::FpaLlh, output_ns + "/fpa/llh", 5);
196+
_PUB(fpa_llh_pub_, fpmsgs::FpaLlh, output_ns + "/fpa/llh", qos_settings_);
197197
driver_.AddFpaObserver(fpa::FpaLlhPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
198198
PublishFpaLlh(dynamic_cast<const fpa::FpaLlhPayload&>(payload), fpa_llh_pub_);
199199
});
200200
}
201201

202202
// FP_A-GNSSANT
203203
if (params_.MessageEnabled(fpa::FpaGnssantPayload::MSG_NAME)) {
204-
_PUB(fpa_gnssant_pub_, fpmsgs::FpaGnssant, output_ns + "/fpa/gnssant", 5);
204+
_PUB(fpa_gnssant_pub_, fpmsgs::FpaGnssant, output_ns + "/fpa/gnssant", qos_settings_);
205205
driver_.AddFpaObserver(fpa::FpaGnssantPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
206206
PublishFpaGnssant(dynamic_cast<const fpa::FpaGnssantPayload&>(payload), fpa_gnssant_pub_);
207207
});
208208
}
209209

210210
// FP_A-GNSSCORR
211211
if (params_.MessageEnabled(fpa::FpaGnsscorrPayload::MSG_NAME)) {
212-
_PUB(fpa_gnsscorr_pub_, fpmsgs::FpaGnsscorr, output_ns + "/fpa/gnsscorr", 5);
212+
_PUB(fpa_gnsscorr_pub_, fpmsgs::FpaGnsscorr, output_ns + "/fpa/gnsscorr", qos_settings_);
213213
driver_.AddFpaObserver(fpa::FpaGnsscorrPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
214214
PublishFpaGnsscorr(dynamic_cast<const fpa::FpaGnsscorrPayload&>(payload), fpa_gnsscorr_pub_);
215215
});
216216
}
217217

218218
// FP_A-IMUBIAS
219219
if (params_.MessageEnabled(fpa::FpaImubiasPayload::MSG_NAME)) {
220-
_PUB(fpa_imubias_pub_, fpmsgs::FpaImubias, output_ns + "/fpa/imubias", 5);
220+
_PUB(fpa_imubias_pub_, fpmsgs::FpaImubias, output_ns + "/fpa/imubias", qos_settings_);
221221
driver_.AddFpaObserver(fpa::FpaImubiasPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
222222
auto imubias_payload = dynamic_cast<const fpa::FpaImubiasPayload&>(payload);
223223
PublishFpaImubias(imubias_payload, fpa_imubias_pub_);
@@ -227,40 +227,40 @@ bool FixpositionDriverNode::StartNode() {
227227

228228
// FP_A-RAWIMU
229229
if (params_.MessageEnabled(fpa::FpaRawimuPayload::MSG_NAME)) {
230-
_PUB(rawimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/rawimu", 5);
230+
_PUB(rawimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/rawimu", qos_settings_);
231231
driver_.AddFpaObserver(fpa::FpaRawimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
232232
PublishFpaRawimu(dynamic_cast<const fpa::FpaRawimuPayload&>(payload), rawimu_pub_);
233233
});
234234
}
235235

236236
// FP_A-CORRIMU
237237
if (params_.MessageEnabled(fpa::FpaCorrimuPayload::MSG_NAME)) {
238-
_PUB(corrimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/corrimu", 5);
238+
_PUB(corrimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/corrimu", qos_settings_);
239239
driver_.AddFpaObserver(fpa::FpaCorrimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
240240
PublishFpaCorrimu(dynamic_cast<const fpa::FpaCorrimuPayload&>(payload), corrimu_pub_);
241241
});
242242
}
243243

244244
// FP_A-TEXT
245245
if (params_.MessageEnabled(fpa::FpaTextPayload::MSG_NAME)) {
246-
_PUB(fpa_text_pub_, fpmsgs::FpaText, output_ns + "/fpa/text", 5);
246+
_PUB(fpa_text_pub_, fpmsgs::FpaText, output_ns + "/fpa/text", qos_settings_);
247247
driver_.AddFpaObserver(fpa::FpaTextPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
248248
PublishFpaText(dynamic_cast<const fpa::FpaTextPayload&>(payload), fpa_text_pub_);
249249
});
250250
}
251251

252252
// FP_A-TP
253253
if (params_.MessageEnabled(fpa::FpaTpPayload::MSG_NAME)) {
254-
_PUB(fpa_tp_pub_, fpmsgs::FpaTp, output_ns + "/fpa/tp", 5);
254+
_PUB(fpa_tp_pub_, fpmsgs::FpaTp, output_ns + "/fpa/tp", qos_settings_);
255255
driver_.AddFpaObserver(fpa::FpaTpPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
256256
PublishFpaTp(dynamic_cast<const fpa::FpaTpPayload&>(payload), fpa_tp_pub_);
257257
});
258258
}
259259

260260
// NOV_B-BESTGNSSPOS
261261
if (params_.MessageEnabled(novb::NOV_B_BESTGNSSPOS_STRID)) {
262-
_PUB(navsatfix_gnss1_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss1", 5);
263-
_PUB(navsatfix_gnss2_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss2", 5);
262+
_PUB(navsatfix_gnss1_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss1", qos_settings_);
263+
_PUB(navsatfix_gnss2_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss2", qos_settings_);
264264
driver_.AddNovbObserver( //
265265
novb::NOV_B_BESTGNSSPOS_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) {
266266
if (!PublishNovbBestgnsspos(header, (novb::NovbBestgnsspos*)payload, navsatfix_gnss1_pub_,
@@ -272,7 +272,7 @@ bool FixpositionDriverNode::StartNode() {
272272

273273
// NOV_B-INSPVAX
274274
if (params_.MessageEnabled(novb::NOV_B_INSPVAX_STRID)) {
275-
_PUB(novb_inspvax_pub_, fpmsgs::NovbInspvax, output_ns + "/novb/inspvax", 5);
275+
_PUB(novb_inspvax_pub_, fpmsgs::NovbInspvax, output_ns + "/novb/inspvax", qos_settings_);
276276
driver_.AddNovbObserver( //
277277
novb::NOV_B_INSPVAX_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) {
278278
if (!PublishNovbInspvax(header, (novb::NovbInspvax*)payload, novb_inspvax_pub_)) {
@@ -284,7 +284,7 @@ bool FixpositionDriverNode::StartNode() {
284284

285285
// NMEA-GP-GGA
286286
if (params_.MessageEnabled(nmea::NmeaGgaPayload::FORMATTER)) {
287-
_PUB(nmea_gga_pub_, fpmsgs::NmeaGga, output_ns + "/nmea/gga", 5);
287+
_PUB(nmea_gga_pub_, fpmsgs::NmeaGga, output_ns + "/nmea/gga", qos_settings_);
288288
driver_.AddNmeaObserver(nmea::NmeaGgaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
289289
auto gga_payload = dynamic_cast<const nmea::NmeaGgaPayload&>(payload);
290290
PublishNmeaGga(gga_payload, nmea_gga_pub_);
@@ -294,7 +294,7 @@ bool FixpositionDriverNode::StartNode() {
294294

295295
// NMEA-GP-GLL
296296
if (params_.MessageEnabled(nmea::NmeaGllPayload::FORMATTER)) {
297-
_PUB(nmea_gll_pub_, fpmsgs::NmeaGll, output_ns + "/nmea/gll", 5);
297+
_PUB(nmea_gll_pub_, fpmsgs::NmeaGll, output_ns + "/nmea/gll", qos_settings_);
298298
driver_.AddNmeaObserver(nmea::NmeaGllPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
299299
auto gll_payload = dynamic_cast<const nmea::NmeaGllPayload&>(payload);
300300
PublishNmeaGll(gll_payload, nmea_gll_pub_);
@@ -304,7 +304,7 @@ bool FixpositionDriverNode::StartNode() {
304304

305305
// NMEA-GN-GSA
306306
if (params_.MessageEnabled(nmea::NmeaGsaPayload::FORMATTER)) {
307-
_PUB(nmea_gsa_pub_, fpmsgs::NmeaGsa, output_ns + "/nmea/gsa", 5);
307+
_PUB(nmea_gsa_pub_, fpmsgs::NmeaGsa, output_ns + "/nmea/gsa", qos_settings_);
308308
driver_.AddNmeaObserver(nmea::NmeaGsaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
309309
auto gsa_payload_ = dynamic_cast<const nmea::NmeaGsaPayload&>(payload);
310310
PublishNmeaGsa(gsa_payload_, nmea_gsa_pub_);
@@ -315,7 +315,7 @@ bool FixpositionDriverNode::StartNode() {
315315

316316
// NMEA-GP-GST
317317
if (params_.MessageEnabled(nmea::NmeaGstPayload::FORMATTER)) {
318-
_PUB(nmea_gst_pub_, fpmsgs::NmeaGst, output_ns + "/nmea/gst", 5);
318+
_PUB(nmea_gst_pub_, fpmsgs::NmeaGst, output_ns + "/nmea/gst", qos_settings_);
319319
driver_.AddNmeaObserver(nmea::NmeaGstPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
320320
auto gst_payload = dynamic_cast<const nmea::NmeaGstPayload&>(payload);
321321
PublishNmeaGst(gst_payload, nmea_gst_pub_);
@@ -325,7 +325,7 @@ bool FixpositionDriverNode::StartNode() {
325325

326326
// NMEA-GX-GSV
327327
if (params_.MessageEnabled(nmea::NmeaGsvPayload::FORMATTER)) {
328-
_PUB(nmea_gsv_pub_, fpmsgs::NmeaGsv, output_ns + "/nmea/gsv", 5);
328+
_PUB(nmea_gsv_pub_, fpmsgs::NmeaGsv, output_ns + "/nmea/gsv", qos_settings_);
329329
driver_.AddNmeaObserver(nmea::NmeaGsvPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
330330
auto gsv_payload_ = dynamic_cast<const nmea::NmeaGsvPayload&>(payload);
331331
PublishNmeaGsv(gsv_payload_, nmea_gsv_pub_);
@@ -335,7 +335,7 @@ bool FixpositionDriverNode::StartNode() {
335335

336336
// NMEA-GP-HDT
337337
if (params_.MessageEnabled(nmea::NmeaHdtPayload::FORMATTER)) {
338-
_PUB(nmea_hdt_pub_, fpmsgs::NmeaHdt, output_ns + "/nmea/hdt", 5);
338+
_PUB(nmea_hdt_pub_, fpmsgs::NmeaHdt, output_ns + "/nmea/hdt", qos_settings_);
339339
driver_.AddNmeaObserver(nmea::NmeaHdtPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
340340
auto hdt_payload = dynamic_cast<const nmea::NmeaHdtPayload&>(payload);
341341
PublishNmeaHdt(hdt_payload, nmea_hdt_pub_);
@@ -345,7 +345,7 @@ bool FixpositionDriverNode::StartNode() {
345345

346346
// NMEA-GP-RMC
347347
if (params_.MessageEnabled(nmea::NmeaRmcPayload::FORMATTER)) {
348-
_PUB(nmea_rmc_pub_, fpmsgs::NmeaRmc, output_ns + "/nmea/rmc", 5);
348+
_PUB(nmea_rmc_pub_, fpmsgs::NmeaRmc, output_ns + "/nmea/rmc", qos_settings_);
349349
driver_.AddNmeaObserver(nmea::NmeaRmcPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
350350
auto rmc_payload = dynamic_cast<const nmea::NmeaRmcPayload&>(payload);
351351
PublishNmeaRmc(rmc_payload, nmea_rmc_pub_);
@@ -355,7 +355,7 @@ bool FixpositionDriverNode::StartNode() {
355355

356356
// NMEA-GP-VTG
357357
if (params_.MessageEnabled(nmea::NmeaVtgPayload::FORMATTER)) {
358-
_PUB(nmea_vtg_pub_, fpmsgs::NmeaVtg, output_ns + "/nmea/vtg", 5);
358+
_PUB(nmea_vtg_pub_, fpmsgs::NmeaVtg, output_ns + "/nmea/vtg", qos_settings_);
359359
driver_.AddNmeaObserver(nmea::NmeaVtgPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
360360
auto vtg_payload = dynamic_cast<const nmea::NmeaVtgPayload&>(payload);
361361
PublishNmeaVtg(vtg_payload, nmea_vtg_pub_);
@@ -365,7 +365,7 @@ bool FixpositionDriverNode::StartNode() {
365365

366366
// NMEA-GP-ZDA
367367
if (params_.MessageEnabled(nmea::NmeaZdaPayload::FORMATTER)) {
368-
_PUB(nmea_zda_pub_, fpmsgs::NmeaZda, output_ns + "/nmea/zda", 5);
368+
_PUB(nmea_zda_pub_, fpmsgs::NmeaZda, output_ns + "/nmea/zda", qos_settings_);
369369
driver_.AddNmeaObserver(nmea::NmeaZdaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
370370
auto zda_payload = dynamic_cast<const nmea::NmeaZdaPayload&>(payload);
371371
PublishNmeaZda(zda_payload, nmea_zda_pub_);
@@ -375,25 +375,25 @@ bool FixpositionDriverNode::StartNode() {
375375

376376
// Raw messages
377377
if (params_.raw_output_) {
378-
_PUB(raw_pub_, fpmsgs::ParserMsg, output_ns + "/raw", 5);
378+
_PUB(raw_pub_, fpmsgs::ParserMsg, output_ns + "/raw", qos_settings_);
379379
driver_.AddRawObserver([this](const parser::ParserMsg& msg) { PublishParserMsg(msg, raw_pub_); });
380380
}
381381

382382
// Fusion epoch
383383
if (params_.fusion_epoch_) {
384-
_PUB(fusion_epoch_pub_, fpmsgs::FusionEpoch, output_ns + "/fusion", 5);
384+
_PUB(fusion_epoch_pub_, fpmsgs::FusionEpoch, output_ns + "/fusion", qos_settings_);
385385
// Publish is triggered by FP_A-EOE above
386386
}
387387

388388
// NMEA epoch
389389
if (params_.nmea_epoch_ != fpa::FpaEpoch::UNSPECIFIED) {
390-
_PUB(nmea_epoch_pub_, fpmsgs::NmeaEpoch, output_ns + "/nmea", 5);
390+
_PUB(nmea_epoch_pub_, fpmsgs::NmeaEpoch, output_ns + "/nmea", qos_settings_);
391391
// Publish is triggered by FP_A-EOE above
392392
}
393393

394394
// Jump warning message
395395
if (params_.cov_warning_) {
396-
_PUB(jump_pub_, fpmsgs::CovWarn, output_ns + "/extras/jump", 5);
396+
_PUB(jump_pub_, fpmsgs::CovWarn, output_ns + "/extras/jump", qos_settings_);
397397
}
398398

399399
// Subscribe to correction data input

0 commit comments

Comments
 (0)