Skip to content

Added QoS to ROS2 #81

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
fgarciacardenas merged 1 commit into from
Feb 12, 2025
Merged

Added QoS to ROS2 #81

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
72 changes: 36 additions & 36 deletions fixposition_driver_ros2/src/fixposition_driver_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<const fpa::FpaOdometryPayload&>(payload);
PublishFpaOdometry(odometry_payload, fpa_odometry_pub_);
Expand All @@ -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<const fpa::FpaOdomshPayload&>(payload);
PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_);
Expand All @@ -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<const fpa::FpaOdomenuPayload&>(payload);
PublishFpaOdomenu(odomenu_payload, fpa_odomenu_pub_);
Expand All @@ -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<const fpa::FpaOdomstatusPayload&>(payload);
PublishFpaOdomstatus(odomstatus_payload, fpa_odomstatus_pub_);
Expand All @@ -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<const fpa::FpaEoePayload&>(payload);
(void)eoe_payload;
Expand All @@ -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;
Expand All @@ -193,31 +193,31 @@ 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<const fpa::FpaLlhPayload&>(payload), fpa_llh_pub_);
});
}

// 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<const fpa::FpaGnssantPayload&>(payload), fpa_gnssant_pub_);
});
}

// 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<const fpa::FpaGnsscorrPayload&>(payload), fpa_gnsscorr_pub_);
});
}

// 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<const fpa::FpaImubiasPayload&>(payload);
PublishFpaImubias(imubias_payload, fpa_imubias_pub_);
Expand All @@ -227,40 +227,40 @@ 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<const fpa::FpaRawimuPayload&>(payload), rawimu_pub_);
});
}

// 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<const fpa::FpaCorrimuPayload&>(payload), corrimu_pub_);
});
}

// 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<const fpa::FpaTextPayload&>(payload), fpa_text_pub_);
});
}

// 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<const fpa::FpaTpPayload&>(payload), fpa_tp_pub_);
});
}

// 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_,
Expand All @@ -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_)) {
Expand All @@ -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<const nmea::NmeaGgaPayload&>(payload);
PublishNmeaGga(gga_payload, nmea_gga_pub_);
Expand All @@ -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<const nmea::NmeaGllPayload&>(payload);
PublishNmeaGll(gll_payload, nmea_gll_pub_);
Expand All @@ -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<const nmea::NmeaGsaPayload&>(payload);
PublishNmeaGsa(gsa_payload_, nmea_gsa_pub_);
Expand All @@ -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<const nmea::NmeaGstPayload&>(payload);
PublishNmeaGst(gst_payload, nmea_gst_pub_);
Expand All @@ -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<const nmea::NmeaGsvPayload&>(payload);
PublishNmeaGsv(gsv_payload_, nmea_gsv_pub_);
Expand All @@ -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<const nmea::NmeaHdtPayload&>(payload);
PublishNmeaHdt(hdt_payload, nmea_hdt_pub_);
Expand All @@ -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<const nmea::NmeaRmcPayload&>(payload);
PublishNmeaRmc(rmc_payload, nmea_rmc_pub_);
Expand All @@ -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<const nmea::NmeaVtgPayload&>(payload);
PublishNmeaVtg(vtg_payload, nmea_vtg_pub_);
Expand All @@ -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<const nmea::NmeaZdaPayload&>(payload);
PublishNmeaZda(zda_payload, nmea_zda_pub_);
Expand All @@ -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
Expand Down
Loading