Refactor GPSFix generation

Previously, GPSFix messages were generated by synchronizing GPGGA and
GPRMC logs, optionally filling in a few fields from BESTPOS logs when
available.

This will instead produce more accurate GPSFix message with more
fields by instead basing them on BESTPOS logs and synchronizing them
with BESTVEL and PSRDOP2 logs when possible.

Distribution Statement A; OPSEC #2893

Signed-off-by: P. J. Reed <preed@swri.org>

README.md

@@ -14,9 +14,9 @@ Features:
 - Supports a variety of common NovAtel logs
 - Easy to add support for more log types
 - Supports ASCII and binary-format NovAtel logs
-- Can synchronize `GPGGA`, `GPRMC`, and `BESTPOS` logs together in order to produce 
+- Can synchronize `BESTPOS`, `BESTVEL`, and `PSRDOP2` logs together in order to produce 
 [gps_common/GPSFix](http://docs.ros.org/kinetic/api/gps_common/html/msg/GPSFix.html) messages
-- Compatible with OEM4, OEM6, and OEM7 receivers
+- Tested with OEM4, OEM6, and OEM7 receivers
 - Can produce IMU data from receives with SPAN support
 
 It has been tested primarily on NovAtel OEM628 receivers, but it has been used with
@@ -70,6 +70,8 @@ def generate_launch_description():
                     'device': '/dev/ttyUSB0',
                     'verbose': True,
                     'publish_novatel_positions': True,
+                    'publish_novatel_velocity': True,
+                    'publish_novatel_psrdop2': True,
                     'frame_id': '/gps'
                 }]
             )
@@ -116,10 +118,6 @@ Nodelets
             - Default: Empty
         - `frame_id`: ROS TF frame to place in the header of published messages.
             - Default: Empty
-        - `gpgga_gprmc_sync_tol`: Tolerance (in seconds) for synchronizing GPGGA and GPRMC logs.
-            - Default: `0.01`
-        - `gpgga_position_sync_tol`: Tolerance (in seconds) for synchronizing GPGGA and BESTPOS logs.
-            - Default `0.01`
         - `imu_frame_id`: TF frame id to use in IMU messages.
             - Default: Empty
         - `imu_rate`: Desired logging rate in Hz for IMU messages.
@@ -149,11 +147,18 @@ Nodelets
             - Default: `false`
         - `publish_novatel_heading2`: `true` to publish novatel_gps_msgs/Heading2 messages.
             - Default: `false`
-        - `publish_novatel_positions`: `true` to publish novatel_gps_msgs/NovatelPosition messages.
+        - `publish_novatel_positions`: `true` to publish novatel_gps_msgs/NovatelPosition messages.  Note that even if
+        this is false, these logs will always be requested from the receiver in order to generate `gps_msgs/GPSFix`
+        messages.
+            - Default: `false`
+        - `publish_novatel_psrdop2`: `true` to publish novatel_gps_msgs/NovatelPsrdop2 messages.  If set, the data from
+        these messages will be used to fill in the DoP values in `gps_msgs/GPSFix` messages.  Note that these messages
+        are only published when the values change, not at the standard polling rate.
             - Default: `false`
         - `publish_novatel_utm_positions`: `true` to publish novatel_gps_msgs/NovatelUtmPosition messages.
             - Default: `false`
-        - `publish_novatel_velocity`: `true` to publish novatel_gps_msgs/NovatelVelocity messages.
+        - `publish_novatel_velocity`: `true` to publish novatel_gps_msgs/NovatelVelocity messages.  If set, the data
+        from these messages will be used to fill in the speed and track values in `gps_msgs/GPSFix` messages.
             - Default: `false`
         - `publish_novatel_xyz_positions`: `true` to publish novatel_gps_msgs/NovatelXYZ messages.
             - Default: `false`
@@ -176,8 +181,11 @@ Nodelets
             - Binary logs are much more efficient and effectively required for IMU data,
             but ASCII logs are easier to parse for a human.
             - Default: `false`
-        - `wait_for_position`: `true` in order to wait for BESTPOS logs before publishing GPSFix messages.
-            - Default: `false`
+        - `wait_for_sync`: `true` in order to wait for both BESTPOS and BESTVEL messages to arrive before publishing
+        `gps_msgs/GPSFix` messages.  If this is `false`, GPSFix messages will be published immediately when BESTPOS
+        messages are received, but a side effect is that the driver will often be unable to fill in the speed & track
+        fields.  This has no effect if `publish_novatel_velocity` is `false`.
+            - Default: `true`
     2. **ROS Topic Subscriptions**
         - `/gps_sync` *(std_msgs/Time)*: *(optional)* Timestamped sync pulses from a DIO module. 
     These are used to improve the accuracy of the time stamps of the messages published.
@@ -195,13 +203,14 @@ Nodelets
         - `/gpgsa` *(novatel_gps_msgs/Gpgsa)*: [GPGSA](http://docs.novatel.com/OEM7/Content/Logs/GPGSA.htm) logs
         - `/gpgsv` *(novatel_gps_msgs/Gpgsv)*: [GPGSV](http://docs.novatel.com/OEM7/Content/Logs/GPGSV.htm) logs
         - `/gprmc` *(novatel_gps_msgs/Gprmc)*: [GPRMC](http://docs.novatel.com/OEM7/Content/Logs/GPRMC.htm) logs
-        - `/gps` *([gps_common/GPSFix](http://docs.ros.org/kinetic/api/gps_common/html/msg/GPSFix.html))*: Fixes produced by combining GPGGA, GPRMC, and BESTPOS messages together
+        - `/gps` *([gps_msgs/GPSFix](http://docs.ros.org/kinetic/api/gps_common/html/msg/GPSFix.html))*: Fixes produced by combining GPGGA, GPRMC, and BESTPOS messages together
             - **Note**:  GPSFix messages will always be published regardless of what other types are enabled.        
         - `/heading2` *(novatel_gps_msgs/NovatelHeadin2)*: [HEADING2](http://docs.novatel.com/OEM7/Content/Logs/HEADING2.htm) logs
         - `/imu` *([sensor_msgs/Imu](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html))*: CORRIMUDATA logs converted to Imu messages
         - `/inspva` *(novatel_gps_msgs/Inspva)*: [INSPVA](http://docs.novatel.com/OEM7/Content/SPAN_Logs/INSPVA.htm) logs
         - `/inspvax` *(novatel_gps_msgs/Inspvax)*: [INSPVAX](http://docs.novatel.com/OEM7/Content/SPAN_Logs/INSPVAX.htm) logs
         - `/insstdev` *(novatel_gps_msgs/Insstdev)*: [INSSTDEV](http://docs.novatel.com/OEM7/Content/SPAN_Logs/INSSTDEV.htm) logs
+        - `/psrdop2` *(novatel_gps_msgs/Psrdop2)*: [PSRDOP2](https://docs.novatel.com/OEM7/Content/Logs/PSRDOP2.htm) logs
         - `/range` *(novatel_gps_msgs/Range)*: [RANGE](http://docs.novatel.com/OEM7/Content/Logs/RANGE.htm) logs
         - `/rosout` *(rosgraph_msgs/Log)*: Console output
         - `/time` *(novatel_gps_msgs/Time)*: [TIME](http://docs.novatel.com/OEM7/Content/Logs/TIME.htm) logs

novatel_gps_driver/CMakeLists.txt

@@ -56,6 +56,7 @@ add_library(${PROJECT_NAME}
   src/parsers/inspvax.cpp
   src/parsers/insstdev.cpp
   src/parsers/parsing_utils.cpp
+  src/parsers/psrdop2.cpp
   src/parsers/range.cpp
   src/parsers/time.cpp
   src/parsers/trackstat.cpp

novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.h

@@ -30,9 +30,9 @@
 /**
  * \file
  *
- * This nodelet is a driver for Novatel OEM6 and FlexPack6 GPS receivers. It
- * publishes standard ROS gps_msgs/GPSFix messages, as well as custom
- * NovatelPosition, GPGGA, and GPRMC messages.
+ * This nodelet is a driver for NovAtel GNSS receivers. It publishes standard
+ * ROS gps_msgs/GPSFix and sensor_msgs/NavSatFix messages as well as NMEA
+ * logs and NovAtel logs.
  *
  * <b>Topics Subscribed:</b>
  *
@@ -60,6 +60,8 @@
  * \e bestvel <tt>novatel_gps_msgs/NovatelVelocity</tt> - High fidelity Novatel-
  *    specific velocity and receiver status data. (only published if
  *    `publish_novatel_velocity` is set `true`)
+ * \e psrdop2 <tt>novatel_gps_msgs/Psrdop2</tt> - Pseudorange Dilution of Precision
+ *    measurements. (only published if `publish_novatel_psrdop2` is set `true`)
  * \e range <tt>novatel_gps_msgs/Range</tt> - Satellite ranging information
  *    (only published if `publish_range_messages` is set `true`)
  * \e time <tt>novatel_gps_msgs/Time</tt> - Novatel-specific time data. (Only
@@ -82,10 +84,6 @@
  *    [""]
  * \e frame_id <tt>str</tt> - The TF frame ID to set in all published message
  *    headers. [""]
- * \e gpgga_gprmc_sync_tol <tt>dbl</tt> - Sync tolarance (seconds) for syncing
- *    GPGGA messages with GPRMC messages. [0.01]
- * \e gpgga_position_sync_tol <tt>dbl</tt> - Sync tolarance (seconds) for
- *    syncing GPGGA messages with BESTPOS messages. [0.01]
  * \e imu_rate <tt>dbl</tt> - Rate at which to publish sensor_msgs/Imu messages.
  *    [100.0]
  * \e imu_sample_rate <tt>dbl</tt> - Rate at which the device internally samples
@@ -103,24 +101,32 @@
  * \e publish_nmea_messages <tt>bool</tt> - If set true, the driver publishes
  *    GPGGA and GPRMC messages (see Topics Published) [false]
  * \e publish_novatel_messages <tt>bool</tt> - If set true, the driver
- *    publishes Novatel Bestpos messages (see Topics Published) [false]
+ *    publishes Novatel Bestpos messages (see Topics Published); even if
+ *    this is false, these logs will still be requested from the receiver [false]
+ * \e publish_novatel_psrdop2 <tt>bool</tt> - If set true, the driver
+ *    published Novatel PSRDOP2 messages (see Topics Published); data from this
+ *    message will be used to filled in DoP values in gps_msgs/GPSFix messages.
+ *    Note that this topic is only published when the values changesages [false]
  * \e publish_novatel_velocity <tt>bool</tt> - If set true, the driver
- *    publishes Novatel Bestvel messages (see Topics Published) [false]
+ *    publishes Novatel Bestvel messages (see Topics Published); data from
+ *    these messages will be used to fill in track/speed fields in
+ *    gps_msgs/GPSFix messages [false]
  * \e publish_range_messages <tt>bool</tt> - If set true, the driver
  *    publishes Novatel RANGE messages [false]
  * \e publish_sync_diagnostic <tt>bool</tt> - If true, publish a Sync diagnostic.
  *    This is ignored if publish_diagnostics is false. [true]
- * \e publish_time <tt>bool</tt> - If set true, the driver publishes Novatel
+ * \e publish_time_messages <tt>bool</tt> - If set true, the driver publishes Novatel
  *    Time messages (see Topics Published) [false]
  * \e publish_trackstat <tt>bool</tt> - If set true, the driver publishes
  *    Novatel Trackstat messages (see Topics Published) [false]
  * \e reconnect_delay_s <tt>bool</t> - If the driver is disconnected from the
  *    device, how long (in seconds) to wait between reconnect attempts. [0.5]
- * \e use_binary_message <tt>bool</tt> - If set true, the driver requests
+ * \e use_binary_messages <tt>bool</tt> - If set true, the driver requests
  *    binary NovAtel messages from the device; if false, it requests ASCII
  *    messages.  [false]
- * \e wait_for_position <tt>bool</tt> - Wait for BESTPOS messages to arrive
- *    before publishing GPSFix messages. [false]
+ * \e wait_for_sync <tt>bool</tt> - Wait for both BESTPOS and BESTVEL
+ *    logs to arrive before pushing GPSFixes.  This has no effect if
+ *    publish_novatel_velocity is false.  [true]
  * \e span_frame_to_ros_frame <tt>bool</tt> - Translate the SPAN coordinate
  *    frame to a ROS coordinate frame using the VEHICLEBODYROTATION and
  *    APPLYVEHICLEBODYROTATION commands. [false]
@@ -195,6 +201,7 @@ namespace novatel_gps_driver
     bool publish_novatel_velocity_;
     bool publish_novatel_heading2_;
     bool publish_novatel_dual_antenna_heading_;
+    bool publish_novatel_psrdop2_;
     bool publish_nmea_messages_;
     bool publish_range_messages_;
     bool publish_time_messages_;
@@ -219,6 +226,7 @@ namespace novatel_gps_driver
     rclcpp::Publisher<novatel_gps_msgs::msg::NovatelVelocity>::SharedPtr novatel_velocity_pub_;
     rclcpp::Publisher<novatel_gps_msgs::msg::NovatelHeading2>::SharedPtr novatel_heading2_pub_;
     rclcpp::Publisher<novatel_gps_msgs::msg::NovatelDualAntennaHeading>::SharedPtr novatel_dual_antenna_heading_pub_;
+    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelPsrdop2>::SharedPtr novatel_psrdop2_pub_;
     rclcpp::Publisher<novatel_gps_msgs::msg::Gpgga>::SharedPtr gpgga_pub_;
     rclcpp::Publisher<novatel_gps_msgs::msg::Gpgsv>::SharedPtr gpgsv_pub_;
     rclcpp::Publisher<novatel_gps_msgs::msg::Gpgsa>::SharedPtr gpgsa_pub_;

novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h

@@ -78,12 +78,14 @@
 #include <novatel_gps_driver/parsers/inspvax.h>
 #include <novatel_gps_driver/parsers/insstdev.h>
 #include <novatel_gps_driver/parsers/range.h>
+#include <novatel_gps_driver/parsers/psrdop2.h>
 #include <novatel_gps_driver/parsers/time.h>
 #include <novatel_gps_driver/parsers/trackstat.h>
 
 namespace novatel_gps_driver
 {
   /// Define NovatelMessageOpts as a map from message name to log period (seconds)
+  /// A negative period will be logged as "onchanged" rather than "ontime"
   typedef std::map<std::string, double> NovatelMessageOpts;
 
   class NovatelGps
@@ -183,6 +185,12 @@ namespace novatel_gps_driver
        * @param[out] headings New DUALANTENNAHEADING messages.
        */
       void GetNovatelDualAntennaHeadingMessages(std::vector<novatel_gps_driver::DualAntennaHeadingParser::MessageType>& headings);
+      /**
+       * @brief Provides any PSRDOP2 messages that have been received since the last time this
+       * was called.
+       * @param[out] psrdop2_messages New PSRDOP2 messages.
+       */
+       void GetNovatelPsrdop2Messages(std::vector<novatel_gps_driver::Psrdop2Parser::MessageType>& psrdop2_messages);
       /**
        * @brief Provides any Imu messages that have been generated since the
        * last time this was called.
@@ -315,11 +323,8 @@ namespace novatel_gps_driver
        */
       bool Write(const std::string& command);
 
-      //parameters
-      double gpgga_gprmc_sync_tol_; //seconds
-      double gpgga_position_sync_tol_; //seconds
-      bool wait_for_position_; //if false, do not require position message to make gps fix message
-      //added this because position message is sometimes > 1 s late.
+      double gpsfix_sync_tol_; //seconds
+      bool wait_for_sync_; // wait until a bestvel has arrived before publishing bestpos
 
     private:
       /**
@@ -495,6 +500,7 @@ namespace novatel_gps_driver
       InspvaParser inspva_parser_;
       InspvaxParser inspvax_parser_;
       InsstdevParser insstdev_parser_;
+      Psrdop2Parser psrdop2_parser_;
       RangeParser range_parser_;
       TimeParser time_parser_;
       TrackstatParser trackstat_parser_;
@@ -503,12 +509,10 @@ namespace novatel_gps_driver
       boost::circular_buffer<novatel_gps_driver::ClockSteeringParser::MessageType> clocksteering_msgs_;
       boost::circular_buffer<novatel_gps_driver::CorrImuDataParser::MessageType> corrimudata_msgs_;
       boost::circular_buffer<novatel_gps_driver::GpggaParser::MessageType> gpgga_msgs_;
-      boost::circular_buffer<novatel_gps_msgs::msg::Gpgga> gpgga_sync_buffer_;
       boost::circular_buffer<novatel_gps_driver::GpgsaParser::MessageType> gpgsa_msgs_;
       boost::circular_buffer<novatel_gps_driver::GpgsvParser::MessageType> gpgsv_msgs_;
       boost::circular_buffer<novatel_gps_driver::GphdtParser::MessageType> gphdt_msgs_;
       boost::circular_buffer<novatel_gps_driver::GprmcParser::MessageType> gprmc_msgs_;
-      boost::circular_buffer<novatel_gps_msgs::msg::Gprmc> gprmc_sync_buffer_;
       boost::circular_buffer<sensor_msgs::msg::Imu::SharedPtr> imu_msgs_;
       boost::circular_buffer<novatel_gps_driver::InscovParser::MessageType> inscov_msgs_;
       boost::circular_buffer<novatel_gps_driver::InspvaParser::MessageType> inspva_msgs_;
@@ -519,12 +523,16 @@ namespace novatel_gps_driver
       boost::circular_buffer<novatel_gps_driver::BestutmParser::MessageType> novatel_utm_positions_;
       boost::circular_buffer<novatel_gps_driver::BestvelParser::MessageType> novatel_velocities_;
       boost::circular_buffer<novatel_gps_driver::BestposParser::MessageType> bestpos_sync_buffer_;
+      boost::circular_buffer<novatel_gps_driver::BestvelParser::MessageType> bestvel_sync_buffer_;
       boost::circular_buffer<novatel_gps_driver::Heading2Parser::MessageType> heading2_msgs_;
       boost::circular_buffer<novatel_gps_driver::DualAntennaHeadingParser::MessageType> dual_antenna_heading_msgs_;
+      boost::circular_buffer<novatel_gps_driver::Psrdop2Parser::MessageType> psrdop2_msgs_;
       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_;
 
+      novatel_gps_driver::Psrdop2Parser::MessageType latest_psrdop2_;
+
       // IMU data synchronization queues
       std::queue<novatel_gps_driver::CorrImuDataParser::MessageType> corrimudata_queue_;
       std::queue<novatel_gps_driver::InspvaParser::MessageType> inspva_queue_;

novatel_gps_driver/include/novatel_gps_driver/parsers/bestvel.h

@@ -36,7 +36,7 @@
 
 namespace novatel_gps_driver
 {
-  class BestvelParser : public MessageParser<novatel_gps_msgs::msg::NovatelVelocity::UniquePtr>
+  class BestvelParser : public MessageParser<novatel_gps_msgs::msg::NovatelVelocity::SharedPtr>
   {
   public:
     uint32_t GetMessageId() const override;

novatel_gps_driver/include/novatel_gps_driver/parsers/psrdop2.h

@@ -0,0 +1,43 @@
+// *****************************************************************************
+//
+// Copyright (C) 2019 All Right Reserved, Southwest Research Institute® (SwRI®)
+//
+// THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
+// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
+// PARTICULAR PURPOSE.
+//
+// *****************************************************************************
+
+#ifndef NOVATEL_GPS_DRIVER_PSRDOP_2_H
+#define NOVATEL_GPS_DRIVER_PSRDOP_2_H
+
+#include <novatel_gps_msgs/msg/novatel_psrdop2.hpp>
+
+#include "message_parser.h"
+
+namespace novatel_gps_driver
+{
+  class Psrdop2Parser : public MessageParser<novatel_gps_msgs::msg::NovatelPsrdop2::SharedPtr>
+  {
+  public:
+    uint32_t GetMessageId() const override;
+
+    const std::string GetMessageName() const override;
+
+    MessageType ParseBinary(const BinaryMessage& bin_msg) override;
+
+    MessageType ParseAscii(const NovatelSentence& sentence) override;
+
+    std::string GetSystemName(uint32_t system_id);
+
+    static constexpr uint16_t MESSAGE_ID = 1163;
+    static constexpr size_t ASCII_BODY_FIELDS = 5;
+    static constexpr size_t ASCII_SYSTEM_FIELDS = 2;
+    static constexpr size_t BINARY_SYSTEM_LENGTH = 8;
+    static constexpr size_t BINARY_BODY_LENGTH = 20;
+    static const std::string MESSAGE_NAME;
+  };
+}
+
+#endif //NOVATEL_GPS_DRIVER_PSRDOP_2_H

novatel_gps_driver/launch/tester_for_eth.launch.py

@@ -24,6 +24,7 @@ def generate_launch_description():
                     'publish_novatel_positions': True,
                     'publish_imu_messages': True,
                     'publish_novatel_velocity': True,
+                    'publish_novatel_psrdop2': True,
                     'imu_frame_id': '/imu',
                     'frame_id': '/gps'
                 }]

novatel_gps_driver/launch/tester_for_usb.launch.py

@@ -20,6 +20,8 @@ def generate_launch_description():
                     'device': '/dev/ttyUSB0',
                     'verbose': True,
                     'publish_novatel_positions': True,
+                    'publish_novatel_psrdop2': True,
+                    'publish_novatel_velocity': True,
                     'frame_id': '/gps'
                 }]
             )