Add support for forwarding RTCM correction data (#169)

Author: LuckierDodge

ublox_gps/CMakeLists.txt

@@ -25,6 +25,7 @@ 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
@@ -57,6 +58,7 @@ target_link_libraries(ublox_gps PUBLIC
   ${rcl_interfaces_TARGETS}
   rclcpp::rclcpp
   rclcpp_components::component
+  ${rtcm_msgs_TARGETS}
   ${sensor_msgs_TARGETS}
   ${std_msgs_TARGETS}
   tf2::tf2

ublox_gps/include/ublox_gps/gps.hpp

@@ -128,6 +128,12 @@ class Gps final {
    */
   void resetSerial(const std::string & port);
 
+  /**
+   * @brief Send rtcm correction messages to the connected device.
+   * @param message the RTCM correction data as a vector
+   */
+  bool sendRtcm(const std::vector<uint8_t> &message);
+
   /**
    * @brief Closes the I/O port, and initiates save on shutdown procedure
    * if enabled.

ublox_gps/include/ublox_gps/node.hpp

@@ -41,6 +41,7 @@
 #include <ublox_msgs/msg/cfg_cfg.hpp>
 #include <ublox_msgs/msg/cfg_dat.hpp>
 #include <ublox_msgs/msg/inf.h>
+#include <rtcm_msgs/msg/message.hpp>
 // Ublox GPS includes
 #include <ublox_gps/component_interface.hpp>
 #include <ublox_gps/fix_diagnostic.hpp>
@@ -136,6 +137,16 @@ class UbloxNode final : public rclcpp::Node {
 
  private:
 
+  /**
+   * @brief Callback for '/ntrip_client/rtcm' subscription to handle RTCM correction data
+   */
+  void rtcmCallback(const rtcm_msgs::msg::Message::SharedPtr msg);
+
+  /**
+   * @brief Subscription handler for RTCM data
+   */
+  rclcpp::Subscription<rtcm_msgs::msg::Message>::SharedPtr subscription_;
+
   /**
    * @brief Initialize the I/O handling.
    */

ublox_gps/package.xml

@@ -26,6 +26,7 @@
   <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

@@ -576,6 +576,11 @@ bool Gps::setUseAdr(bool enable) {
   return configure(msg);
 }
 
+bool Gps::sendRtcm(const std::vector<uint8_t>& rtcm) {
+  worker_->send(rtcm.data(), rtcm.size());
+  return true;
+}
+
 bool Gps::poll(uint8_t class_id, uint8_t message_id,
                const std::vector<uint8_t>& payload) {
   if (!worker_) {

ublox_gps/src/node.cpp

@@ -190,6 +190,10 @@ UbloxNode::UbloxNode(const rclcpp::NodeOptions & options) : rclcpp::Node("ublox_
   initialize();
 }
 
+void UbloxNode::rtcmCallback(const rtcm_msgs::msg::Message::SharedPtr msg) {
+  gps_->sendRtcm(msg->message);
+}
+
 void UbloxNode::addFirmwareInterface() {
   int ublox_version;
   if (protocol_version_ < 14.0) {
@@ -483,6 +487,9 @@ void UbloxNode::getRosParams() {
   if (getRosBoolean(this, "publish.aid.hui")) {
     aid_hui_pub_ = this->create_publisher<ublox_msgs::msg::AidHUI>("aidhui", 1);
   }
+
+  // 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));
 }
 
 void UbloxNode::keepAlive() {