diff --git a/README.md b/README.md
index 8f14879..4da6497 100644
--- a/README.md
+++ b/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
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
index 8120385..7afd770 100644
--- 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
@@ -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.
*
* Topics Subscribed:
*
@@ -84,10 +84,6 @@
* [""]
* \e frame_id str - The TF frame ID to set in all published message
* headers. [""]
- * \e gpgga_gprmc_sync_tol dbl - Sync tolarance (seconds) for syncing
- * GPGGA messages with GPRMC messages. [0.01]
- * \e gpgga_position_sync_tol dbl - Sync tolarance (seconds) for
- * syncing GPGGA messages with BESTPOS messages. [0.01]
* \e imu_rate dbl - Rate at which to publish sensor_msgs/Imu messages.
* [100.0]
* \e imu_sample_rate dbl - Rate at which the device internally samples
@@ -105,26 +101,32 @@
* \e publish_nmea_messages bool - If set true, the driver publishes
* GPGGA and GPRMC messages (see Topics Published) [false]
* \e publish_novatel_messages bool - 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 bool - 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 bool - 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 bool - If set true, the driver
* publishes Novatel RANGE messages [false]
* \e publish_sync_diagnostic bool - If true, publish a Sync diagnostic.
* This is ignored if publish_diagnostics is false. [true]
- * \e publish_time bool - If set true, the driver publishes Novatel
+ * \e publish_time_messages bool - If set true, the driver publishes Novatel
* Time messages (see Topics Published) [false]
* \e publish_trackstat bool - If set true, the driver publishes
* Novatel Trackstat messages (see Topics Published) [false]
* \e reconnect_delay_s bool - If the driver is disconnected from the
* device, how long (in seconds) to wait between reconnect attempts. [0.5]
- * \e use_binary_message bool - If set true, the driver requests
+ * \e use_binary_messages bool - If set true, the driver requests
* binary NovAtel messages from the device; if false, it requests ASCII
* messages. [false]
- * \e wait_for_position bool - Wait for BESTPOS messages to arrive
- * before publishing GPSFix messages. [false]
+ * \e wait_for_sync bool - 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 bool - Translate the SPAN coordinate
* frame to a ROS coordinate frame using the VEHICLEBODYROTATION and
* APPLYVEHICLEBODYROTATION commands. [false]
diff --git a/novatel_gps_driver/launch/tester_for_eth.launch.py b/novatel_gps_driver/launch/tester_for_eth.launch.py
index 066da34..77ba9f8 100644
--- a/novatel_gps_driver/launch/tester_for_eth.launch.py
+++ b/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'
}]
diff --git a/novatel_gps_driver/launch/tester_for_usb.launch.py b/novatel_gps_driver/launch/tester_for_usb.launch.py
index 1078aec..26abe59 100644
--- a/novatel_gps_driver/launch/tester_for_usb.launch.py
+++ b/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'
}]
)
diff --git a/novatel_gps_driver/src/nodes/novatel_gps_node.cpp b/novatel_gps_driver/src/nodes/novatel_gps_node.cpp
index 45ddb46..a5fb627 100644
--- a/novatel_gps_driver/src/nodes/novatel_gps_node.cpp
+++ b/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.
- *
- * Topics Subscribed:
- *
- * \e gps_sync std_msgs/Time - Timestamped sync pulses
- * from a DIO module (optional). These are used to improve the accuracy of
- * the time stamps of the messages published.
- *
- * Topics Published:
- *
- * \e gps gps_msgs/GPSFix - GPS data for navigation
- * \e corrimudata novatel_gps_message/NovatelCorrectedImuData - Raw
- * Novatel IMU data. (only published if `publish_imu_messages` is set `true`)
- * \e gppga novatel_gps_driver/Gpgga - Raw GPGGA data for debugging (only
- * published if `publish_nmea_messages` is set `true`)
- * \e gpgsa novatel_gps_msgs/Gpgsa - Raw GPGSA data for debugging (only
- * published if `publish_gpgsa` is set `true`)
- * \e gprmc novatel_gps_msgs/Gprmc - Raw GPRMC data for debugging (only
- * published if `publish_nmea_messages` is set `true`)
- * \e bestpos novatel_gps_msgs/NovatelPosition - High fidelity Novatel-
- * specific position and receiver status data. (only published if
- * `publish_novatel_positions` is set `true`)
- * \e bestutm novatel_gps_msgs/NovatelUtmPosition - High fidelity Novatel-
- * specific position in UTM coordinates and receiver status data. (only published
- * if `publish_novatel_utm_positions` is set `true`)
- * \e bestvel novatel_gps_msgs/NovatelVelocity - High fidelity Novatel-
- * specific velocity and receiver status data. (only published if
- * `publish_novatel_velocity` is set `true`)
- * \e range novatel_gps_msgs/Range - Satellite ranging information
- * (only published if `publish_range_messages` is set `true`)
- * \e time novatel_gps_msgs/Time - Novatel-specific time data. (Only
- * published if `publish_time` is set `true`.)
- * \e trackstat novatel_gps_msgs/Trackstat - Novatel-specific trackstat
- * data at 1 Hz. (Only published if `publish_trackstat` is set `true`.)
- *
- * Services:
- *
- * \e freset novatel_gps_msgs/NovatelFRESET - Sends a freset message to the
- * device with the specified target string to reset. By default does
- * FRESET standard
- *
- * Parameters:
- *
- * \e connection_type str - "serial", "udp", or "tcp" as appropriate
- * for the Novatel device connected. ["serial"]
- * \e device str - The path to the device, e.g. /dev/ttyUSB0 for
- * serial connections or "192.168.1.10:3001" for IP.
- * [""]
- * \e frame_id str - The TF frame ID to set in all published message
- * headers. [""]
- * \e gpgga_gprmc_sync_tol dbl - Sync tolarance (seconds) for syncing
- * GPGGA messages with GPRMC messages. [0.01]
- * \e gpgga_position_sync_tol dbl - Sync tolarance (seconds) for
- * syncing GPGGA messages with BESTPOS messages. [0.01]
- * \e imu_rate dbl - Rate at which to publish sensor_msgs/Imu messages.
- * [100.0]
- * \e imu_sample_rate dbl - Rate at which the device internally samples
- * its IMU. [200.0]
- * \e polling_period dbl - The number of seconds in between messages
- * requested from the GPS. (Does not affect time messages) [0.05]
- * \e publish_diagnostics bool - If set true, the driver publishes
- * ROS diagnostics [true]
- * \e publish_clocksteering bool - If set to true, the driver publishes
- * Novatel ClockSteering messages [false]
- * \e publish_imu_messages boot - If set true, the driver publishes
- * Novatel CorrImuData, InsPva, InsPvax, InsStdev, and sensor_msgs/Imu messages [false]
- * \e publish_gpgsa bool - If set true, the driver requests GPGSA
- * messages from the device at 20 Hz and publishes them on `gpgsa`
- * \e publish_nmea_messages bool - If set true, the driver publishes
- * GPGGA and GPRMC messages (see Topics Published) [false]
- * \e publish_novatel_messages bool - If set true, the driver
- * publishes Novatel Bestpos messages (see Topics Published) [false]
- * \e publish_novatel_velocity bool - If set true, the driver
- * publishes Novatel Bestvel messages (see Topics Published) [false]
- * \e publish_range_messages bool - If set true, the driver
- * publishes Novatel RANGE messages [false]
- * \e publish_sync_diagnostic bool - If true, publish a Sync diagnostic.
- * This is ignored if publish_diagnostics is false. [true]
- * \e publish_time_messages bool - If set true, the driver publishes Novatel
- * Time messages (see Topics Published) [false]
- * \e publish_trackstat bool - If set true, the driver publishes
- * Novatel Trackstat messages (see Topics Published) [false]
- * \e reconnect_delay_s bool - If the driver is disconnected from the
- * device, how long (in seconds) to wait between reconnect attempts. [0.5]
- * \e use_binary_message bool - If set true, the driver requests
- * binary NovAtel messages from the device; if false, it requests ASCII
- * messages. [false]
- * \e wait_for_position bool - Wait for BESTPOS messages to arrive
- * before publishing GPSFix messages. [false]
- * \e span_frame_to_ros_frame bool - Translate the SPAN coordinate
- * frame to a ROS coordinate frame using the VEHICLEBODYROTATION and
- * APPLYVEHICLEBODYROTATION commands. [false]
- */
#include
#include
@@ -295,6 +197,10 @@ namespace novatel_gps_driver
{
novatel_velocity_pub_ = swri::advertise(*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(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));
diff --git a/novatel_gps_driver/src/novatel_gps.cpp b/novatel_gps_driver/src/novatel_gps.cpp
index 97904b7..f41b51b 100644
--- a/novatel_gps_driver/src/novatel_gps.cpp
+++ b/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();
- 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_)
{