Add ROS2 ublox_msg_filters package with exact time synchronization

README.md

@@ -167,6 +167,7 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below
 * `publish/mon/hw`: Topic `~monhw`
 
 ### NAV messages
+* NAV messages are time stamped with `i_tow`, and multiple messages can be synchronized with [ublox_msg_filters](ublox_msg_filters)
 * `publish/nav/all`: This is the default value for the `publish/mon/<message>` parameters below. It defaults to `publish/all`. Individual messages can be enabled or disabled by setting the parameters below.
 * `publish/nav/att`: Topic `~navatt`. **ADR/UDR devices only**
 * `publish/nav/clock`: Topic `~navclock`

ublox/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <exec_depend>ublox_serialization</exec_depend>
   <exec_depend>ublox_msgs</exec_depend>
+  <exec_depend>ublox_msg_filters</exec_depend>
   <exec_depend>ublox_gps</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->

ublox_msg_filters/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.5)
+project(ublox_msg_filters)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(ublox_msgs REQUIRED)
+
+set(dependencies "rclcpp" "message_filters" "ublox_msgs")
+
+add_executable(talker src/talker.cpp)
+ament_target_dependencies(talker ${dependencies})
+target_include_directories(talker PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+add_executable(listener src/listener.cpp)
+ament_target_dependencies(listener ${dependencies})
+target_include_directories(listener PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION include/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(TARGETS talker listener
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  ament_add_gtest(${PROJECT_NAME}-test_exact_time_policy test/test_exact_time_policy.cpp)
+  ament_target_dependencies(${PROJECT_NAME}-test_exact_time_policy ${dependencies})
+  target_include_directories(${PROJECT_NAME}-test_exact_time_policy PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+endif()
+
+ament_export_include_directories(include)
+ament_package()

ublox_msg_filters/README.md

@@ -0,0 +1,11 @@
+# ublox_msg_filters
+
+Time synchronize multiple uBlox messages to get a single callback
+
+Port of [message_filters::ExactTime](http://wiki.ros.org/message_filters#ExactTime_Policy) message synchronization policy to use integer time of week in milliseconds `i_tow` instead of `ros::Time` in a header.
+
+The [message_filters::ApproximateTime](http://wiki.ros.org/message_filters#ApproximateTime_Policy) message synchronization policy is not relevent because messages generated by a ublox sensor for a single update contain identical `i_tow` time stamps.
+
+See [src/listener.cpp](src/listener.cpp) for example usage.
+
+Launch the example with `ros2 launch ublox_msg_filters example.launch.xml`

ublox_msg_filters/include/ublox_msg_filters/exact_time.h

@@ -0,0 +1,251 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  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 the Willow Garage 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 THE
+*  COPYRIGHT OWNER 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.
+*********************************************************************/
+
+#ifndef UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
+#define UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
+
+#include <deque>
+#include <string>
+#include <tuple>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "message_filters/connection.h"
+#include "message_filters/message_traits.h"
+#include "message_filters/null_types.h"
+#include "message_filters/signal9.h"
+#include "message_filters/synchronizer.h"
+
+namespace ublox_msg_filters
+{
+namespace sync_policies
+{
+
+using NullType = message_filters::NullType;
+using Connection = message_filters::Connection;
+
+template<typename M0, typename M1, typename M2, typename M3, typename M4,
+         typename M5, typename M6, typename M7, typename M8>
+using PolicyBase = message_filters::PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>;
+
+template<class Policy>
+using Synchronizer = message_filters::Synchronizer<Policy>;
+
+template<typename M>
+struct iTOW
+{
+  static u_int32_t value(const M& m) { return m.i_tow; }
+};
+
+template<>
+struct iTOW<NullType>
+{
+  static u_int32_t value(const NullType&) { return 0; }
+};
+
+template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
+         typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
+struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
+{
+  typedef Synchronizer<ExactTime> Sync;
+  typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> Super;
+  typedef typename Super::Messages Messages;
+  typedef typename Super::Signal Signal;
+  typedef typename Super::Events Events;
+  typedef typename Super::RealTypeCount RealTypeCount;
+  typedef typename Super::M0Event M0Event;
+  typedef typename Super::M1Event M1Event;
+  typedef typename Super::M2Event M2Event;
+  typedef typename Super::M3Event M3Event;
+  typedef typename Super::M4Event M4Event;
+  typedef typename Super::M5Event M5Event;
+  typedef typename Super::M6Event M6Event;
+  typedef typename Super::M7Event M7Event;
+  typedef typename Super::M8Event M8Event;
+  typedef Events Tuple;
+
+  ExactTime(uint32_t queue_size)
+  : parent_(0)
+  , queue_size_(queue_size)
+  , last_signal_time_(0)
+  {
+  }
+
+  ExactTime(const ExactTime& e)
+  {
+    *this = e;
+  }
+
+  ExactTime& operator=(const ExactTime& rhs)
+  {
+    parent_ = rhs.parent_;
+    queue_size_ = rhs.queue_size_;
+    last_signal_time_ = rhs.last_signal_time_;
+    tuples_ = rhs.tuples_;
+
+    return *this;
+  }
+
+  void initParent(Sync* parent)
+  {
+    parent_ = parent;
+  }
+
+  template<int i>
+  void add(const typename std::tuple_element<i, Events>::type& evt)
+  {
+    RCUTILS_ASSERT(parent_);
+
+    namespace mt = message_filters::message_traits;
+
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    Tuple& t = tuples_[iTOW<typename std::tuple_element<i, Messages>::type>::value(*evt.getMessage())];
+    std::get<i>(t) = evt;
+
+    checkTuple(t);
+  }
+
+  template<class C>
+  Connection registerDropCallback(const C& callback)
+  {
+    return drop_signal_.addCallback(callback);
+  }
+
+  template<class C>
+  Connection registerDropCallback(C& callback)
+  {
+    return drop_signal_.addCallback(callback);
+  }
+
+  template<class C, typename T>
+  Connection registerDropCallback(const C& callback, T* t)
+  {
+    return drop_signal_.addCallback(callback, t);
+  }
+
+  template<class C, typename T>
+  Connection registerDropCallback(C& callback, T* t)
+  {
+    return drop_signal_.addCallback(callback, t);
+  }
+
+private:
+
+  // assumes mutex_ is already locked
+  void checkTuple(Tuple& t)
+  {
+    namespace mt = message_filters::message_traits;
+
+    bool full = true;
+    full = full && (bool)std::get<0>(t).getMessage();
+    full = full && (bool)std::get<1>(t).getMessage();
+    full = full && (RealTypeCount::value > 2 ? (bool)std::get<2>(t).getMessage() : true);
+    full = full && (RealTypeCount::value > 3 ? (bool)std::get<3>(t).getMessage() : true);
+    full = full && (RealTypeCount::value > 4 ? (bool)std::get<4>(t).getMessage() : true);
+    full = full && (RealTypeCount::value > 5 ? (bool)std::get<5>(t).getMessage() : true);
+    full = full && (RealTypeCount::value > 6 ? (bool)std::get<6>(t).getMessage() : true);
+    full = full && (RealTypeCount::value > 7 ? (bool)std::get<7>(t).getMessage() : true);
+    full = full && (RealTypeCount::value > 8 ? (bool)std::get<8>(t).getMessage() : true);
+
+    if (full)
+    {
+      parent_->signal(std::get<0>(t), std::get<1>(t), std::get<2>(t),
+                       std::get<3>(t), std::get<4>(t), std::get<5>(t),
+                       std::get<6>(t), std::get<7>(t), std::get<8>(t));
+
+      last_signal_time_ = iTOW<M0>::value(*std::get<0>(t).getMessage());
+
+      tuples_.erase(last_signal_time_);
+
+      clearOldTuples();
+    }
+
+    if (queue_size_ > 0)
+    {
+      while (tuples_.size() > queue_size_)
+      {
+        Tuple& t2 = tuples_.begin()->second;
+        drop_signal_.call(std::get<0>(t2), std::get<1>(t2), std::get<2>(t2),
+                          std::get<3>(t2), std::get<4>(t2), std::get<5>(t2),
+                          std::get<6>(t2), std::get<7>(t2), std::get<8>(t2));
+        tuples_.erase(tuples_.begin());
+      }
+    }
+  }
+
+  // assumes mutex_ is already locked
+  void clearOldTuples()
+  {
+    typename M_TimeToTuple::iterator it = tuples_.begin();
+    typename M_TimeToTuple::iterator end = tuples_.end();
+    for (; it != end;)
+    {
+      if (it->first <= last_signal_time_)
+      {
+        typename M_TimeToTuple::iterator old = it;
+        ++it;
+
+        Tuple& t = old->second;
+        drop_signal_.call(std::get<0>(t), std::get<1>(t), std::get<2>(t),
+                          std::get<3>(t), std::get<4>(t), std::get<5>(t),
+                          std::get<6>(t), std::get<7>(t), std::get<8>(t));
+        tuples_.erase(old);
+      }
+      else
+      {
+        // the map is sorted by time, so we can ignore anything after this if this one's time is ok
+        break;
+      }
+    }
+  }
+
+private:
+  Sync* parent_;
+
+  uint32_t queue_size_;
+  typedef std::map<uint32_t, Tuple> M_TimeToTuple;
+  M_TimeToTuple tuples_;
+  uint32_t last_signal_time_;
+
+  Signal drop_signal_;
+
+  std::mutex mutex_;
+};
+
+}  // namespace sync_policies
+}  // namespace ublox_msg_filters
+
+#endif  // UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
+

ublox_msg_filters/launch/example.launch.xml

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Generate and transmit messages -->
+  <node pkg="ublox_msg_filters" exec="talker" name="talker" output="screen" />
+
+  <!-- Receive and sync messages -->
+  <node pkg="ublox_msg_filters" exec="listener" name="listener" output="screen" />
+
+</launch>

ublox_msg_filters/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ublox_msg_filters</name>
+  <version>2.3.0</version>
+  <description>Time synchronize multiple uBlox messages to get a single callback</description>
+  <author>Kevin Hallenbeck</author>
+  <maintainer email="[email protected]">Kevin Hallenbeck</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/ublox</url>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>message_filters</depend>
+  <depend>ublox_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>