Feature: Add support for new HPS firmware and publishing NMEA Sentences (#209)

* Fix for NMEA messages overwhelming publishing queue

* Apply suggestions from code review

Co-authored-by: Chris Lalancette <[email protected]>

Author: LuckierDodge

ublox_gps/CMakeLists.txt

@@ -17,15 +17,16 @@ find_package(asio REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(nmea_msgs REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(rtcm_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(ublox_msgs REQUIRED)
 find_package(ublox_serialization REQUIRED)
-find_package(rtcm_msgs REQUIRED)
 
 # build node
 add_library(ublox_gps
@@ -58,6 +59,7 @@ target_link_libraries(ublox_gps PUBLIC
   ${rcl_interfaces_TARGETS}
   rclcpp::rclcpp
   rclcpp_components::component
+  ${nmea_msgs_TARGETS}
   ${rtcm_msgs_TARGETS}
   ${sensor_msgs_TARGETS}
   ${std_msgs_TARGETS}

ublox_gps/include/ublox_gps/callback.hpp

@@ -38,6 +38,7 @@
 #include <mutex>
 #include <sstream>
 #include <stdexcept>
+#include <string>
 #include <utility>
 
 #include <ublox_msgs/serialization.hpp>
@@ -163,6 +164,15 @@ class CallbackHandlers final {
                      std::make_shared<CallbackHandler_<T>>(callback, debug_)));
   }
 
+  /**
+   * @brief Add a callback handler for nmea messages
+   * @param callback the callback handler for the message
+   */
+  void set_nmea_callback(std::function<void(const std::string&)> callback) {
+    std::lock_guard<std::mutex> lock(callback_mutex_);
+    callback_nmea_ = callback;
+  }
+
   /**
    * @brief Calls the callback handler for the message in the reader.
    * @param reader a reader containing a u-blox message
@@ -178,6 +188,28 @@ class CallbackHandlers final {
     }
   }
 
+  /**
+   * @brief Calls the callback handler for the nmea messages in the reader.
+   * @param reader a reader containing an nmea message
+   */
+  void handle_nmea(ublox::Reader& reader) {
+    std::lock_guard<std::mutex> lock(callback_mutex_);
+    if (callback_nmea_ == nullptr) {
+      return;
+    }
+
+    const std::string buffer = reader.getExtraData();
+    size_t nmea_start = buffer.find('$', 0);
+    size_t nmea_end = buffer.find('\n', nmea_start);
+    while(nmea_start != std::string::npos && nmea_end != std::string::npos) {
+      std::string sentence = buffer.substr(nmea_start, nmea_end - nmea_start + 1);
+      callback_nmea_(sentence);
+
+      nmea_start = buffer.find('$', nmea_end + 1);
+      nmea_end = buffer.find('\n', nmea_start);
+    }
+  }
+
   /**
    * @brief Read a u-blox message of the given type.
    * @param message the received u-blox message
@@ -225,12 +257,13 @@ class CallbackHandlers final {
              it != reader.pos() + reader.length() + 8; ++it) {
           oss << std::hex << static_cast<unsigned int>(*it) << " ";
         }
-        // RCLCPP_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8,
+        // RCLCPP_DEBUG(logger_, "U-blox: reading %d bytes\n%s", reader.length() + 8,
         //           oss.str().c_str());
       }
 
       handle(reader);
     }
+    handle_nmea(reader);
 
     // delete read bytes from ASIO input buffer
     std::copy(reader.pos(), reader.end(), data);
@@ -246,6 +279,9 @@ class CallbackHandlers final {
   Callbacks callbacks_;
   std::mutex callback_mutex_;
   int debug_;
+
+  //! Callback handler for nmea messages
+  std::function<void(const std::string &)> callback_nmea_{nullptr};
 };
 
 }  // namespace ublox_gps

ublox_gps/include/ublox_gps/gps.hpp

@@ -35,6 +35,7 @@
 #include <locale>
 #include <map>
 #include <stdexcept>
+#include <string>
 #include <vector>
 
 #include <rclcpp/rclcpp.hpp>
@@ -350,6 +351,12 @@ class Gps final {
   template <typename T>
   void subscribe(typename CallbackHandler_<T>::Callback callback);
 
+  /**
+   * @brief Subscribe to the given Ublox message.
+   * @param callback the callback handler for the message
+   */
+  void subscribe_nmea(std::function<void(const std::string &)> callback);
+
   /**
    * @brief Subscribe to the message with the given ID. This is used for
    * messages which have the same format but different message IDs,

ublox_gps/include/ublox_gps/node.hpp

@@ -42,6 +42,7 @@
 #include <ublox_msgs/msg/cfg_dat.hpp>
 #include <ublox_msgs/msg/inf.h>
 #include <rtcm_msgs/msg/message.hpp>
+#include <nmea_msgs/msg/sentence.hpp>
 // Ublox GPS includes
 #include <ublox_gps/component_interface.hpp>
 #include <ublox_gps/fix_diagnostic.hpp>
@@ -269,6 +270,9 @@ class UbloxNode final : public rclcpp::Node {
   rclcpp::Publisher<ublox_msgs::msg::AidALM>::SharedPtr aid_alm_pub_;
   rclcpp::Publisher<ublox_msgs::msg::AidEPH>::SharedPtr aid_eph_pub_;
   rclcpp::Publisher<ublox_msgs::msg::AidHUI>::SharedPtr aid_hui_pub_;
+  rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_pub_;
+
+  void publish_nmea(const std::string & sentence, const std::string & topic);
 
   //! Navigation rate in measurement cycles, see CfgRate.msg
   uint16_t nav_rate_{0};

ublox_gps/package.xml

@@ -18,15 +18,16 @@
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
+  <depend>nmea_msgs</depend>
   <depend>rcl_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>rtcm_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>ublox_msgs</depend>
   <depend>ublox_serialization</depend>
-  <depend>rtcm_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

ublox_gps/src/gps.cpp

@@ -69,6 +69,10 @@ void Gps::setWorker(const std::shared_ptr<Worker>& worker) {
   configured_ = static_cast<bool>(worker);
 }
 
+void Gps::subscribe_nmea(std::function<void(const std::string &)> callback) {
+  callbacks_.set_nmea_callback(callback);
+}
+
 void Gps::subscribeAcks() {
   // Set NACK handler
   subscribeId<ublox_msgs::msg::Ack>(std::bind(&Gps::processNack, this,

ublox_gps/src/node.cpp

@@ -56,6 +56,8 @@
 #include <ublox_msgs/msg/nav_posecef.hpp>
 #include <ublox_msgs/msg/nav_status.hpp>
 
+#include <nmea_msgs/msg/sentence.hpp>
+
 #include <ublox_gps/adr_udr_product.hpp>
 #include <ublox_gps/fix_diagnostic.hpp>
 #include <ublox_gps/fts_product.hpp>
@@ -217,11 +219,11 @@ void UbloxNode::addFirmwareInterface() {
 
 void UbloxNode::addProductInterface(const std::string & product_category,
                                     const std::string & ref_rov) {
-  if (product_category == "HPG" && ref_rov == "REF") {
+  if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "REF") {
     components_.push_back(std::make_shared<HpgRefProduct>(nav_rate_, meas_rate_, updater_, rtcms_, this));
-  } else if (product_category == "HPG" && ref_rov == "ROV") {
+  } else if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "ROV") {
     components_.push_back(std::make_shared<HpgRovProduct>(nav_rate_, updater_, this));
-  } else if (product_category == "HPG") {
+  } else if (product_category == "HPG" || product_category == "HPS") {
     components_.push_back(std::make_shared<HpPosRecProduct>(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this));
   } else if (product_category == "TIM") {
     components_.push_back(std::make_shared<TimProduct>(frame_id_, updater_, this));
@@ -441,6 +443,8 @@ void UbloxNode::getRosParams() {
 
   this->declare_parameter("publish.tim.tm2", false);
 
+  this->declare_parameter("publish.nmea", true);
+
   // INF parameters
   this->declare_parameter("inf.all", true);
   this->declare_parameter("inf.debug", false);
@@ -489,6 +493,10 @@ void UbloxNode::getRosParams() {
   if (getRosBoolean(this, "publish.aid.hui")) {
     aid_hui_pub_ = this->create_publisher<ublox_msgs::msg::AidHUI>("aidhui", 1);
   }
+  if (getRosBoolean(this, "publish.nmea")) {
+    // Larger queue depth to handle all NMEA strings being published consecutively
+    nmea_pub_ = this->create_publisher<nmea_msgs::msg::Sentence>("nmea", 20);
+  }
 
   // Create subscriber for RTCM correction data to enable RTK
   this->subscription_ = this->create_subscription<rtcm_msgs::msg::Message>("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1));
@@ -606,6 +614,16 @@ void UbloxNode::subscribe() {
                                         1);
   }
 
+  if (getRosBoolean(this, "publish.nmea")) {
+    gps_->subscribe_nmea([this](const std::string &sentence) {
+      nmea_msgs::msg::Sentence m;
+      m.header.stamp = this->now();
+      m.header.frame_id = frame_id_;
+      m.sentence = sentence;
+      nmea_pub_->publish(m);
+    });
+  }
+
   for (const std::shared_ptr<ComponentInterface> & component : components_) {
     component->subscribe(gps_);
   }

ublox_serialization/include/ublox_serialization/serialization.hpp

@@ -33,6 +33,7 @@
 #include <array>
 #include <cstdint>
 #include <cstring>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -367,7 +368,10 @@ class Reader {
    */
   Reader(const uint8_t *data, uint32_t count,
          const Options &options = Options()) :
-      data_(data), count_(count), found_(false), options_(options) {}
+      data_(data), count_(count), found_(false), options_(options)
+  {
+    extra_data_.reserve(1024);
+  }
 
   using iterator = const uint8_t *;
 
@@ -386,6 +390,8 @@ class Reader {
       if (data_[0] == options_.sync_a &&
           (count_ == 1 || data_[1] == options_.sync_b)) {
         break;
+      } else {
+        extra_data_.push_back(data_[0]);
       }
     }
 
@@ -532,9 +538,15 @@ class Reader {
     return (classId() == class_id && messageId() == message_id);
   }
 
- private:
+  const std::string &getExtraData() const {
+    return extra_data_;
+  }
+
+private:
   //! The buffer of message bytes
   const uint8_t *data_;
+  //! Unused data from the read buffer, contains nmea messages.
+  std::string extra_data_;
   //! the number of bytes in the buffer, //! decrement as the buffer is read
   uint32_t count_;
   //! Whether or not a message has been found