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_) {