From 0b5da36aa77533a35703fb58c68f2d704a961eb6 Mon Sep 17 00:00:00 2001 From: agennart Date: Fri, 9 Jan 2026 09:27:48 +0100 Subject: [PATCH] Subscribe to topic relative to the node name - This allows to run multiple Ublox node without having the same topic name defined twice. - This generalizes the topic name definition as the "fix", "fix_velocity" and "navpvt" were already using relative topic name. Signed-off-by: agennart --- .../include/ublox_gps/ublox_firmware7.hpp | 4 ++-- .../include/ublox_gps/ublox_firmware8.hpp | 6 +++--- ublox_gps/src/adr_udr_product.cpp | 16 ++++++++-------- ublox_gps/src/hp_pos_rec_product.cpp | 4 ++-- ublox_gps/src/hpg_ref_product.cpp | 2 +- ublox_gps/src/hpg_rov_product.cpp | 2 +- ublox_gps/src/node.cpp | 18 +++++++++--------- ublox_gps/src/raw_data_pa.cpp | 4 ++-- ublox_gps/src/raw_data_product.cpp | 8 ++++---- ublox_gps/src/tim_product.cpp | 8 ++++---- ublox_gps/src/ublox_firmware6.cpp | 10 +++++----- ublox_gps/src/ublox_firmware9.cpp | 6 +++--- 12 files changed, 44 insertions(+), 44 deletions(-) diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp index 6f6f95d4..3267195b 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp @@ -26,10 +26,10 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { if (getRosBoolean(node_, "publish.nav.svinfo")) { - nav_svinfo_pub_ = node->create_publisher("navsvinfo", 1); + nav_svinfo_pub_ = node->create_publisher("~/navsvinfo", 1); } if (getRosBoolean(node_, "publish.mon.hw")) { - mon_hw_pub_ = node->create_publisher("monhw", 1); + mon_hw_pub_ = node->create_publisher("~/monhw", 1); } } diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp index e5bbc895..c91ec621 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -26,13 +26,13 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { if (getRosBoolean(node_, "publish.nav.sat")) { - nav_sat_pub_ = node->create_publisher("navstate", 1); + nav_sat_pub_ = node->create_publisher("~/navstate", 1); } if (getRosBoolean(node_, "publish.mon.hw")) { - mon_hw_pub_ = node->create_publisher("monhw", 1); + mon_hw_pub_ = node->create_publisher("~/monhw", 1); } if (getRosBoolean(node_, "publish.rxm.rtcm")) { - rxm_rtcm_pub_ = node->create_publisher("rxmrtcm", 1); + rxm_rtcm_pub_ = node->create_publisher("~/rxmrtcm", 1); } } diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 7713ba7c..57cf680c 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -30,26 +30,26 @@ AdrUdrProduct::AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t { if (getRosBoolean(node_, "publish.esf.meas")) { imu_pub_ = - node_->create_publisher("imu_meas", 1); + node_->create_publisher("~/imu_meas", 1); time_ref_pub_ = - node_->create_publisher("interrupt_time", 1); + node_->create_publisher("~/interrupt_time", 1); - esf_meas_pub_ = node_->create_publisher("esfmeas", 1); + esf_meas_pub_ = node_->create_publisher("~/esfmeas", 1); } if (getRosBoolean(node_, "publish.nav.att")) { - nav_att_pub_ = node_->create_publisher("navatt", 1); + nav_att_pub_ = node_->create_publisher("~/navatt", 1); } if (getRosBoolean(node_, "publish.esf.ins")) { - esf_ins_pub_ = node_->create_publisher("esfins", 1); + esf_ins_pub_ = node_->create_publisher("~/esfins", 1); } if (getRosBoolean(node_, "publish.esf.raw")) { - esf_raw_pub_ = node_->create_publisher("esfraw", 1); + esf_raw_pub_ = node_->create_publisher("~/esfraw", 1); } if (getRosBoolean(node_, "publish.esf.status")) { - esf_status_pub_ = node_->create_publisher("esfstatus", 1); + esf_status_pub_ = node_->create_publisher("~/esfstatus", 1); } if (getRosBoolean(node_, "publish.hnr.pvt")) { - hnr_pvt_pub_ = node_->create_publisher("hnrpvt", 1); + hnr_pvt_pub_ = node_->create_publisher("~/hnrpvt", 1); } } diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp index 59d40570..9155c3a7 100644 --- a/ublox_gps/src/hp_pos_rec_product.cpp +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -25,12 +25,12 @@ HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const st { if (getRosBoolean(node_, "publish.nav.relposned")) { nav_relposned_pub_ = - node_->create_publisher("navrelposned", 1); + node_->create_publisher("~/navrelposned", 1); } if (getRosBoolean(node_, "publish.nav.heading")) { imu_pub_ = - node_->create_publisher("navheading", 1); + node_->create_publisher("~/navheading", 1); } } diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index 5fef3573..946c2a36 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -27,7 +27,7 @@ HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ { if (getRosBoolean(node_, "publish.nav.svin")) { navsvin_pub_ = - node_->create_publisher("navsvin", 1); + node_->create_publisher("~/navsvin", 1); } } diff --git a/ublox_gps/src/hpg_rov_product.cpp b/ublox_gps/src/hpg_rov_product.cpp index 76baef27..a846330c 100644 --- a/ublox_gps/src/hpg_rov_product.cpp +++ b/ublox_gps/src/hpg_rov_product.cpp @@ -25,7 +25,7 @@ HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptrcreate_publisher("navrelposned", 1); + node_->create_publisher("~/navrelposned", 1); } } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 39c14821..7d4e5df5 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -475,33 +475,33 @@ void UbloxNode::getRosParams() { // Create publishers based on parameters if (getRosBoolean(this, "publish.nav.status")) { - nav_status_pub_ = this->create_publisher("navstatus", 1); + nav_status_pub_ = this->create_publisher("~/navstatus", 1); } if (getRosBoolean(this, "publish.nav.posecef")) { - nav_posecef_pub_ = this->create_publisher("navposecef", 1); + nav_posecef_pub_ = this->create_publisher("~/navposecef", 1); } if (getRosBoolean(this, "publish.nav.cov")) { - nav_cov_pub_ = this->create_publisher("navcov", 1); + nav_cov_pub_ = this->create_publisher("~/navcov", 1); } if (getRosBoolean(this, "publish.nav.clock")) { - nav_clock_pub_ = this->create_publisher("navclock", 1); + nav_clock_pub_ = this->create_publisher("~/navclock", 1); } if (getRosBoolean(this, "publish.aid.alm")) { - aid_alm_pub_ = this->create_publisher("aidalm", 1); + aid_alm_pub_ = this->create_publisher("~/aidalm", 1); } if (getRosBoolean(this, "publish.aid.eph")) { - aid_eph_pub_ = this->create_publisher("aideph", 1); + aid_eph_pub_ = this->create_publisher("~/aideph", 1); } if (getRosBoolean(this, "publish.aid.hui")) { - aid_hui_pub_ = this->create_publisher("aidhui", 1); + aid_hui_pub_ = this->create_publisher("~/aidhui", 1); } if (getRosBoolean(this, "publish.nmea")) { // Larger queue depth to handle all NMEA strings being published consecutively - nmea_pub_ = this->create_publisher("nmea", 20); + nmea_pub_ = this->create_publisher("~/nmea", 20); } // Create subscriber for RTCM correction data to enable RTK - this->subscription_ = this->create_subscription("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); + this->subscription_ = this->create_subscription("~/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); } void UbloxNode::keepAlive() { diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index dedd7bb9..cd739236 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -58,7 +58,7 @@ RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : rclcpp::Node("raw_dat flag_publish_(false), is_ros_subscriber_(is_ros_subscriber) { - raw_pub_ = this->create_publisher("raw_data_stream", 100); + raw_pub_ = this->create_publisher("~/raw_data_stream", 100); this->declare_parameter("dir", ""); this->declare_parameter("raw_data_stream.dir", ""); @@ -89,7 +89,7 @@ void RawDataStreamPa::initialize() { if (is_ros_subscriber_) { RCLCPP_INFO(this->get_logger(), "Subscribing to raw data stream."); raw_data_stream_sub_ = - this->create_subscription("raw_data_stream", rclcpp::QoS(100), + this->create_subscription("~/raw_data_stream", rclcpp::QoS(100), std::bind(&RawDataStreamPa::msgCallback, this, std::placeholders::_1)); } else if (flag_publish_) { RCLCPP_INFO(this->get_logger(), "Publishing raw data stream."); diff --git a/ublox_gps/src/raw_data_product.cpp b/ublox_gps/src/raw_data_product.cpp index 23c953aa..fc3fc47c 100644 --- a/ublox_gps/src/raw_data_product.cpp +++ b/ublox_gps/src/raw_data_product.cpp @@ -21,16 +21,16 @@ namespace ublox_node { RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node) : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { if (getRosBoolean(node_, "publish.rxm.raw")) { - rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + rxm_raw_pub_ = node_->create_publisher("~/rxmraw", 1); } if (getRosBoolean(node_, "publish.rxm.sfrb")) { - rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + rxm_sfrb_pub_ = node_->create_publisher("~/rxmsfrb", 1); } if (getRosBoolean(node_, "publish.rxm.eph")) { - rxm_eph_pub_ = node_->create_publisher("rxmeph", 1); + rxm_eph_pub_ = node_->create_publisher("~/rxmeph", 1); } if (getRosBoolean(node_, "publish.rxm.almRaw")) { - rxm_alm_pub_ = node_->create_publisher("rxmalm", 1); + rxm_alm_pub_ = node_->create_publisher("~/rxmalm", 1); } } diff --git a/ublox_gps/src/tim_product.cpp b/ublox_gps/src/tim_product.cpp index 3f9000f1..5f98e52c 100644 --- a/ublox_gps/src/tim_product.cpp +++ b/ublox_gps/src/tim_product.cpp @@ -23,15 +23,15 @@ namespace ublox_node { TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) : frame_id_(frame_id), updater_(updater), node_(node) { timtm2_pub_ = - node_->create_publisher("timtm2", 1); + node_->create_publisher("~/timtm2", 1); interrupt_time_pub_ = - node_->create_publisher("interrupt_time", 1); + node_->create_publisher("~/interrupt_time", 1); if (getRosBoolean(node_, "publish.rxm.sfrb")) { - rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + rxm_sfrb_pub_ = node_->create_publisher("~/rxmsfrb", 1); } if (getRosBoolean(node_, "publish.rxm.raw")) { - rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + rxm_raw_pub_ = node_->create_publisher("~/rxmraw", 1); } } diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp index 65922b82..62369dee 100644 --- a/ublox_gps/src/ublox_firmware6.cpp +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -32,7 +32,7 @@ UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptrcreate_publisher("navposllh", 1); + node_->create_publisher("~/navposllh", 1); } fix_pub_ = @@ -40,7 +40,7 @@ UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptrcreate_publisher("navvelned", 1); + node_->create_publisher("~/navvelned", 1); } vel_pub_ = @@ -49,15 +49,15 @@ UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptrcreate_publisher("navsol", 1); + node_->create_publisher("~/navsol", 1); } if (getRosBoolean(node_, "publish.nav.svinfo")) { nav_svinfo_pub_ = - node_->create_publisher("navinfo", 1); + node_->create_publisher("~/navinfo", 1); } if (getRosBoolean(node_, "publish.mon.hw")) { mon_hw_pub_ = - node_->create_publisher("monhw", 1); + node_->create_publisher("~/monhw", 1); } } diff --git a/ublox_gps/src/ublox_firmware9.cpp b/ublox_gps/src/ublox_firmware9.cpp index 1a51a021..484008f1 100644 --- a/ublox_gps/src/ublox_firmware9.cpp +++ b/ublox_gps/src/ublox_firmware9.cpp @@ -17,11 +17,11 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptrcreate_publisher("navtimegps", 1); + nav_timegps_pub_ = node->create_publisher("~/navtimegps", 1); } if (getRosBoolean(node_, "publish.nav.timeutc")) { - nav_timeutc_pub_ = node->create_publisher("navtimeutc", 1); + nav_timeutc_pub_ = node->create_publisher("~/navtimeutc", 1); } } @@ -127,7 +127,7 @@ ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t } void UbloxFirmware9::subscribe(std::shared_ptr gps) -{ +{ UbloxFirmware8::subscribe(gps); // Subscribe to NAV TIMEGPS messages