diff --git a/README.md b/README.md index 156caa2..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,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 diff --git a/novatel_gps_driver/CMakeLists.txt b/novatel_gps_driver/CMakeLists.txt index e34beb5..8f4651a 100644 --- a/novatel_gps_driver/CMakeLists.txt +++ b/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 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 2de0baf..270fdf6 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: * @@ -60,6 +60,8 @@ * \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 psrdop2 novatel_gps_msgs/Psrdop2 - Pseudorange Dilution of Precision + * measurements. (only published if `publish_novatel_psrdop2` 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 @@ -82,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 @@ -103,24 +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); 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 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] @@ -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::SharedPtr novatel_velocity_pub_; rclcpp::Publisher::SharedPtr novatel_heading2_pub_; rclcpp::Publisher::SharedPtr novatel_dual_antenna_heading_pub_; + rclcpp::Publisher::SharedPtr novatel_psrdop2_pub_; rclcpp::Publisher::SharedPtr gpgga_pub_; rclcpp::Publisher::SharedPtr gpgsv_pub_; rclcpp::Publisher::SharedPtr gpgsa_pub_; diff --git a/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h b/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h index 757471c..0094440 100644 --- a/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h +++ b/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h @@ -78,12 +78,14 @@ #include #include #include +#include #include #include 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 NovatelMessageOpts; class NovatelGps @@ -183,6 +185,12 @@ namespace novatel_gps_driver * @param[out] headings New DUALANTENNAHEADING messages. */ void GetNovatelDualAntennaHeadingMessages(std::vector& 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& 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 clocksteering_msgs_; boost::circular_buffer corrimudata_msgs_; boost::circular_buffer gpgga_msgs_; - boost::circular_buffer gpgga_sync_buffer_; boost::circular_buffer gpgsa_msgs_; boost::circular_buffer gpgsv_msgs_; boost::circular_buffer gphdt_msgs_; boost::circular_buffer gprmc_msgs_; - boost::circular_buffer gprmc_sync_buffer_; boost::circular_buffer imu_msgs_; boost::circular_buffer inscov_msgs_; boost::circular_buffer inspva_msgs_; @@ -519,12 +523,16 @@ namespace novatel_gps_driver boost::circular_buffer novatel_utm_positions_; boost::circular_buffer novatel_velocities_; boost::circular_buffer bestpos_sync_buffer_; + boost::circular_buffer bestvel_sync_buffer_; boost::circular_buffer heading2_msgs_; boost::circular_buffer dual_antenna_heading_msgs_; + boost::circular_buffer psrdop2_msgs_; boost::circular_buffer range_msgs_; boost::circular_buffer time_msgs_; boost::circular_buffer trackstat_msgs_; + novatel_gps_driver::Psrdop2Parser::MessageType latest_psrdop2_; + // IMU data synchronization queues std::queue corrimudata_queue_; std::queue inspva_queue_; diff --git a/novatel_gps_driver/include/novatel_gps_driver/parsers/bestvel.h b/novatel_gps_driver/include/novatel_gps_driver/parsers/bestvel.h index d1fbb71..e80f17c 100644 --- a/novatel_gps_driver/include/novatel_gps_driver/parsers/bestvel.h +++ b/novatel_gps_driver/include/novatel_gps_driver/parsers/bestvel.h @@ -36,7 +36,7 @@ namespace novatel_gps_driver { - class BestvelParser : public MessageParser + class BestvelParser : public MessageParser { public: uint32_t GetMessageId() const override; diff --git a/novatel_gps_driver/include/novatel_gps_driver/parsers/psrdop2.h b/novatel_gps_driver/include/novatel_gps_driver/parsers/psrdop2.h new file mode 100644 index 0000000..44ea9d9 --- /dev/null +++ b/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 + +#include "message_parser.h" + +namespace novatel_gps_driver +{ + class Psrdop2Parser : public MessageParser + { + 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 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 7978012..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 @@ -157,6 +59,7 @@ namespace novatel_gps_driver publish_novatel_velocity_(false), publish_novatel_heading2_(false), publish_novatel_dual_antenna_heading_(false), + publish_novatel_psrdop2_(false), publish_nmea_messages_(false), publish_range_messages_(false), publish_time_messages_(false), @@ -201,6 +104,7 @@ namespace novatel_gps_driver publish_novatel_velocity_ = this->declare_parameter("publish_novatel_velocity", publish_novatel_velocity_); publish_novatel_heading2_ = this->declare_parameter("publish_novatel_heading2", publish_novatel_heading2_); publish_novatel_dual_antenna_heading_ = this->declare_parameter("publish_novatel_dual_antenna_heading", publish_novatel_dual_antenna_heading_); + publish_novatel_psrdop2_ = this->declare_parameter("publish_novatel_psrdop2", publish_novatel_psrdop2_); publish_nmea_messages_ = this->declare_parameter("publish_nmea_messages", publish_nmea_messages_); publish_range_messages_ = this->declare_parameter("publish_range_messages", publish_range_messages_); publish_time_messages_ = this->declare_parameter("publish_time_messages", publish_time_messages_); @@ -220,9 +124,8 @@ namespace novatel_gps_driver frame_id_ = this->declare_parameter("frame_id", std::string("")); //set NovatelGps parameters - gps_.gpgga_gprmc_sync_tol_ = this->declare_parameter("gpgga_gprmc_sync_tol", 0.01); - gps_.gpgga_position_sync_tol_ = this->declare_parameter("gpgga_position_sync_tol", 0.01); - gps_.wait_for_position_ = this->declare_parameter("wait_for_position", false); + gps_.gpsfix_sync_tol_ = this->declare_parameter("gpsfix_sync_tol", 0.01); + gps_.wait_for_sync_ = this->declare_parameter("wait_for_sync", true); // Reset Service reset_service_ = this->create_service("freset", @@ -294,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_) { @@ -307,6 +214,14 @@ namespace novatel_gps_driver 100); } + if (publish_novatel_psrdop2_) + { + novatel_psrdop2_pub_ = swri::advertise(*this, + "psrdop2", + 100, + true); + } + if (publish_range_messages_) { range_pub_ = swri::advertise(*this, "range", 100); @@ -406,6 +321,10 @@ namespace novatel_gps_driver { opts["dualantennaheading" + format_suffix] = polling_period_; } + if (publish_novatel_psrdop2_) + { + opts["psrdop2" + format_suffix] = -1.0; + } if (publish_gpgsa_) { opts["gpgsa"] = polling_period_; @@ -507,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)); @@ -773,6 +684,18 @@ namespace novatel_gps_driver } } + if (publish_novatel_psrdop2_) + { + std::vector psrdop2_msgs; + gps_.GetNovatelPsrdop2Messages(psrdop2_msgs); + for (auto& msg : psrdop2_msgs) + { + msg->header.stamp = rclcpp::Time(msg->header.stamp, this->get_clock()->get_clock_type()) + sync_offset; + msg->header.frame_id = frame_id_; + novatel_psrdop2_pub_->publish(*msg); + } + } + if (publish_clock_steering_) { std::vector msgs; @@ -782,7 +705,6 @@ namespace novatel_gps_driver clocksteering_pub_->publish(std::move(msg)); } } - if (publish_novatel_velocity_) { std::vector velocity_msgs; @@ -791,7 +713,7 @@ namespace novatel_gps_driver { msg->header.stamp = rclcpp::Time(msg->header.stamp, this->get_clock()->get_clock_type()) + sync_offset; msg->header.frame_id = frame_id_; - novatel_velocity_pub_->publish(std::move(msg)); + novatel_velocity_pub_->publish(*msg); } } if (publish_time_messages_) diff --git a/novatel_gps_driver/src/novatel_gps.cpp b/novatel_gps_driver/src/novatel_gps.cpp index f348b57..f41b51b 100644 --- a/novatel_gps_driver/src/novatel_gps.cpp +++ b/novatel_gps_driver/src/novatel_gps.cpp @@ -45,9 +45,8 @@ namespace novatel_gps_driver { NovatelGps::NovatelGps(rclcpp::Node& node) : - gpgga_gprmc_sync_tol_(0.01), - gpgga_position_sync_tol_(0.01), - wait_for_position_(false), + gpsfix_sync_tol_(0.01), + wait_for_sync_(true), node_(node), connection_(SERIAL), is_connected_(false), @@ -60,12 +59,10 @@ namespace novatel_gps_driver clocksteering_msgs_(MAX_BUFFER_SIZE), corrimudata_msgs_(MAX_BUFFER_SIZE), gpgga_msgs_(MAX_BUFFER_SIZE), - gpgga_sync_buffer_(SYNC_BUFFER_SIZE), gpgsa_msgs_(MAX_BUFFER_SIZE), gpgsv_msgs_(MAX_BUFFER_SIZE), gphdt_msgs_(MAX_BUFFER_SIZE), gprmc_msgs_(MAX_BUFFER_SIZE), - gprmc_sync_buffer_(SYNC_BUFFER_SIZE), imu_msgs_(MAX_BUFFER_SIZE), inscov_msgs_(MAX_BUFFER_SIZE), inspva_msgs_(MAX_BUFFER_SIZE), @@ -76,8 +73,10 @@ namespace novatel_gps_driver novatel_utm_positions_(MAX_BUFFER_SIZE), novatel_velocities_(MAX_BUFFER_SIZE), bestpos_sync_buffer_(SYNC_BUFFER_SIZE), + bestvel_sync_buffer_(SYNC_BUFFER_SIZE), heading2_msgs_(MAX_BUFFER_SIZE), dual_antenna_heading_msgs_(MAX_BUFFER_SIZE), + psrdop2_msgs_(MAX_BUFFER_SIZE), range_msgs_(MAX_BUFFER_SIZE), time_msgs_(MAX_BUFFER_SIZE), trackstat_msgs_(MAX_BUFFER_SIZE), @@ -324,138 +323,114 @@ namespace novatel_gps_driver // Clear out the fix_messages list before filling it fix_messages.clear(); - // both a gpgga and a gprmc message are required to fill the GPSFix message - while (!gpgga_sync_buffer_.empty() && !gprmc_sync_buffer_.empty()) + while (!bestpos_sync_buffer_.empty()) { - double gpgga_time = UtcFloatToSeconds(gpgga_sync_buffer_.front().utc_seconds); - double gprmc_time = UtcFloatToSeconds(gprmc_sync_buffer_.front().utc_seconds); + auto& bestpos = bestpos_sync_buffer_.front(); - // Find the time difference between gpgga and gprmc time - double dt = gpgga_time - gprmc_time; - // Handle times around midnight - if (dt > 43200.0) + bool synced = false; + + auto gpsFix = std::make_unique(); + + // Speed & Track are filled in from BESTVEL logs, if available + while (!bestvel_sync_buffer_.empty()) { - dt -= 86400.0; + auto& bestvel = bestvel_sync_buffer_.front(); + + // TODO pjr handle wrapping around week boundary? + double time_diff = std::fabs(bestvel->novatel_msg_header.gps_seconds - + bestpos->novatel_msg_header.gps_seconds); + if (time_diff < gpsfix_sync_tol_) + { + // 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)); + synced = true; + break; + } + else if (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 + { + // Latest bestvel message is too new, do nothing for now + break; + } } - if (dt < -43200.0) + + if (!synced && wait_for_sync_) { - dt += 86400.0; + // TODO Handle this properly if bestvel is disabled + // If we have a bestpos and are configured to wait for a sync, we need + // to wait until a bestvel arrives. + + break; } - // Get the front elements of the gpgga and gprmc buffers synced to within tolerance - if (dt > gpgga_gprmc_sync_tol_) + gpsFix->header.stamp = node_.get_clock()->now(); + gpsFix->altitude = bestpos->height; + gpsFix->latitude = bestpos->lat; + gpsFix->longitude = bestpos->lon; + + if (bestpos->solution_status == "SOL_COMPUTED") { - // The gprmc message is more than tol older than the gpgga message, - // discard it and continue - gprmc_sync_buffer_.pop_front(); + gpsFix->status.status = gps_msgs::msg::GPSStatus::STATUS_FIX; } - else if (-dt > gpgga_gprmc_sync_tol_) + else { - // The gpgga message is more than tol older than the gprmc message, - // discard it and continue - gpgga_sync_buffer_.pop_front(); + gpsFix->status.status = gps_msgs::msg::GPSStatus::STATUS_NO_FIX; } - else // The gpgga and gprmc messages are synced - { - bool has_position = false; - //bool pop_position = false; - // Iterate over the bestpos message buffer until we find one - // that is synced with the gpgga message - while (!bestpos_sync_buffer_.empty()) - { - // Calculate UTC position time from GPS seconds by subtracting out - // full days and applying the UTC offset - double gps_seconds = bestpos_sync_buffer_.front()->novatel_msg_header.gps_seconds + utc_offset_; - if (gps_seconds < 0) - { - // Handle times around week boundaries - gps_seconds = gps_seconds + 604800; // 604800 = 7 * 24 * 60 * 60 (seconds in a week) - } - auto days = static_cast(gps_seconds / 86400.0); - double position_time = gps_seconds - days * 86400.0; + gpsFix->time = bestpos->novatel_msg_header.gps_seconds; - // Find the time difference between gpgga and position time - double pos_dt = gpgga_time - position_time; - // Handle times around midnight - if (pos_dt > 43200.0) - { - pos_dt -= 86400.0; - } - if (pos_dt < -43200.0) - { - pos_dt += 86400.0; - } + gpsFix->status.header.stamp = gpsFix->header.stamp; + gpsFix->status.satellites_visible = bestpos->num_satellites_tracked; + gpsFix->status.satellites_used = bestpos->num_satellites_used_in_solution; - if (pos_dt > gpgga_position_sync_tol_) - { - // The position message is more than tol older than the gpgga message, - // discard it and continue - RCLCPP_DEBUG(node_.get_logger(), "Discarding a position message that is too old (%f < %f)", position_time, gpgga_time); - bestpos_sync_buffer_.pop_front(); - } - else - { - // There's a bestpos message that is close enough to the GPGGA message to use it - // It's ok if it's far ahead (even though that's weird), it's better than nothing - has_position = true; - break; - } - } + // We have a position message, so we can calculate variances + // from the standard deviations + double sigma_x = bestpos->lon_sigma; + gpsFix->position_covariance[0] = sigma_x * sigma_x; - if (has_position || !wait_for_position_) - { - // If we have a position message (or don't need one), create and fill - // a GPS fix message - gps_msgs::msg::GPSFix::UniquePtr gps_fix = std::make_unique(); - // Fill GPS fix message using the messages at the front of the two - // sync queues - extractor_.GetGpsFixMessage( - gprmc_sync_buffer_.front(), - gpgga_sync_buffer_.front(), - gps_fix); - - // Remove the used messages from the two sync queues - gpgga_sync_buffer_.pop_front(); - gprmc_sync_buffer_.pop_front(); - - if (has_position) - { - auto& position = bestpos_sync_buffer_.front(); - // We have a position message, so we can calculate variances - // from the standard deviations - double sigma_x = position->lon_sigma; - gps_fix->position_covariance[0] = sigma_x * sigma_x; + double sigma_y = bestpos->lat_sigma; + gpsFix->position_covariance[4] = sigma_y * sigma_y; - double sigma_y = position->lat_sigma; - gps_fix->position_covariance[4] = sigma_y * sigma_y; + double sigma_z = bestpos->height_sigma; + gpsFix->position_covariance[8] = sigma_z * sigma_z; - double sigma_z = position->height_sigma; - gps_fix->position_covariance[8] = sigma_z * sigma_z; + // 2D and 3D error values are the DRMS and and MRSE as described in + // https://www.novatel.com/assets/Documents/Bulletins/apn029.pdf + double horz_sum = std::pow(bestpos->lat_sigma, 2) + std::pow(bestpos->lon_sigma, 2); + gpsFix->err_horz = std::sqrt(horz_sum); - // 2D and 3D error values are the DRMS and and MRSE as described in - // https://www.novatel.com/assets/Documents/Bulletins/apn029.pdf - double horz_sum = std::pow(position->lat_sigma, 2) + std::pow(position->lon_sigma, 2); - gps_fix->err_horz = std::sqrt(horz_sum); + gpsFix->err = std::sqrt(horz_sum + std::pow(bestpos->height_sigma, 2)); - gps_fix->err = std::sqrt(horz_sum + std::pow(position->height_sigma, 2)); + gpsFix->err_vert = bestpos->height_sigma; - gps_fix->err_vert = position->height_sigma; + gpsFix->position_covariance_type = + gps_msgs::msg::GPSFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - gps_fix->position_covariance_type = - gps_msgs::msg::GPSFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - } - - // Add the message to the fix message list - fix_messages.push_back(std::move(gps_fix)); - } - else // There is no position message (and wait_for_position is true) + // DOPs are filled in from the PSRDOP2 log, if available + if (latest_psrdop2_) + { + gpsFix->gdop = latest_psrdop2_->gdop; + gpsFix->pdop = latest_psrdop2_->pdop; + gpsFix->hdop = latest_psrdop2_->hdop; + gpsFix->vdop = latest_psrdop2_->vdop; + if (!latest_psrdop2_->systems.empty()) { - // return without pushing any more gps fix messages to the list - return; + // The PSRDOP2 log can have multiple TDOPs, but the GPSFix message + // only has one... so just use the first one. + gpsFix->tdop = latest_psrdop2_->systems.front().tdop; } - } // else (gpgga and gprmc synced) - } // while (gpgga and gprmc buffers contain messages) + } + + // TODO Fill in orientation from INSATT on SPAN systems + + fix_messages.push_back(std::move(gpsFix)); + bestpos_sync_buffer_.pop_front(); + } } void NovatelGps::GetNovatelHeading2Messages(std::vector& headings) @@ -474,6 +449,13 @@ namespace novatel_gps_driver DrainQueue(corrimudata_msgs_, imu_messages); } + void NovatelGps::GetNovatelPsrdop2Messages(std::vector& psrdop2_messages) + { + psrdop2_messages.clear(); + psrdop2_messages.insert(psrdop2_messages.end(), psrdop2_msgs_.begin(), psrdop2_msgs_.end()); + psrdop2_msgs_.clear(); + } + void NovatelGps::GetGpggaMessages(std::vector& gpgga_messages) { DrainQueue(gpgga_msgs_, gpgga_messages); @@ -1048,7 +1030,8 @@ namespace novatel_gps_driver { auto velocity = bestvel_parser_.ParseBinary(msg); velocity->header.stamp = stamp; - novatel_velocities_.push_back(std::move(velocity)); + novatel_velocities_.push_back(velocity); + bestvel_sync_buffer_.push_back(velocity); break; } case Heading2Parser::MESSAGE_ID: @@ -1118,6 +1101,14 @@ namespace novatel_gps_driver latest_insstdev_ = insstdev; break; } + case Psrdop2Parser::MESSAGE_ID: + { + auto psrdop2 = psrdop2_parser_.ParseBinary(msg); + psrdop2->header.stamp = stamp; + psrdop2_msgs_.push_back(psrdop2); + latest_psrdop2_ = psrdop2; + break; + } case RangeParser::MESSAGE_ID: { auto range = range_parser_.ParseBinary(msg); @@ -1166,18 +1157,7 @@ namespace novatel_gps_driver gpgga->header.stamp = stamp - std::chrono::duration(most_recent_utc_time - gpgga_time); - if (gpgga_parser_.WasLastGpsValid()) - { - // Make a deep copy for the sync buffer so the GPSFix messages - // don't get adjusted multiple times for the sync offset. - gpgga_sync_buffer_.push_back(*gpgga); - - gpgga_msgs_.push_back(std::move(gpgga)); - } - else - { - gpgga_msgs_.push_back(std::move(gpgga)); - } + gpgga_msgs_.push_back(std::move(gpgga)); } else if (sentence.id == GprmcParser::MESSAGE_NAME) { @@ -1192,18 +1172,7 @@ namespace novatel_gps_driver gprmc->header.stamp = stamp - std::chrono::duration(most_recent_utc_time - gprmc_time); - if (gprmc_parser_.WasLastGpsValid()) - { - // Make a deep copy for the sync buffer so the GPSFix messages - // don't get adjusted multiple times for the sync offset. - gprmc_sync_buffer_.push_back(*gprmc); - - gprmc_msgs_.push_back(std::move(gprmc)); - } - else - { - gprmc_msgs_.push_back(std::move(gprmc)); - } + gprmc_msgs_.push_back(std::move(gprmc)); } else if (sentence.id == GpgsaParser::MESSAGE_NAME) { @@ -1254,7 +1223,8 @@ namespace novatel_gps_driver { auto velocity = bestvel_parser_.ParseAscii(sentence); velocity->header.stamp = stamp; - novatel_velocities_.push_back(std::move(velocity)); + novatel_velocities_.push_back(velocity); + bestvel_sync_buffer_.push_back(velocity); } else if (sentence.id == "HEADING2A") { @@ -1316,6 +1286,13 @@ namespace novatel_gps_driver insstdev_msgs_.push_back(insstdev); latest_insstdev_ = insstdev; } + else if (sentence.id == "PSRDOP2A") + { + auto psrdop2 = psrdop2_parser_.ParseAscii(sentence); + psrdop2->header.stamp = stamp; + psrdop2_msgs_.push_back(psrdop2); + latest_psrdop2_ = psrdop2; + } else if (sentence.id == "TIMEA") { auto time = time_parser_.ParseAscii(sentence); @@ -1464,6 +1441,10 @@ namespace novatel_gps_driver { command << "log " << option.first << " onnew " << "\r\n"; } + else if (option.second < 0.0) + { + command << "log " << option.first << " onchanged\r\n"; + } else { command << "log " << option.first << " ontime " << option.second << "\r\n"; diff --git a/novatel_gps_driver/src/parsers/bestvel.cpp b/novatel_gps_driver/src/parsers/bestvel.cpp index f09f714..1751b7a 100644 --- a/novatel_gps_driver/src/parsers/bestvel.cpp +++ b/novatel_gps_driver/src/parsers/bestvel.cpp @@ -53,7 +53,7 @@ novatel_gps_driver::BestvelParser::MessageType novatel_gps_driver::BestvelParser error << "Unexpected velocity message size: " << bin_msg.data_.size(); throw ParseException(error.str()); } - auto ros_msg = std::make_unique(); + auto ros_msg = std::make_shared(); HeaderParser h_parser; ros_msg->novatel_msg_header = h_parser.ParseBinary(bin_msg); ros_msg->novatel_msg_header.message_name = MESSAGE_NAME; @@ -85,7 +85,7 @@ novatel_gps_driver::BestvelParser::MessageType novatel_gps_driver::BestvelParser novatel_gps_driver::BestvelParser::MessageType novatel_gps_driver::BestvelParser::ParseAscii(const NovatelSentence& sentence) noexcept(false) { - auto msg = std::make_unique(); + auto msg = std::make_shared(); HeaderParser h_parser; msg->novatel_msg_header = h_parser.ParseAscii(sentence); diff --git a/novatel_gps_driver/src/parsers/psrdop2.cpp b/novatel_gps_driver/src/parsers/psrdop2.cpp new file mode 100644 index 0000000..8fbdb4d --- /dev/null +++ b/novatel_gps_driver/src/parsers/psrdop2.cpp @@ -0,0 +1,136 @@ +// ***************************************************************************** +// +// 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. +// +// ***************************************************************************** + +#include +#include +#include + +const std::string novatel_gps_driver::Psrdop2Parser::MESSAGE_NAME = "PSRDOP2"; + +uint32_t novatel_gps_driver::Psrdop2Parser::GetMessageId() const +{ + return MESSAGE_ID; +} + +const std::string novatel_gps_driver::Psrdop2Parser::GetMessageName() const +{ + return MESSAGE_NAME; +} + +novatel_gps_driver::Psrdop2Parser::MessageType +novatel_gps_driver::Psrdop2Parser::ParseBinary(const novatel_gps_driver::BinaryMessage& bin_msg) +{ + uint32_t num_systems = ParseUInt32(&bin_msg.data_[16]); + if (bin_msg.data_.size() != (BINARY_SYSTEM_LENGTH * num_systems) + BINARY_BODY_LENGTH) + { + std::stringstream error; + error << "Unexpected PSRDOP2 message size: " << bin_msg.data_.size(); + throw ParseException(error.str()); + } + + auto ros_msg = std::make_shared(); + + HeaderParser header_parser; + ros_msg->novatel_msg_header = header_parser.ParseBinary(bin_msg); + ros_msg->novatel_msg_header.message_name = MESSAGE_NAME; + + ros_msg->gdop = ParseFloat(&bin_msg.data_[0]); + + ros_msg->pdop = ParseFloat(&bin_msg.data_[4]); + + ros_msg->hdop = ParseFloat(&bin_msg.data_[8]); + + ros_msg->vdop = ParseFloat(&bin_msg.data_[12]); + + ros_msg->systems.reserve(num_systems); + for (uint32_t i = 0; i < num_systems; i++) + { + size_t system_offset = BINARY_BODY_LENGTH + i * BINARY_SYSTEM_LENGTH; + novatel_gps_msgs::msg::NovatelPsrdop2System system; + + system.system = GetSystemName(ParseUInt32(&bin_msg.data_[system_offset])); + system.tdop = ParseFloat(&bin_msg.data_[system_offset+4]); + + ros_msg->systems.push_back(system); + } + + return ros_msg; +} + +novatel_gps_driver::Psrdop2Parser::MessageType +novatel_gps_driver::Psrdop2Parser::ParseAscii(const novatel_gps_driver::NovatelSentence& sentence) +{ + if (sentence.body.size() < ASCII_BODY_FIELDS) + { + std::stringstream error; + error << "Unexpected number of body fields in PSRDOP2 log: " << sentence.body.size(); + throw ParseException(error.str()); + } + + uint32_t num_systems = 0; + ParseUInt32(sentence.body[4], num_systems); + + if (sentence.body.size() != ASCII_BODY_FIELDS + num_systems * ASCII_SYSTEM_FIELDS) + { + std::stringstream error; + error << "Size of PSRDOP2 log (" << sentence.body.size() << ") did not match expected size (" + << ASCII_BODY_FIELDS + num_systems * ASCII_SYSTEM_FIELDS << ")."; + throw ParseException(error.str()); + } + + bool valid = true; + auto msg = std::make_unique(); + HeaderParser h_parser; + msg->novatel_msg_header = h_parser.ParseAscii(sentence); + valid &= ParseFloat(sentence.body[0], msg->gdop); + valid &= ParseFloat(sentence.body[1], msg->pdop); + valid &= ParseFloat(sentence.body[2], msg->hdop); + valid &= ParseFloat(sentence.body[3], msg->vdop); + + msg->systems.reserve(num_systems); + for (size_t i = 0; i < num_systems; i++) + { + novatel_gps_msgs::msg::NovatelPsrdop2System system; + size_t offset = 5 + i * ASCII_SYSTEM_FIELDS; + system.system = sentence.body[offset]; + valid &= ParseFloat(sentence.body[offset+1], system.tdop); + msg->systems.push_back(system); + } + + if (!valid) + { + std::stringstream error; + error << "Error parsing PSRDOP2 log."; + throw ParseException(error.str()); + } + return msg; +} + +std::string novatel_gps_driver::Psrdop2Parser::GetSystemName(uint32_t system_id) +{ + switch (system_id) + { + case 0: + return "GPS"; + case 1: + return "GLONASS"; + case 2: + return "GALILEO"; + case 3: + return "BEIDOU"; + case 4: + return "NAVIC"; + case 99: + return "AUTO"; + default: + return "UNKNOWN"; + } +} \ No newline at end of file diff --git a/novatel_gps_driver/test/bestpos-bestvel-psrdop2-sync.pcap b/novatel_gps_driver/test/bestpos-bestvel-psrdop2-sync.pcap new file mode 100644 index 0000000..5e269d3 Binary files /dev/null and b/novatel_gps_driver/test/bestpos-bestvel-psrdop2-sync.pcap differ diff --git a/novatel_gps_driver/test/gpgga-gprmc-bestpos.pcap b/novatel_gps_driver/test/gpgga-gprmc-bestpos.pcap deleted file mode 100644 index 149e988..0000000 Binary files a/novatel_gps_driver/test/gpgga-gprmc-bestpos.pcap and /dev/null differ diff --git a/novatel_gps_driver/test/novatel_gps_tests.cpp b/novatel_gps_driver/test/novatel_gps_tests.cpp index 613019e..a45d731 100644 --- a/novatel_gps_driver/test/novatel_gps_tests.cpp +++ b/novatel_gps_driver/test/novatel_gps_tests.cpp @@ -48,9 +48,11 @@ class NovatelGpsTestSuite : public ::testing::Test, public rclcpp::Node TEST_F(NovatelGpsTestSuite, testGpsFixParsing) { novatel_gps_driver::NovatelGps gps(*this); + gps.wait_for_sync_ = true; std::string path = ament_index_cpp::get_package_prefix("novatel_gps_driver"); - ASSERT_TRUE(gps.Connect(path + "/test/gpgga-gprmc-bestpos.pcap", novatel_gps_driver::NovatelGps::PCAP)); + ASSERT_TRUE(gps.Connect(path + "/test/bestpos-bestvel-psrdop2-sync.pcap", + novatel_gps_driver::NovatelGps::PCAP)); std::vector fix_messages; @@ -64,7 +66,13 @@ TEST_F(NovatelGpsTestSuite, testGpsFixParsing) std::back_inserter(fix_messages)); } - ASSERT_EQ(40, fix_messages.size()); + ASSERT_EQ(22, fix_messages.size()); + + EXPECT_DOUBLE_EQ(fix_messages.front()->latitude, 29.443917634921949); + EXPECT_DOUBLE_EQ(fix_messages.front()->longitude, -98.614755510637181); + EXPECT_DOUBLE_EQ(fix_messages.front()->speed, 0.041456376659522925); + EXPECT_DOUBLE_EQ(fix_messages.front()->track, 135.51629763185957); + EXPECT_DOUBLE_EQ(fix_messages.front()->gdop, 1.9980000257492065); } TEST_F(NovatelGpsTestSuite, testCorrImuDataParsing) diff --git a/novatel_gps_msgs/CMakeLists.txt b/novatel_gps_msgs/CMakeLists.txt index 786f901..1652fbb 100644 --- a/novatel_gps_msgs/CMakeLists.txt +++ b/novatel_gps_msgs/CMakeLists.txt @@ -27,6 +27,8 @@ set(MSG_FILES msg/NovatelHeading2.msg msg/NovatelMessageHeader.msg msg/NovatelPosition.msg + msg/NovatelPsrdop2.msg + msg/NovatelPsrdop2System.msg msg/NovatelReceiverStatus.msg msg/NovatelSignalMask.msg msg/NovatelUtmPosition.msg diff --git a/novatel_gps_msgs/msg/NovatelPsrdop2.msg b/novatel_gps_msgs/msg/NovatelPsrdop2.msg new file mode 100644 index 0000000..a0963aa --- /dev/null +++ b/novatel_gps_msgs/msg/NovatelPsrdop2.msg @@ -0,0 +1,16 @@ +# PSR Dilution of Precision (DOP) values +std_msgs/Header header + +NovatelMessageHeader novatel_msg_header + +# Geometry dilution of precision +float32 gdop +# Position dilution of precision +float32 pdop +# Horizontal dilution of precision +float32 hdop +# Vertical dilution of precision +float32 vdop + +# Time dilution of precision for each GNSS system +NovatelPsrdop2System[] systems diff --git a/novatel_gps_msgs/msg/NovatelPsrdop2System.msg b/novatel_gps_msgs/msg/NovatelPsrdop2System.msg new file mode 100644 index 0000000..0a70853 --- /dev/null +++ b/novatel_gps_msgs/msg/NovatelPsrdop2System.msg @@ -0,0 +1,3 @@ +# GNSS System information provided by PSRDOP2 logs +string system +float32 tdop