Updates to handle NavPVT

CMakeLists.txt

@@ -24,7 +24,8 @@ find_package(catkin REQUIRED COMPONENTS
   std_srvs
   tf2
   tf2_geometry_msgs
-  tf2_ros)
+  tf2_ros
+  ublox_msgs)
 
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(YAML_CPP yaml-cpp)
@@ -122,6 +123,7 @@ catkin_package(
     tf2
     tf2_geometry_msgs
     tf2_ros
+    ublox_msgs
   DEPENDS
     ${EIGEN_PACKAGE}
     GeographicLib
@@ -150,7 +152,7 @@ add_library(ros_robot_localization_listener src/ros_robot_localization_listener.
 add_library(ros_filter_utilities src/ros_filter_utilities.cpp)
 add_library(ros_filter_bias_estimator src/ros_filter_bias_estimator.cpp)
 add_library(ros_filter src/ros_filter.cpp)
-add_library(navsat_transform src/navsat_transform.cpp)
+add_library(navsat_transform src/navsat_transform.cpp src/mkgmtime.c)
 add_library(ekf_localization_nodelet src/ekf_localization_nodelet.cpp)
 add_library(ukf_localization_nodelet src/ukf_localization_nodelet.cpp)
 add_library(navsat_transform_nodelet src/navsat_transform_nodelet.cpp)

include/robot_localization/mkgmtime.h

@@ -0,0 +1,55 @@
+/* mkgmtime.h -- make a time_t from a gmtime struct tm
+ $Id: mkgmtime.h,v 1.5 2003/02/13 20:15:41 rjs3 Exp $
+ 
+ * Copyright (c) 1998-2003 Carnegie Mellon University.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer. 
+ *
+ * 2. 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.
+ *
+ * 3. The name "Carnegie Mellon University" must not be used to
+ *    endorse or promote products derived from this software without
+ *    prior written permission. For permission or any other legal
+ *    details, please contact  
+ *      Office of Technology Transfer
+ *      Carnegie Mellon University
+ *      5000 Forbes Avenue
+ *      Pittsburgh, PA  15213-3890
+ *      (412) 268-4387, fax: (412) 268-7395
+ *      [email protected]
+ *
+ * 4. Redistributions of any form whatsoever must retain the following
+ *    acknowledgment:
+ *    "This product includes software developed by Computing Services
+ *     at Carnegie Mellon University (http://www.cmu.edu/computing/)."
+ *
+ * CARNEGIE MELLON UNIVERSITY DISCLAIMS ALL WARRANTIES WITH REGARD TO
+ * THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS, IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE
+ * FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN
+ * AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING
+ * OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+#ifndef INCLUDED_MKGMTIME_H
+#define INCLUDED_MKGMTIME_H
+
+#include <time.h>
+
+/**
+ * @brief Get the UTC time in seconds and nano-seconds from a time struct in
+ * GM time.
+ */
+extern time_t mkgmtime(struct tm * const tmp);
+
+#endif /* INCLUDED_MKGMTIME_H */

include/robot_localization/navsat_transform.h

@@ -43,6 +43,11 @@
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/NavSatFix.h>
+#include <ublox_msgs/NavPVT.h>
+
+extern "C" {
+  #include "mkgmtime.h"
+}
 
 #include <tf2/LinearMath/Transform.h>
 #include <tf2_ros/static_transform_broadcaster.h>
@@ -130,6 +135,11 @@ class NavSatTransform
     //!
     void odomCallback(const nav_msgs::OdometryConstPtr& msg);
 
+    //! @brief Callback for the GPS Nav PVT data
+    //! @param[in] msg The NavPVT message to process
+    //!
+    void gpsNavPVTCallback(const ublox_msgs::NavPVTConstPtr& msg);
+
     //! @brief Converts the odometry data back to GPS and broadcasts it
     //! @param[out] filtered_gps The NavSatFix message to prepare
     //!
@@ -160,6 +170,27 @@ class NavSatTransform
     //!
     void mapToLL(const tf2::Vector3& point, double& latitude, double& longitude, double& altitude) const;
 
+    //! @brief converts a NavPVT message time to seconds since epoc
+    //! @param[in] msg NavPVT the message with time to convert
+    //! @return time in seconds since epoc
+    //!
+    inline long uBloxTimeToUtcSeconds(const ublox_msgs::NavPVTConstPtr& msg) {
+      // Create TM struct for mkgmtime
+      struct tm time = {0};
+      time.tm_year = msg->year - 1900; 
+      time.tm_mon = msg->month - 1; 
+      time.tm_mday = msg->day;
+      time.tm_hour = msg->hour;   
+      time.tm_min = msg->min; 
+      time.tm_sec = msg->sec;
+      // Use the version that is included (with attribution)
+      return mkgmtime(&time);
+    }
+
+    //! @brief Whether using NavPVT message (doesn't need a filter node)
+    //!
+    bool use_nav_pvt_;
+
     //! @brief Whether or not we broadcast the cartesian transform
     //!
     bool broadcast_cartesian_transform_;
@@ -359,6 +390,10 @@ class NavSatTransform
     //!
     ros::Subscriber gps_sub_;
 
+    //! @brief GPS Nav PVT subscriber
+    //!
+    ros::Subscriber gps_nav_pvt_sub_;
+
     //! @brief Subscribes to imu topic
     //!
     ros::Subscriber imu_sub_;

package.xml

@@ -34,6 +34,8 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>yaml-cpp</depend>
+  <depend>ublox_msgs</depend>
+  <depend>ublox_gps</depend>
 
   <build_depend>message_generation</build_depend>
   <build_depend condition="$ROS_PYTHON_VERSION == 2">python-catkin-pkg</build_depend>

src/mkgmtime.c

@@ -0,0 +1,156 @@
+/* mkgmtime.c - make time corresponding to a GMT timeval struct
+ $Id: mkgmtime.c,v 1.10 2003/10/22 18:50:12 rjs3 Exp $
+ 
+ * Copyright (c) 1998-2003 Carnegie Mellon University.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer. 
+ *
+ * 2. 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.
+ *
+ * 3. The name "Carnegie Mellon University" must not be used to
+ *    endorse or promote products derived from this software without
+ *    prior written permission. For permission or any other legal
+ *    details, please contact  
+ *      Office of Technology Transfer
+ *      Carnegie Mellon University
+ *      5000 Forbes Avenue
+ *      Pittsburgh, PA  15213-3890
+ *      (412) 268-4387, fax: (412) 268-7395
+ *      [email protected]
+ *
+ * 4. Redistributions of any form whatsoever must retain the following
+ *    acknowledgment:
+ *    "This product includes software developed by Computing Services
+ *     at Carnegie Mellon University (http://www.cmu.edu/computing/)."
+ *
+ * CARNEGIE MELLON UNIVERSITY DISCLAIMS ALL WARRANTIES WITH REGARD TO
+ * THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS, IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE
+ * FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN
+ * AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING
+ * OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ *
+ */
+/*
+ * Copyright (c) 1987, 1989, 1993
+ *  The Regents of the University of California.  All rights reserved.
+ *
+ * This code is derived from software contributed to Berkeley by
+ * Arthur David Olson of the National Cancer Institute.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *  This product includes software developed by the University of
+ *  California, Berkeley and its contributors.
+ * 4. Neither the name of the University 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 REGENTS 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 THE REGENTS OR CONTRIBUTORS 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.
+ */
+
+/*
+** Adapted from code provided by Robert Elz, who writes:
+**  The "best" way to do mktime I think is based on an idea of Bob
+**  Kridle's (so its said...) from a long time ago. (mtxinu!kridle now).
+**  It does a binary search of the time_t space.  Since time_t's are
+**  just 32 bits, its a max of 32 iterations (even at 64 bits it
+**  would still be very reasonable).
+*/
+
+#include "ublox_gps/mkgmtime.h"
+#ifndef WRONG
+#define WRONG   (-1)
+#endif /* !defined WRONG */
+
+static int tmcomp(register const struct tm * const  atmp, 
+                  register const struct tm * const btmp)
+{
+    register int    result;
+
+    if ((result = (atmp->tm_year - btmp->tm_year)) == 0 &&
+        (result = (atmp->tm_mon - btmp->tm_mon)) == 0 &&
+        (result = (atmp->tm_mday - btmp->tm_mday)) == 0 &&
+        (result = (atmp->tm_hour - btmp->tm_hour)) == 0 &&
+        (result = (atmp->tm_min - btmp->tm_min)) == 0)
+            result = atmp->tm_sec - btmp->tm_sec;
+    return result;
+}
+
+time_t mkgmtime(struct tm * const tmp) {
+    register int            dir;
+    register int            bits;
+    register int            saved_seconds;
+    time_t              t;
+    struct tm           yourtm, *mytm;
+
+    yourtm = *tmp;
+    saved_seconds = yourtm.tm_sec;
+    yourtm.tm_sec = 0;
+    /*
+    ** Calculate the number of magnitude bits in a time_t
+    ** (this works regardless of whether time_t is
+    ** signed or unsigned, though lint complains if unsigned).
+    */
+    for (bits = 0, t = 1; t > 0; ++bits, t <<= 1)
+        ;
+    /*
+    ** If time_t is signed, then 0 is the median value,
+    ** if time_t is unsigned, then 1 << bits is median.
+    */
+    t = (t < 0) ? 0 : ((time_t) 1 << bits);
+
+    /* Some gmtime() implementations are broken and will return
+     * NULL for time_ts larger than 40 bits even on 64-bit platforms
+     * so we'll just cap it at 40 bits */
+    if(bits > 40) bits = 40;
+
+    for ( ; ; ) {
+        mytm = gmtime(&t);
+
+        if(!mytm) return WRONG;
+
+        dir = tmcomp(mytm, &yourtm);
+        if (dir != 0) {
+            if (bits-- < 0)
+                return WRONG;
+            if (bits < 0)
+                --t;
+            else if (dir > 0)
+                t -= (time_t) 1 << bits;
+            else    t += (time_t) 1 << bits;
+            continue;
+        }
+        break;
+    }
+    t += saved_seconds;
+    return t;
+}