Add ublox_msg_filters package to time synchronize multiple messages w…

…ith i_tow to get a single callback

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

@@ -32,8 +32,8 @@
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
-#ifndef MESSAGE_FILTERS__SYNC_EXACT_TIME_H_
-#define MESSAGE_FILTERS__SYNC_EXACT_TIME_H_
+#ifndef UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
+#define UBLOX_MSG_FILTERS__EXACT_TIME_HPP_
 
 #include <deque>
 #include <string>
@@ -47,11 +47,33 @@
 #include "message_filters/signal9.h"
 #include "message_filters/synchronizer.h"
 
-namespace message_filters
+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>
@@ -76,6 +98,7 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
   ExactTime(uint32_t queue_size)
   : parent_(0)
   , queue_size_(queue_size)
+  , last_signal_time_(0)
   {
   }
 
@@ -108,7 +131,7 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
 
     std::lock_guard<std::mutex> lock(mutex_);
 
-    Tuple& t = tuples_[mt::TimeStamp<typename std::tuple_element<i, Messages>::type>::value(*evt.getMessage())];
+    Tuple& t = tuples_[iTOW<typename std::tuple_element<i, Messages>::type>::value(*evt.getMessage())];
     std::get<i>(t) = evt;
 
     checkTuple(t);
@@ -162,7 +185,7 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
                        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_ = mt::TimeStamp<M0>::value(*std::get<0>(t).getMessage());
+      last_signal_time_ = iTOW<M0>::value(*std::get<0>(t).getMessage());
 
       tuples_.erase(last_signal_time_);
 
@@ -212,17 +235,17 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
   Sync* parent_;
 
   uint32_t queue_size_;
-  typedef std::map<rclcpp::Time, Tuple> M_TimeToTuple;
+  typedef std::map<uint32_t, Tuple> M_TimeToTuple;
   M_TimeToTuple tuples_;
-  rclcpp::Time last_signal_time_;
+  uint32_t last_signal_time_;
 
   Signal drop_signal_;
 
   std::mutex mutex_;
 };
 
 }  // namespace sync_policies
-}  // namespace message_filters
+}  // namespace ublox_msg_filters
 
-#endif  // MESSAGE_FILTERS__SYNC_EXACT_TIME_H_
+#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>

ublox_msg_filters/src/listener.cpp

@@ -0,0 +1,46 @@
+#include <rclcpp/rclcpp.hpp>
+#include <ublox_msgs/msg/nav_posllh.hpp>
+#include <ublox_msgs/msg/nav_relposned9.hpp>
+#include <ublox_msgs/msg/nav_velned.hpp>
+#include <ublox_msg_filters/exact_time.h>
+#include <message_filters/subscriber.h>
+
+class Listener : public rclcpp::Node {
+public:
+  Listener() : Node("listener"),
+    sub1_(this, "msg1"),
+    sub2_(this, "msg2"),
+    sub3_(this, "msg3"),
+    sync_(MySyncPolicy(10), sub1_, sub2_, sub3_)
+  {
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+    using std::placeholders::_3;
+    sync_.registerCallback(std::bind(&Listener::callback, this, _1, _2, _3));
+    RCLCPP_INFO(this->get_logger(), "Ready to receive");
+  }
+
+private:
+  void callback(const ublox_msgs::msg::NavPOSLLH::ConstSharedPtr msg1,
+                const ublox_msgs::msg::NavRELPOSNED9::ConstSharedPtr msg2,
+                const ublox_msgs::msg::NavVELNED::ConstSharedPtr msg3) {
+    RCLCPP_INFO(this->get_logger(), "RX %u %u %u", msg1->i_tow, msg2->i_tow, msg3->i_tow);
+  }
+
+private:
+  message_filters::Subscriber<ublox_msgs::msg::NavPOSLLH> sub1_;
+  message_filters::Subscriber<ublox_msgs::msg::NavRELPOSNED9> sub2_;
+  message_filters::Subscriber<ublox_msgs::msg::NavVELNED> sub3_;
+  typedef ublox_msg_filters::sync_policies::ExactTime<ublox_msgs::msg::NavPOSLLH,
+                                                      ublox_msgs::msg::NavRELPOSNED9,
+                                                      ublox_msgs::msg::NavVELNED> MySyncPolicy;
+  message_filters::Synchronizer<MySyncPolicy> sync_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc,argv);
+  rclcpp::spin(std::make_shared<Listener>());
+  rclcpp::shutdown();
+  return 0;
+}

ublox_msg_filters/src/talker.cpp

@@ -0,0 +1,50 @@
+#include <rclcpp/rclcpp.hpp>
+#include <ublox_msgs/msg/nav_posllh.hpp>
+#include <ublox_msgs/msg/nav_relposned9.hpp>
+#include <ublox_msgs/msg/nav_velned.hpp>
+
+class Talker : public rclcpp::Node {
+public:
+  Talker() : Node("talker") {
+    using namespace std::chrono_literals;
+    pub1_ = this->create_publisher<ublox_msgs::msg::NavPOSLLH>("msg1", 2);
+    pub2_ = this->create_publisher<ublox_msgs::msg::NavRELPOSNED9>("msg2", 2);
+    pub3_ = this->create_publisher<ublox_msgs::msg::NavVELNED>("msg3", 2);
+    timer_ = this->create_wall_timer(0.2s, std::bind(&Talker::publish, this));
+    RCLCPP_INFO(this->get_logger(), "Ready to transmit");
+  }
+
+private:
+  void publish() {
+    RCLCPP_INFO(this->get_logger(), "TX %u %u %u", i_tow_, i_tow_ + 1, i_tow_ + 3);
+
+    auto msg1 = ublox_msgs::msg::NavPOSLLH();
+    msg1.i_tow = i_tow_;
+    pub1_->publish(msg1);
+
+    auto msg2 = ublox_msgs::msg::NavRELPOSNED9();
+    msg2.i_tow = i_tow_ + 1;
+    pub2_->publish(msg2);
+
+    auto msg3 = ublox_msgs::msg::NavVELNED();
+    msg3.i_tow = i_tow_ + 3;
+    pub3_->publish(msg3);
+
+    i_tow_++;
+  }
+
+private:
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<ublox_msgs::msg::NavPOSLLH>::SharedPtr pub1_;
+  rclcpp::Publisher<ublox_msgs::msg::NavRELPOSNED9>::SharedPtr pub2_;
+  rclcpp::Publisher<ublox_msgs::msg::NavVELNED>::SharedPtr pub3_;
+  uint32_t i_tow_ = 0;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Talker>());
+  rclcpp::shutdown();
+  return 0;
+}