Code cleanup and documentation

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,13 +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.
+        - `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`
@@ -178,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.
@@ -197,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/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>
  *
@@ -84,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
@@ -105,26 +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) [false]
+ *    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 change [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]

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'
                 }]
             )

novatel_gps_driver/src/nodes/novatel_gps_node.cpp

@@ -27,104 +27,6 @@
 //
 // *****************************************************************************
 
-/**
- * \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.
- *
- * <b>Topics Subscribed:</b>
- *
- * \e gps_sync <tt>std_msgs/Time<tt> - Timestamped sync pulses
- *    from a DIO module (optional). These are used to improve the accuracy of
- *    the time stamps of the messages published.
- *
- * <b>Topics Published:</b>
- *
- * \e gps <tt>gps_msgs/GPSFix</tt> - GPS data for navigation
- * \e corrimudata <tt>novatel_gps_message/NovatelCorrectedImuData</tt> - Raw
- *    Novatel IMU data. (only published if `publish_imu_messages` is set `true`)
- * \e gppga <tt>novatel_gps_driver/Gpgga</tt> - Raw GPGGA data for debugging (only
- *    published if `publish_nmea_messages` is set `true`)
- * \e gpgsa <tt>novatel_gps_msgs/Gpgsa</tt> - Raw GPGSA data for debugging (only
- *    published if `publish_gpgsa` is set `true`)
- * \e gprmc <tt>novatel_gps_msgs/Gprmc</tt> - Raw GPRMC data for debugging (only
- *    published if `publish_nmea_messages` is set `true`)
- * \e bestpos <tt>novatel_gps_msgs/NovatelPosition</tt> - High fidelity Novatel-
- *    specific position and receiver status data. (only published if
- *    `publish_novatel_positions` is set `true`)
- * \e bestutm <tt>novatel_gps_msgs/NovatelUtmPosition</tt> - High fidelity Novatel-
- *    specific position in UTM coordinates and receiver status data. (only published
- *    if `publish_novatel_utm_positions` is set `true`)
- * \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 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
- *    published if `publish_time` is set `true`.)
- * \e trackstat <tt>novatel_gps_msgs/Trackstat</tt> - Novatel-specific trackstat
- *    data at 1 Hz. (Only published if `publish_trackstat` is set `true`.)
- *
- * <b>Services:</b>
- *
- * \e freset <tt>novatel_gps_msgs/NovatelFRESET</tt> - Sends a freset message to the
- *    device with the specified target string to reset. By default does
- *    FRESET standard
- *
- * <b>Parameters:</b>
- *
- * \e connection_type <tt>str</tt> - "serial", "udp", or "tcp" as appropriate
- *    for the Novatel device connected. ["serial"]
- * \e device <tt>str</tt> - The path to the device, e.g. /dev/ttyUSB0 for
- *    serial connections or "192.168.1.10:3001" for IP.
- *    [""]
- * \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
- *    its IMU. [200.0]
- * \e polling_period <tt>dbl</tt> - The number of seconds in between messages
- *    requested from the GPS. (Does not affect time messages) [0.05]
- * \e publish_diagnostics <tt>bool</tt> - If set true, the driver publishes
- *    ROS diagnostics [true]
- * \e publish_clocksteering <tt>bool</tt> - If set to true, the driver publishes
- *    Novatel ClockSteering messages [false]
- * \e publish_imu_messages <tt>boot</tt> - If set true, the driver publishes
- *    Novatel CorrImuData, InsPva, InsPvax, InsStdev, and sensor_msgs/Imu messages [false]
- * \e publish_gpgsa <tt>bool</tt> - If set true, the driver requests GPGSA
- *    messages from the device at 20 Hz and publishes them on `gpgsa`
- * \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]
- * \e publish_novatel_velocity <tt>bool</tt> - If set true, the driver
- *    publishes Novatel Bestvel messages (see Topics Published) [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_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
- *    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 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]
- */
 #include <novatel_gps_driver/nodes/novatel_gps_node.h>
 
 #include <swri_math_util/math_util.h>
@@ -295,6 +197,10 @@ namespace novatel_gps_driver
     {
       novatel_velocity_pub_ = swri::advertise<novatel_gps_msgs::msg::NovatelVelocity>(*this, "bestvel", 100);
     }
+    else
+    {
+      gps_.wait_for_sync_ = false;
+    }
 
     if (publish_novatel_heading2_)
     {
@@ -520,14 +426,6 @@ namespace novatel_gps_driver
         rclcpp::sleep_for(std::chrono::milliseconds(static_cast<long>(reconnect_delay_s_ * 1000.0)));
       }
 
-      // Poke the diagnostic updater. It will only fire diagnostics if
-      // its internal timer (1 Hz) has elapsed. Otherwise, this is a
-      // noop
-      if (publish_diagnostics_)
-      {
-        //diagnostic_updater_.force_update();
-      }
-
       // Sleep for a microsecond to prevent CPU hogging
       boost::this_thread::sleep(boost::posix_time::microseconds(1));
 

novatel_gps_driver/src/novatel_gps.cpp

@@ -323,7 +323,6 @@ namespace novatel_gps_driver
     // Clear out the fix_messages list before filling it
     fix_messages.clear();
 
-
     while (!bestpos_sync_buffer_.empty())
     {
       auto& bestpos = bestpos_sync_buffer_.front();
@@ -332,7 +331,6 @@ namespace novatel_gps_driver
 
       auto gpsFix = std::make_unique<gps_msgs::msg::GPSFix>();
 
-      RCLCPP_INFO(node_.get_logger(), "Trying to sync bestvel...");
       // Speed & Track are filled in from BESTVEL logs, if available
       while (!bestvel_sync_buffer_.empty())
       {
@@ -343,9 +341,6 @@ namespace novatel_gps_driver
                                      bestpos->novatel_msg_header.gps_seconds);
         if (time_diff < gpsfix_sync_tol_)
         {
-          RCLCPP_INFO(node_.get_logger(), "  Synced (%f vs %f)",
-                      bestvel->novatel_msg_header.gps_seconds,
-                      bestpos->novatel_msg_header.gps_seconds);
           // Within tolerance, messages synced
           gpsFix->track = bestvel->track_ground;
           gpsFix->speed = std::sqrt(std::pow(bestvel->horizontal_speed, 2) + std::pow(bestvel->vertical_speed, 2));
@@ -354,17 +349,11 @@ namespace novatel_gps_driver
         }
         else if (bestvel->novatel_msg_header.gps_seconds < bestpos->novatel_msg_header.gps_seconds)
         {
-          RCLCPP_INFO(node_.get_logger(), "  Discarding too old bestvel (%f vs %f)",
-                      bestvel->novatel_msg_header.gps_seconds,
-                      bestpos->novatel_msg_header.gps_seconds);
           // Bestvel timestamp is too old, throw it away and try again
           bestvel_sync_buffer_.pop_front();
         }
         else
         {
-          RCLCPP_INFO(node_.get_logger(), "  Newest bestvel is too new (%f vs %f)",
-                      bestvel->novatel_msg_header.gps_seconds,
-                      bestpos->novatel_msg_header.gps_seconds);
           // Latest bestvel message is too new, do nothing for now
           break;
         }
@@ -373,7 +362,6 @@ namespace novatel_gps_driver
       if (!synced && wait_for_sync_)
       {
         // TODO Handle this properly if bestvel is disabled
-        RCLCPP_INFO(node_.get_logger(), "Waiting for bestvels to arrive...");
         // If we have a bestpos and are configured to wait for a sync, we need
         // to wait until a bestvel arrives.
 
@@ -423,8 +411,6 @@ namespace novatel_gps_driver
       gpsFix->position_covariance_type =
           gps_msgs::msg::GPSFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
 
-
-
       // DOPs are filled in from the PSRDOP2 log, if available
       if (latest_psrdop2_)
       {