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