Add support for BESTXYZ topic

novatel_gps_driver/CMakeLists.txt

@@ -52,6 +52,7 @@ add_library(${PROJECT_NAME}
   src/parsers/bestpos.cpp
   src/parsers/bestutm.cpp
   src/parsers/bestvel.cpp
+  src/parsers/bestxyz.cpp
   src/parsers/corrimudata.cpp
   src/parsers/clocksteering.cpp
   src/parsers/gpgga.cpp

novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h

@@ -51,6 +51,7 @@
 #include <novatel_gps_msgs/Insstdev.h>
 #include <novatel_gps_msgs/NovatelCorrectedImuData.h>
 #include <novatel_gps_msgs/NovatelPosition.h>
+#include <novatel_gps_msgs/NovatelXYZ.h>
 #include <novatel_gps_msgs/NovatelUtmPosition.h>
 #include <novatel_gps_msgs/NovatelVelocity.h>
 #include <novatel_gps_msgs/Range.h>
@@ -60,6 +61,7 @@
 #include <novatel_gps_driver/novatel_message_extractor.h>
 
 #include <novatel_gps_driver/parsers/bestpos.h>
+#include <novatel_gps_driver/parsers/bestxyz.h>
 #include <novatel_gps_driver/parsers/bestutm.h>
 #include <novatel_gps_driver/parsers/bestvel.h>
 #include <novatel_gps_driver/parsers/clocksteering.h>
@@ -197,6 +199,12 @@ namespace novatel_gps_driver
        * @param[out] positions New BESTPOS messages.
        */
       void GetNovatelPositions(std::vector<novatel_gps_msgs::NovatelPositionPtr>& positions);
+      /**
+       * @brief Provides any BESTXYZ messages that have been received since the
+       * last time this was called.
+       * @param[out] positions New BESTXYZ messages.
+       */
+      void GetNovatelXYZPositions(std::vector<novatel_gps_msgs::NovatelXYZPtr>& positions);
       /**
        * @brief Provides any BESTUTM messages that have been received since the
        * last time this was called.
@@ -432,6 +440,7 @@ namespace novatel_gps_driver
 
       // Message parsers
       BestposParser bestpos_parser_;
+      BestxyzParser bestxyz_parser_;
       BestutmParser bestutm_parser_;
       BestvelParser bestvel_parser_;
       ClockSteeringParser clocksteering_parser_;
@@ -461,6 +470,7 @@ namespace novatel_gps_driver
       boost::circular_buffer<novatel_gps_msgs::InspvaPtr> inspva_msgs_;
       boost::circular_buffer<novatel_gps_msgs::InsstdevPtr> insstdev_msgs_;
       boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> novatel_positions_;
+      boost::circular_buffer<novatel_gps_msgs::NovatelXYZPtr> novatel_xyz_positions_;
       boost::circular_buffer<novatel_gps_msgs::NovatelUtmPositionPtr> novatel_utm_positions_;
       boost::circular_buffer<novatel_gps_msgs::NovatelVelocityPtr> novatel_velocities_;
       boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> position_sync_buffer_;

novatel_gps_driver/include/novatel_gps_driver/parsers/bestxyz.h

@@ -0,0 +1,58 @@
+// *****************************************************************************
+//
+// Copyright (c) 2017, Southwest Research Institute® (SwRI®)
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// *****************************************************************************
+
+#ifndef NOVATEL_GPS_DRIVER_BESTXYZ_H
+#define NOVATEL_GPS_DRIVER_BESTXYZ_H
+
+#include <novatel_gps_msgs/NovatelXYZ.h>
+
+#include <novatel_gps_driver/parsers/parsing_utils.h>
+#include <novatel_gps_driver/parsers/message_parser.h>
+
+namespace novatel_gps_driver
+{
+  class BestxyzParser : public MessageParser<novatel_gps_msgs::NovatelXYZPtr>
+  {
+  public:
+    uint32_t GetMessageId() const override;
+
+    const std::string GetMessageName() const override;
+
+    novatel_gps_msgs::NovatelXYZPtr ParseBinary(const BinaryMessage& bin_msg) throw(ParseException) override;
+
+    novatel_gps_msgs::NovatelXYZPtr ParseAscii(const NovatelSentence& sentence) throw(ParseException) override;
+
+    static constexpr uint16_t MESSAGE_ID = 241;
+    static constexpr size_t BINARY_LENGTH = 112;
+    static constexpr size_t ASCII_LENGTH = 28;
+    static const std::string MESSAGE_NAME;
+  };
+}
+
+#endif //NOVATEL_GPS_DRIVER_BESTXYZ_H

novatel_gps_driver/src/nodelets/novatel_gps_nodelet.cpp

@@ -180,6 +180,7 @@ namespace novatel_gps_driver
       imu_sample_rate_(-1),
       publish_imu_messages_(false),
       publish_novatel_positions_(false),
+      publish_novatel_xyz_positions_(false),
       publish_novatel_utm_positions_(false),
       publish_novatel_velocity_(false),
       publish_nmea_messages_(false),
@@ -230,6 +231,7 @@ namespace novatel_gps_driver
       swri::param(priv, "publish_gpgsv", publish_gpgsv_, publish_gpgsv_);
       swri::param(priv, "publish_imu_messages", publish_imu_messages_, publish_imu_messages_);
       swri::param(priv, "publish_novatel_positions", publish_novatel_positions_, publish_novatel_positions_);
+      swri::param(priv, "publish_novatel_xyz_positions", publish_novatel_xyz_positions_, publish_novatel_xyz_positions_);
       swri::param(priv, "publish_novatel_utm_positions", publish_novatel_utm_positions_, publish_novatel_utm_positions_);
       swri::param(priv, "publish_novatel_velocity", publish_novatel_velocity_, publish_novatel_velocity_);
       swri::param(priv, "publish_nmea_messages", publish_nmea_messages_, publish_nmea_messages_);
@@ -299,6 +301,11 @@ namespace novatel_gps_driver
         novatel_position_pub_ = swri::advertise<novatel_gps_msgs::NovatelPosition>(node, "bestpos", 100);
       }
 
+      if (publish_novatel_xyz_positions_)
+      { 
+        novatel_xyz_position_pub_ = swri::advertise<novatel_gps_msgs::NovatelXYZ>(node, "bestxyz", 100);
+      }
+
       if (publish_novatel_utm_positions_)
       { 
         novatel_utm_pub_ = swri::advertise<novatel_gps_msgs::NovatelUtmPosition>(node, "bestutm", 100);
@@ -386,7 +393,10 @@ namespace novatel_gps_driver
       opts["gprmc"] = polling_period_;
       opts["bestpos" + format_suffix] = polling_period_;  // Best position
       opts["time" + format_suffix] = 1.0;  // Time
-
+      if (publish_novatel_xyz_positions_)
+      {
+        opts["bestxyz" + format_suffix] = polling_period_;
+      }
       if (publish_novatel_utm_positions_)
       {
         opts["bestutm" + format_suffix] = polling_period_;
@@ -524,6 +534,7 @@ namespace novatel_gps_driver
     bool publish_clock_steering_;
     bool publish_imu_messages_;
     bool publish_novatel_positions_;
+    bool publish_novatel_xyz_positions_;
     bool publish_novatel_utm_positions_;
     bool publish_novatel_velocity_;
     bool publish_nmea_messages_;
@@ -544,6 +555,7 @@ namespace novatel_gps_driver
     ros::Publisher insstdev_pub_;
     ros::Publisher novatel_imu_pub_;
     ros::Publisher novatel_position_pub_;
+    ros::Publisher novatel_xyz_position_pub_;
     ros::Publisher novatel_utm_pub_;
     ros::Publisher novatel_velocity_pub_;
     ros::Publisher gpgga_pub_;
@@ -631,6 +643,7 @@ namespace novatel_gps_driver
     void CheckDeviceForData()
     {
       std::vector<novatel_gps_msgs::NovatelPositionPtr> position_msgs;
+      std::vector<novatel_gps_msgs::NovatelXYZPtr> xyz_position_msgs;
       std::vector<novatel_gps_msgs::NovatelUtmPositionPtr> utm_msgs;
       std::vector<gps_common::GPSFixPtr> fix_msgs;
       std::vector<novatel_gps_msgs::GpggaPtr> gpgga_msgs;
@@ -673,6 +686,7 @@ namespace novatel_gps_driver
       gps_.GetGpggaMessages(gpgga_msgs);
       gps_.GetGprmcMessages(gprmc_msgs);
       gps_.GetNovatelPositions(position_msgs);
+      gps_.GetNovatelXYZPositions(xyz_position_msgs);
       gps_.GetNovatelUtmPositions(utm_msgs);
       gps_.GetFixMessages(fix_msgs);
 
@@ -769,6 +783,16 @@ namespace novatel_gps_driver
         }
       }
 
+      if (publish_novatel_xyz_positions_)
+      {
+        for (const auto& msg : xyz_position_msgs)
+        {
+          msg->header.stamp += sync_offset;
+          msg->header.frame_id = frame_id_;
+          novatel_xyz_position_pub_.publish(msg);
+        }
+      }
+
       if (publish_novatel_utm_positions_)
       {
         for (const auto& msg : utm_msgs)

novatel_gps_driver/src/novatel_gps.cpp

@@ -65,6 +65,7 @@ namespace novatel_gps_driver
       inspva_msgs_(MAX_BUFFER_SIZE),
       insstdev_msgs_(MAX_BUFFER_SIZE),
       novatel_positions_(MAX_BUFFER_SIZE),
+      novatel_xyz_positions_(MAX_BUFFER_SIZE),
       novatel_utm_positions_(MAX_BUFFER_SIZE),
       novatel_velocities_(MAX_BUFFER_SIZE),
       position_sync_buffer_(SYNC_BUFFER_SIZE),
@@ -295,6 +296,13 @@ namespace novatel_gps_driver
     novatel_positions_.clear();
   }
 
+  void NovatelGps::GetNovatelXYZPositions(std::vector<novatel_gps_msgs::NovatelXYZPtr>& positions)
+  {
+    positions.clear();
+    positions.insert(positions.end(), novatel_xyz_positions_.begin(), novatel_xyz_positions_.end());
+    novatel_xyz_positions_.clear();
+  }
+
   void NovatelGps::GetNovatelUtmPositions(std::vector<novatel_gps_msgs::NovatelUtmPositionPtr>& utm_positions)
   {
     utm_positions.clear();
@@ -1016,6 +1024,13 @@ namespace novatel_gps_driver
         position_sync_buffer_.push_back(position);
         break;
       }
+      case BestxyzParser::MESSAGE_ID:
+      {
+        novatel_gps_msgs::NovatelXYZPtr xyz_position = bestxyz_parser_.ParseBinary(msg);
+        xyz_position->header.stamp = stamp;
+        novatel_xyz_positions_.push_back(xyz_position);
+        break;
+      }
       case BestutmParser::MESSAGE_ID:
       {
         novatel_gps_msgs::NovatelUtmPositionPtr utm_position = bestutm_parser_.ParseBinary(msg);
@@ -1185,6 +1200,12 @@ namespace novatel_gps_driver
       novatel_positions_.push_back(position);
       position_sync_buffer_.push_back(position);
     }
+    if (sentence.id == "BESTXYZ")
+    {
+      novatel_gps_msgs::NovatelXYZPtr position = bestxyz_parser_.ParseAscii(sentence);
+      position->header.stamp = stamp;
+      novatel_xyz_positions_.push_back(position);
+    }
     if (sentence.id == "BESTUTMA")
     {
       novatel_gps_msgs::NovatelUtmPositionPtr utm_position = bestutm_parser_.ParseAscii(sentence);