Publish current versions

Publish the different versions received from the ublox device on a
topic with local durability policy. This is useful to allow for a
control node to check whether the different hardware components have the
expected versions.

Author: agennart

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 <ublox_msgs/msg/versions.hpp>
 #include <rtcm_msgs/msg/message.hpp>
 #include <nmea_msgs/msg/sentence.hpp>
 // Ublox GPS includes
@@ -217,6 +218,10 @@ class UbloxNode final : public rclcpp::Node {
 
   //! Determined From Mon VER
   float protocol_version_ = 0.0;
+  std::string hardware_version_ = "";
+  std::string software_version_ = "";
+  //! Determined From FWVER
+  std::string firmware_version_ = "";
   // Variables set from parameter server
   //! Device port
   std::string device_;
@@ -271,9 +276,12 @@ class UbloxNode final : public rclcpp::Node {
   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_;
+  rclcpp::Publisher<ublox_msgs::msg::Versions>::SharedPtr versions_pub_;
 
   void publish_nmea(const std::string & sentence, const std::string & topic);
 
+  void publishVersions();
+
   //! Navigation rate in measurement cycles, see CfgRate.msg
   uint16_t nav_rate_{0};
 

ublox_gps/src/node.cpp

@@ -444,6 +444,7 @@ void UbloxNode::getRosParams() {
   this->declare_parameter("publish.tim.tm2", false);
 
   this->declare_parameter("publish.nmea", true);
+  this->declare_parameter("publish.versions", true);
 
   // INF parameters
   this->declare_parameter("inf.all", true);
@@ -497,6 +498,11 @@ void UbloxNode::getRosParams() {
     // Larger queue depth to handle all NMEA strings being published consecutively
     nmea_pub_ = this->create_publisher<nmea_msgs::msg::Sentence>("nmea", 20);
   }
+  if (getRosBoolean(this, "publish.versions")) {
+    rclcpp::QoS qos(1);
+    qos.transient_local();
+    versions_pub_ = this->create_publisher<ublox_msgs::msg::Versions>("versions", qos);
+  }
 
   // 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));
@@ -641,9 +647,11 @@ void UbloxNode::processMonVer() {
     throw std::runtime_error("Failed to poll MonVER & set relevant settings");
   }
 
+  software_version_ = std::string(monVer.sw_version.begin(), monVer.sw_version.end());
+  hardware_version_ = std::string(monVer.hw_version.begin(), monVer.hw_version.end());
   RCLCPP_INFO(this->get_logger(), "%s, HW VER: %s",
-              std::string(monVer.sw_version.begin(), monVer.sw_version.end()).c_str(),
-              std::string(monVer.hw_version.begin(), monVer.hw_version.end()).c_str());
+              software_version_.c_str(),
+              hardware_version_.c_str());
   // Convert extension to vector of strings
   std::vector<std::string> extensions;
   extensions.reserve(monVer.extension.size());
@@ -694,6 +702,8 @@ void UbloxNode::processMonVer() {
         strs = stringSplit(extensions[i], "=");
         if (strs.size() > 1) {
           if (strs[0] == "FWVER") {
+            firmware_version_ = strs[1];
+            RCLCPP_INFO(this->get_logger(), "FWVER: %s", firmware_version_.c_str());
             if (strs[1].length() > 8) {
               addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10));
             } else {
@@ -729,6 +739,7 @@ void UbloxNode::processMonVer() {
       }
     }
   }
+  publishVersions();
 }
 
 bool UbloxNode::configureUblox() {
@@ -925,6 +936,15 @@ void UbloxNode::initialize() {
   }
 }
 
+void UbloxNode::publishVersions() {
+  ublox_msgs::msg::Versions msg;
+  msg.hardware_version = hardware_version_;
+  msg.software_version = software_version_;
+  msg.firmware_version = firmware_version_;
+  msg.protocol_version = std::to_string(protocol_version_);
+  versions_pub_->publish(msg);
+}
+
 void UbloxNode::shutdown() {
   if (gps_->isInitialized()) {
     gps_->close();

ublox_msgs/CMakeLists.txt

@@ -99,6 +99,7 @@ set(msg_files
   "msg/TimTM2.msg"
   "msg/UpdSOSAck.msg"
   "msg/UpdSOS.msg"
+  "msg/Versions.msg"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}

ublox_msgs/msg/Versions.msg

@@ -0,0 +1,4 @@
+string software_version
+string hardware_version
+string firmware_version
+string protocol_version