diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index e4d70575..d0737c67 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include // 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::SharedPtr aid_eph_pub_; rclcpp::Publisher::SharedPtr aid_hui_pub_; rclcpp::Publisher::SharedPtr nmea_pub_; + rclcpp::Publisher::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}; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 85cd615e..b059a350 100644 --- a/ublox_gps/src/node.cpp +++ b/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", 20); } + if (getRosBoolean(this, "publish.versions")) { + rclcpp::QoS qos(1); + qos.transient_local(); + versions_pub_ = this->create_publisher("versions", qos); + } // Create subscriber for RTCM correction data to enable RTK this->subscription_ = this->create_subscription("/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 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(); diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 4e3db305..16b19b6d 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/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} diff --git a/ublox_msgs/msg/Versions.msg b/ublox_msgs/msg/Versions.msg new file mode 100644 index 00000000..099e9bba --- /dev/null +++ b/ublox_msgs/msg/Versions.msg @@ -0,0 +1,4 @@ +string software_version +string hardware_version +string firmware_version +string protocol_version