From 3a6bf97d912dbdfb92077b938a42ae55efd72b08 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Wed, 12 Feb 2025 14:56:16 +0000 Subject: [PATCH] Added QoS to ROS2 --- .../src/fixposition_driver_node.cpp | 72 +++++++++---------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 36cdc2d..7a9b3b3 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -98,10 +98,10 @@ bool FixpositionDriverNode::StartNode() { // FP_A-ODOMETRY if (params_.MessageEnabled(fpa::FpaOdometryPayload::MSG_NAME)) { - _PUB(fpa_odometry_pub_, fpmsgs::FpaOdometry, output_ns + "/fpa/odometry", 5); - _PUB(odometry_ecef_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_ecef", 5); - _PUB(odometry_llh_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/odometry_llh", 5); - _PUB(poiimu_pub_, sensor_msgs::msg::Imu, output_ns + "/poiimu", 5); + _PUB(fpa_odometry_pub_, fpmsgs::FpaOdometry, output_ns + "/fpa/odometry", qos_settings_); + _PUB(odometry_ecef_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_ecef", qos_settings_); + _PUB(odometry_llh_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/odometry_llh", qos_settings_); + _PUB(poiimu_pub_, sensor_msgs::msg::Imu, output_ns + "/poiimu", qos_settings_); driver_.AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odometry_payload = dynamic_cast(payload); PublishFpaOdometry(odometry_payload, fpa_odometry_pub_); @@ -117,8 +117,8 @@ bool FixpositionDriverNode::StartNode() { // FP_A-ODOMSH if (params_.MessageEnabled(fpa::FpaOdomshPayload::MSG_NAME)) { - _PUB(fpa_odomsh_pub_, fpmsgs::FpaOdomsh, output_ns + "/fpa/odomsh", 5); - _PUB(odometry_smooth_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_smooth", 5); + _PUB(fpa_odomsh_pub_, fpmsgs::FpaOdomsh, output_ns + "/fpa/odomsh", qos_settings_); + _PUB(odometry_smooth_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_smooth", qos_settings_); driver_.AddFpaObserver(fpa::FpaOdomshPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odomsh_payload = dynamic_cast(payload); PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_); @@ -132,9 +132,9 @@ bool FixpositionDriverNode::StartNode() { // FP_A-ODOMENU if (params_.MessageEnabled(fpa::FpaOdomenuPayload::MSG_NAME)) { - _PUB(fpa_odomenu_pub_, fpmsgs::FpaOdomenu, output_ns + "/fpa/odomenu", 5); - _PUB(odometry_enu_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_enu", 5); - _PUB(eul_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/ypr", 5); + _PUB(fpa_odomenu_pub_, fpmsgs::FpaOdomenu, output_ns + "/fpa/odomenu", qos_settings_); + _PUB(odometry_enu_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_enu", qos_settings_); + _PUB(eul_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/ypr", qos_settings_); driver_.AddFpaObserver(fpa::FpaOdomenuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odomenu_payload = dynamic_cast(payload); PublishFpaOdomenu(odomenu_payload, fpa_odomenu_pub_); @@ -149,7 +149,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-ODOMSTATUS if (params_.MessageEnabled(fpa::FpaOdomstatusPayload::MSG_NAME)) { - _PUB(fpa_odomstatus_pub_, fpmsgs::FpaOdomstatus, output_ns + "/fpa/odomstatus", 5); + _PUB(fpa_odomstatus_pub_, fpmsgs::FpaOdomstatus, output_ns + "/fpa/odomstatus", qos_settings_); driver_.AddFpaObserver(fpa::FpaOdomstatusPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odomstatus_payload = dynamic_cast(payload); PublishFpaOdomstatus(odomstatus_payload, fpa_odomstatus_pub_); @@ -159,7 +159,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-EOE if (params_.MessageEnabled(fpa::FpaEoePayload::MSG_NAME)) { - _PUB(fpa_eoe_pub_, fpmsgs::FpaEoe, output_ns + "/fpa/eoe", 5); + _PUB(fpa_eoe_pub_, fpmsgs::FpaEoe, output_ns + "/fpa/eoe", qos_settings_); driver_.AddFpaObserver(fpa::FpaEoePayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto eoe_payload = dynamic_cast(payload); (void)eoe_payload; @@ -182,7 +182,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-TF if (params_.MessageEnabled(fpa::FpaTfPayload::MSG_NAME)) { - _PUB(eul_imu_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/imu_ypr", 5); + _PUB(eul_imu_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/imu_ypr", qos_settings_); driver_.AddFpaObserver(fpa::FpaTfPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { (void)payload; TfData tf; @@ -193,7 +193,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-LLH if (params_.MessageEnabled(fpa::FpaLlhPayload::MSG_NAME)) { - _PUB(fpa_llh_pub_, fpmsgs::FpaLlh, output_ns + "/fpa/llh", 5); + _PUB(fpa_llh_pub_, fpmsgs::FpaLlh, output_ns + "/fpa/llh", qos_settings_); driver_.AddFpaObserver(fpa::FpaLlhPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaLlh(dynamic_cast(payload), fpa_llh_pub_); }); @@ -201,7 +201,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-GNSSANT if (params_.MessageEnabled(fpa::FpaGnssantPayload::MSG_NAME)) { - _PUB(fpa_gnssant_pub_, fpmsgs::FpaGnssant, output_ns + "/fpa/gnssant", 5); + _PUB(fpa_gnssant_pub_, fpmsgs::FpaGnssant, output_ns + "/fpa/gnssant", qos_settings_); driver_.AddFpaObserver(fpa::FpaGnssantPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaGnssant(dynamic_cast(payload), fpa_gnssant_pub_); }); @@ -209,7 +209,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-GNSSCORR if (params_.MessageEnabled(fpa::FpaGnsscorrPayload::MSG_NAME)) { - _PUB(fpa_gnsscorr_pub_, fpmsgs::FpaGnsscorr, output_ns + "/fpa/gnsscorr", 5); + _PUB(fpa_gnsscorr_pub_, fpmsgs::FpaGnsscorr, output_ns + "/fpa/gnsscorr", qos_settings_); driver_.AddFpaObserver(fpa::FpaGnsscorrPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaGnsscorr(dynamic_cast(payload), fpa_gnsscorr_pub_); }); @@ -217,7 +217,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-IMUBIAS if (params_.MessageEnabled(fpa::FpaImubiasPayload::MSG_NAME)) { - _PUB(fpa_imubias_pub_, fpmsgs::FpaImubias, output_ns + "/fpa/imubias", 5); + _PUB(fpa_imubias_pub_, fpmsgs::FpaImubias, output_ns + "/fpa/imubias", qos_settings_); driver_.AddFpaObserver(fpa::FpaImubiasPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto imubias_payload = dynamic_cast(payload); PublishFpaImubias(imubias_payload, fpa_imubias_pub_); @@ -227,7 +227,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-RAWIMU if (params_.MessageEnabled(fpa::FpaRawimuPayload::MSG_NAME)) { - _PUB(rawimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/rawimu", 5); + _PUB(rawimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/rawimu", qos_settings_); driver_.AddFpaObserver(fpa::FpaRawimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaRawimu(dynamic_cast(payload), rawimu_pub_); }); @@ -235,7 +235,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-CORRIMU if (params_.MessageEnabled(fpa::FpaCorrimuPayload::MSG_NAME)) { - _PUB(corrimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/corrimu", 5); + _PUB(corrimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/corrimu", qos_settings_); driver_.AddFpaObserver(fpa::FpaCorrimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaCorrimu(dynamic_cast(payload), corrimu_pub_); }); @@ -243,7 +243,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-TEXT if (params_.MessageEnabled(fpa::FpaTextPayload::MSG_NAME)) { - _PUB(fpa_text_pub_, fpmsgs::FpaText, output_ns + "/fpa/text", 5); + _PUB(fpa_text_pub_, fpmsgs::FpaText, output_ns + "/fpa/text", qos_settings_); driver_.AddFpaObserver(fpa::FpaTextPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaText(dynamic_cast(payload), fpa_text_pub_); }); @@ -251,7 +251,7 @@ bool FixpositionDriverNode::StartNode() { // FP_A-TP if (params_.MessageEnabled(fpa::FpaTpPayload::MSG_NAME)) { - _PUB(fpa_tp_pub_, fpmsgs::FpaTp, output_ns + "/fpa/tp", 5); + _PUB(fpa_tp_pub_, fpmsgs::FpaTp, output_ns + "/fpa/tp", qos_settings_); driver_.AddFpaObserver(fpa::FpaTpPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { PublishFpaTp(dynamic_cast(payload), fpa_tp_pub_); }); @@ -259,8 +259,8 @@ bool FixpositionDriverNode::StartNode() { // NOV_B-BESTGNSSPOS if (params_.MessageEnabled(novb::NOV_B_BESTGNSSPOS_STRID)) { - _PUB(navsatfix_gnss1_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss1", 5); - _PUB(navsatfix_gnss2_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss2", 5); + _PUB(navsatfix_gnss1_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss1", qos_settings_); + _PUB(navsatfix_gnss2_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss2", qos_settings_); driver_.AddNovbObserver( // novb::NOV_B_BESTGNSSPOS_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) { if (!PublishNovbBestgnsspos(header, (novb::NovbBestgnsspos*)payload, navsatfix_gnss1_pub_, @@ -272,7 +272,7 @@ bool FixpositionDriverNode::StartNode() { // NOV_B-INSPVAX if (params_.MessageEnabled(novb::NOV_B_INSPVAX_STRID)) { - _PUB(novb_inspvax_pub_, fpmsgs::NovbInspvax, output_ns + "/novb/inspvax", 5); + _PUB(novb_inspvax_pub_, fpmsgs::NovbInspvax, output_ns + "/novb/inspvax", qos_settings_); driver_.AddNovbObserver( // novb::NOV_B_INSPVAX_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) { if (!PublishNovbInspvax(header, (novb::NovbInspvax*)payload, novb_inspvax_pub_)) { @@ -284,7 +284,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GP-GGA if (params_.MessageEnabled(nmea::NmeaGgaPayload::FORMATTER)) { - _PUB(nmea_gga_pub_, fpmsgs::NmeaGga, output_ns + "/nmea/gga", 5); + _PUB(nmea_gga_pub_, fpmsgs::NmeaGga, output_ns + "/nmea/gga", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaGgaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto gga_payload = dynamic_cast(payload); PublishNmeaGga(gga_payload, nmea_gga_pub_); @@ -294,7 +294,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GP-GLL if (params_.MessageEnabled(nmea::NmeaGllPayload::FORMATTER)) { - _PUB(nmea_gll_pub_, fpmsgs::NmeaGll, output_ns + "/nmea/gll", 5); + _PUB(nmea_gll_pub_, fpmsgs::NmeaGll, output_ns + "/nmea/gll", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaGllPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto gll_payload = dynamic_cast(payload); PublishNmeaGll(gll_payload, nmea_gll_pub_); @@ -304,7 +304,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GN-GSA if (params_.MessageEnabled(nmea::NmeaGsaPayload::FORMATTER)) { - _PUB(nmea_gsa_pub_, fpmsgs::NmeaGsa, output_ns + "/nmea/gsa", 5); + _PUB(nmea_gsa_pub_, fpmsgs::NmeaGsa, output_ns + "/nmea/gsa", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaGsaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto gsa_payload_ = dynamic_cast(payload); PublishNmeaGsa(gsa_payload_, nmea_gsa_pub_); @@ -315,7 +315,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GP-GST if (params_.MessageEnabled(nmea::NmeaGstPayload::FORMATTER)) { - _PUB(nmea_gst_pub_, fpmsgs::NmeaGst, output_ns + "/nmea/gst", 5); + _PUB(nmea_gst_pub_, fpmsgs::NmeaGst, output_ns + "/nmea/gst", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaGstPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto gst_payload = dynamic_cast(payload); PublishNmeaGst(gst_payload, nmea_gst_pub_); @@ -325,7 +325,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GX-GSV if (params_.MessageEnabled(nmea::NmeaGsvPayload::FORMATTER)) { - _PUB(nmea_gsv_pub_, fpmsgs::NmeaGsv, output_ns + "/nmea/gsv", 5); + _PUB(nmea_gsv_pub_, fpmsgs::NmeaGsv, output_ns + "/nmea/gsv", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaGsvPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto gsv_payload_ = dynamic_cast(payload); PublishNmeaGsv(gsv_payload_, nmea_gsv_pub_); @@ -335,7 +335,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GP-HDT if (params_.MessageEnabled(nmea::NmeaHdtPayload::FORMATTER)) { - _PUB(nmea_hdt_pub_, fpmsgs::NmeaHdt, output_ns + "/nmea/hdt", 5); + _PUB(nmea_hdt_pub_, fpmsgs::NmeaHdt, output_ns + "/nmea/hdt", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaHdtPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto hdt_payload = dynamic_cast(payload); PublishNmeaHdt(hdt_payload, nmea_hdt_pub_); @@ -345,7 +345,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GP-RMC if (params_.MessageEnabled(nmea::NmeaRmcPayload::FORMATTER)) { - _PUB(nmea_rmc_pub_, fpmsgs::NmeaRmc, output_ns + "/nmea/rmc", 5); + _PUB(nmea_rmc_pub_, fpmsgs::NmeaRmc, output_ns + "/nmea/rmc", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaRmcPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto rmc_payload = dynamic_cast(payload); PublishNmeaRmc(rmc_payload, nmea_rmc_pub_); @@ -355,7 +355,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GP-VTG if (params_.MessageEnabled(nmea::NmeaVtgPayload::FORMATTER)) { - _PUB(nmea_vtg_pub_, fpmsgs::NmeaVtg, output_ns + "/nmea/vtg", 5); + _PUB(nmea_vtg_pub_, fpmsgs::NmeaVtg, output_ns + "/nmea/vtg", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaVtgPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto vtg_payload = dynamic_cast(payload); PublishNmeaVtg(vtg_payload, nmea_vtg_pub_); @@ -365,7 +365,7 @@ bool FixpositionDriverNode::StartNode() { // NMEA-GP-ZDA if (params_.MessageEnabled(nmea::NmeaZdaPayload::FORMATTER)) { - _PUB(nmea_zda_pub_, fpmsgs::NmeaZda, output_ns + "/nmea/zda", 5); + _PUB(nmea_zda_pub_, fpmsgs::NmeaZda, output_ns + "/nmea/zda", qos_settings_); driver_.AddNmeaObserver(nmea::NmeaZdaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) { auto zda_payload = dynamic_cast(payload); PublishNmeaZda(zda_payload, nmea_zda_pub_); @@ -375,25 +375,25 @@ bool FixpositionDriverNode::StartNode() { // Raw messages if (params_.raw_output_) { - _PUB(raw_pub_, fpmsgs::ParserMsg, output_ns + "/raw", 5); + _PUB(raw_pub_, fpmsgs::ParserMsg, output_ns + "/raw", qos_settings_); driver_.AddRawObserver([this](const parser::ParserMsg& msg) { PublishParserMsg(msg, raw_pub_); }); } // Fusion epoch if (params_.fusion_epoch_) { - _PUB(fusion_epoch_pub_, fpmsgs::FusionEpoch, output_ns + "/fusion", 5); + _PUB(fusion_epoch_pub_, fpmsgs::FusionEpoch, output_ns + "/fusion", qos_settings_); // Publish is triggered by FP_A-EOE above } // NMEA epoch if (params_.nmea_epoch_ != fpa::FpaEpoch::UNSPECIFIED) { - _PUB(nmea_epoch_pub_, fpmsgs::NmeaEpoch, output_ns + "/nmea", 5); + _PUB(nmea_epoch_pub_, fpmsgs::NmeaEpoch, output_ns + "/nmea", qos_settings_); // Publish is triggered by FP_A-EOE above } // Jump warning message if (params_.cov_warning_) { - _PUB(jump_pub_, fpmsgs::CovWarn, output_ns + "/extras/jump", 5); + _PUB(jump_pub_, fpmsgs::CovWarn, output_ns + "/extras/jump", qos_settings_); } // Subscribe to correction data input