Add dual antenna diagnostics (#124)

* Update the parameters in the README * Added RXSTATUS message and new Second Antenna diagnostic
swri-robotics · Nov 8, 2024 · d72bba3 · d72bba3
1 parent fdcad6d
commit d72bba3
Show file tree

Hide file tree

Showing 10 changed files with 274 additions and 1 deletion.
diff --git a/README.md b/README.md
@@ -134,7 +134,7 @@ Nodelets
         - `publish_clocksteering`: `true` to publish novatel_gps_msgs/ClockSteering messages.
             - Default: `false`
         - `publish_diagnostics`: `true` to publish node diagnostics.
-            - Default: `false`
+            - Default: `true`
         - `publish_gpgsa`: `true` to publish novatel_gps_msgs/Gpgsa messages.
             - Default: `false`
         - `publish_gpgsv`: `true` to publish novatel_gps_msgs/Gpgsv messages.
@@ -170,10 +170,15 @@ Nodelets
         - `publish_sync_diagnostic`: If true, publish a time Sync diagnostic.
             - Ignored if `publish_diagnostics` is false.
             - Default: `true`
+        - `publish_dual_antenna_diagnostic`: If true, publish diagnostics for the second antenna.
+            - Ignored if `publish_diagnostics` is false.
+            - Default: same as `publish_novatel_dual_antenna_heading`
         - `publish_time_messages`: `true` to publish novatel_gps_msgs/Time messages.
             - Default: `false`
         - `publish_trackstat`: `true` to publish novatel_gps_msgs/Trackstat messages.
             - Default: `false`
+        - `publish_invalid_gpsfix`: `true` to publish the `/gps` topic, even if the status of the GPS fix is `STATUS_NO_FIX`.
+            - Default: `false`
         - `reconnect_delay_s`: Second delay between reconnection attempts.
             - Default: `0.5`
         - `serial_baud`: Select the serial baud rate to be used in a serial connection.

diff --git a/novatel_gps_driver/CMakeLists.txt b/novatel_gps_driver/CMakeLists.txt
@@ -60,6 +60,7 @@ add_library(${PROJECT_NAME}
   src/parsers/range.cpp
   src/parsers/time.cpp
   src/parsers/trackstat.cpp
+  src/parsers/rxstatus.cpp
 )
 set_property(TARGET ${PROJECT_NAME}
   PROPERTY POSITION_INDEPENDENT_CODE ON)

diff --git a/novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.h b/novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.h
@@ -220,6 +220,7 @@ namespace novatel_gps_driver
     bool publish_trackstat_;
     bool publish_diagnostics_;
     bool publish_sync_diagnostic_;
+    bool publish_dual_antenna_diagnostic_;
     bool publish_invalid_gpsfix_;
     double reconnect_delay_s_;
     bool use_binary_messages_;
@@ -288,6 +289,11 @@ namespace novatel_gps_driver
     int32_t measurement_count_;
     rclcpp::Time last_published_;
     novatel_gps_msgs::msg::NovatelPosition::SharedPtr last_novatel_position_;
+    // ROS Dual Antenna diagnostics
+    uint32_t aux1stat_;
+    uint32_t aux2stat_;
+    uint32_t aux3stat_;
+    uint32_t aux4stat_;
 
     std::string imu_frame_id_;
     std::string frame_id_;
@@ -327,6 +333,8 @@ namespace novatel_gps_driver
 
     void RateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);
 
+    void DualAntennaDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);
+
     rclcpp::Time NovatelTimeToLocalTime(const TimeParserMsgT & utc_time);
   };
 }

diff --git a/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h b/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h
@@ -81,6 +81,7 @@
 #include <novatel_gps_driver/parsers/psrdop2.h>
 #include <novatel_gps_driver/parsers/time.h>
 #include <novatel_gps_driver/parsers/trackstat.h>
+#include <novatel_gps_driver/parsers/rxstatus.h>
 
 namespace novatel_gps_driver
 {
@@ -275,6 +276,12 @@ namespace novatel_gps_driver
        * @param[out] clocksteering_msgs New CLOCKSTEERING messages.
        */
       void GetClockSteeringMessages(std::vector<novatel_gps_driver::ClockSteeringParser::MessageType>& clocksteering_msgs);
+      /**
+       * @brief Provides any RXSTATUS messages that have been received since the
+       * last time this was called.
+       * @param[out] rxstatus_msgs New RXSTATUS messages.
+       */
+      void GetRxStatusMessages(std::vector<novatel_gps_driver::RxStatusParser::MessageType>& rxstatus_msgs);
 
       /**
        * @return true if we are connected to a NovAtel device, false otherwise.
@@ -504,6 +511,7 @@ namespace novatel_gps_driver
       RangeParser range_parser_;
       TimeParser time_parser_;
       TrackstatParser trackstat_parser_;
+      RxStatusParser rxstatus_parser_;
 
       // Message buffers
       boost::circular_buffer<novatel_gps_driver::ClockSteeringParser::MessageType> clocksteering_msgs_;
@@ -530,6 +538,7 @@ namespace novatel_gps_driver
       boost::circular_buffer<novatel_gps_driver::RangeParser::MessageType> range_msgs_;
       boost::circular_buffer<novatel_gps_driver::TimeParser::MessageType> time_msgs_;
       boost::circular_buffer<novatel_gps_driver::TrackstatParser::MessageType> trackstat_msgs_;
+      boost::circular_buffer<novatel_gps_driver::RxStatusParser::MessageType> rxstatus_msgs_;
 
       novatel_gps_driver::Psrdop2Parser::MessageType latest_psrdop2_;
 

diff --git a/novatel_gps_driver/include/novatel_gps_driver/parsers/rxstatus.h b/novatel_gps_driver/include/novatel_gps_driver/parsers/rxstatus.h
@@ -0,0 +1,58 @@
+// *****************************************************************************
+//
+// Copyright (c) 2019, Southwest Research Institute® (SwRI®)
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// *****************************************************************************
+
+#ifndef NOVATEL_GPS_DRIVER_RXSTATUS_H
+#define NOVATEL_GPS_DRIVER_RXSTATUS_H
+
+#include <novatel_gps_msgs/msg/novatel_rx_status.hpp>
+
+#include <novatel_gps_driver/parsers/parsing_utils.h>
+#include <novatel_gps_driver/parsers/message_parser.h>
+
+namespace novatel_gps_driver
+{
+  class RxStatusParser : public MessageParser<novatel_gps_msgs::msg::NovatelRxStatus::SharedPtr>
+  {
+  public:
+    uint32_t GetMessageId() const override;
+
+    const std::string GetMessageName() const override;
+
+    MessageType ParseBinary(const BinaryMessage& bin_msg) noexcept(false) override;
+
+    MessageType ParseAscii(const NovatelSentence& sentence) noexcept(false) override;
+
+    static constexpr uint16_t MESSAGE_ID = 93;
+    static constexpr size_t BINARY_LENGTH = 88;
+    static constexpr size_t ASCII_LENGTH = 22;
+    static const std::string MESSAGE_NAME;
+  };
+}
+
+#endif //NOVATEL_GPS_DRIVER_RXSTATUS_H
diff --git a/novatel_gps_driver/src/nodes/novatel_gps_node.cpp b/novatel_gps_driver/src/nodes/novatel_gps_node.cpp
@@ -27,6 +27,7 @@
 //
 // *****************************************************************************
 
+#include "novatel_gps_driver/parsers/rxstatus.h"
 #include <novatel_gps_driver/nodes/novatel_gps_node.h>
 
 #include <swri_math_util/math_util.h>
@@ -70,6 +71,7 @@ namespace novatel_gps_driver
       publish_trackstat_(false),
       publish_diagnostics_(true),
       publish_sync_diagnostic_(true),
+      publish_dual_antenna_diagnostic_(publish_novatel_dual_antenna_heading_),
       publish_invalid_gpsfix_(false),
       reconnect_delay_s_(0.5),
       use_binary_messages_(false),
@@ -86,6 +88,10 @@ namespace novatel_gps_driver
       gps_insufficient_data_warnings_(0),
       publish_rate_warnings_(0),
       measurement_count_(0),
+      aux1stat_(-1),
+      aux2stat_(-1),
+      aux3stat_(-1),
+      aux4stat_(-1),
       last_published_(get_clock()->get_clock_type()),
       imu_frame_id_(""),
       frame_id_("")
@@ -118,6 +124,7 @@ namespace novatel_gps_driver
     publish_trackstat_ = this->declare_parameter("publish_trackstat", publish_trackstat_);
     publish_diagnostics_ = this->declare_parameter("publish_diagnostics", publish_diagnostics_);
     publish_sync_diagnostic_ = this->declare_parameter("publish_sync_diagnostic", publish_sync_diagnostic_);
+    publish_dual_antenna_diagnostic_ = this->declare_parameter("publish_dual_antenna_diagnostic", publish_dual_antenna_diagnostic_);
     polling_period_ = this->declare_parameter("polling_period", polling_period_);
     expected_rate_ = this->declare_parameter("expected_rate", 1.0 / polling_period_);
     reconnect_delay_s_ = this->declare_parameter("reconnect_delay_s", reconnect_delay_s_);
@@ -275,6 +282,13 @@ namespace novatel_gps_driver
                                 this,
                                 &NovatelGpsNode::SyncDiagnostic);
       }
+
+      if (publish_dual_antenna_diagnostic_)
+      {
+        diagnostic_updater_.add("Dual Antenna",
+                                this,
+                                &NovatelGpsNode::DualAntennaDiagnostic);
+      }
     }
 
 
@@ -386,6 +400,10 @@ namespace novatel_gps_driver
     {
       opts["trackstat" + format_suffix] = 1.0;  // Trackstat
     }
+    if (publish_dual_antenna_diagnostic_ && publish_diagnostics_)
+    {
+      opts["rxstatus" + format_suffix] = 1.0;
+    }
     // Set the serial baud rate if needed
     if (connection_ == NovatelGps::SERIAL)
     {
@@ -777,6 +795,18 @@ namespace novatel_gps_driver
         trackstat_pub_->publish(std::move(msg));
       }
     }
+    if (publish_dual_antenna_diagnostic_)
+    {
+      std::vector<novatel_gps_driver::RxStatusParser::MessageType> rxstatus_msgs;
+      gps_.GetRxStatusMessages(rxstatus_msgs);
+      for (auto& msg : rxstatus_msgs)
+      {
+          aux1stat_ = msg->aux1stat;
+          aux2stat_ = msg->aux2stat;
+          aux3stat_ = msg->aux3stat;
+          aux4stat_ = msg->aux4stat;
+      }
+    }
     if (publish_imu_messages_)
     {
       std::vector<novatel_gps_driver::CorrImuDataParser::MessageType> novatel_imu_msgs;
@@ -1168,6 +1198,28 @@ namespace novatel_gps_driver
     publish_rate_warnings_ = 0;
   }
 
+  void NovatelGpsNode::DualAntennaDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status)
+  {
+    bool powered = aux2stat_ & 0x10000000;
+    bool open = aux2stat_ & 0x20000000;
+    bool shorted = aux2stat_ & 0x40000000;
+
+    if (open || shorted)
+    {
+      status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Second Antenna Connection Error");
+    }
+    else 
+    {
+      status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nominal");
+    }
+
+    status.add("Second Antenna Not Powered", powered ? "false" : "true");
+    status.add("Second Antenna Open", aux2stat_ & 0x20000000 ? "true" : "false");
+    status.add("Second Antenna Shorted", aux2stat_ & 0x40000000 ? "true" : "false");
+
+    status.add("aux2stat", aux2stat_);
+  }
+
   rclcpp::Time NovatelGpsNode::NovatelTimeToLocalTime(const TimeParserMsgT & time_msg)
   {
     // Build tm struct from NovatelTime

diff --git a/novatel_gps_driver/src/novatel_gps.cpp b/novatel_gps_driver/src/novatel_gps.cpp
@@ -27,6 +27,7 @@
 //
 // *****************************************************************************
 
+#include "novatel_gps_driver/parsers/rxstatus.h"
 #include <iomanip>
 #include <sstream>
 #include <net/ethernet.h>
@@ -86,6 +87,7 @@ namespace novatel_gps_driver
       range_msgs_(MAX_BUFFER_SIZE),
       time_msgs_(MAX_BUFFER_SIZE),
       trackstat_msgs_(MAX_BUFFER_SIZE),
+      rxstatus_msgs_(MAX_BUFFER_SIZE),
       imu_rate_(-1.0),
       apply_vehicle_body_rotation_(false)
   {
@@ -532,6 +534,11 @@ namespace novatel_gps_driver
     DrainQueue(clocksteering_msgs_, clocksteering_msgs);
   }
 
+  void NovatelGps::GetRxStatusMessages(std::vector<novatel_gps_driver::RxStatusParser::MessageType>& rxstatus_msgs)
+  {
+    DrainQueue(rxstatus_msgs_, rxstatus_msgs);
+  }
+
   bool NovatelGps::CreatePcapConnection(const std::string& device, NovatelMessageOpts const& opts)
   {
     RCLCPP_INFO(node_.get_logger(), "Opening pcap file: %s", device.c_str());
@@ -1139,6 +1146,13 @@ namespace novatel_gps_driver
         trackstat_msgs_.push_back(std::move(trackstat));
         break;
       }
+      case RxStatusParser::MESSAGE_ID:
+      {
+        auto rxstatus = rxstatus_parser_.ParseBinary(msg);
+        rxstatus->header.stamp = stamp;
+        rxstatus_msgs_.push_back(std::move(rxstatus));
+        break;
+      }
       default:
         RCLCPP_WARN(node_.get_logger(), "Unexpected binary message id: %u", msg.header_.message_id_);
         break;
@@ -1320,6 +1334,12 @@ namespace novatel_gps_driver
       trackstat->header.stamp = stamp;
       trackstat_msgs_.push_back(std::move(trackstat));
     }
+    else if (sentence.id == "RXSTATUSA")
+    {
+      auto rxstatus = rxstatus_parser_.ParseAscii(sentence);
+      rxstatus->header.stamp = stamp;
+      rxstatus_msgs_.push_back(std::move(rxstatus));
+    }
     else if (sentence.id == "RAWIMUXA")
     {
       static std::map<std::string, std::pair<double, std::string>> rates = {