Merge pull request #46 from ethz-asl/feature/offsetAndMockCuckooClock

Feature/offset and mock cuckoo clock

Author: HannesSommer

cuckoo_time_translator/cfg/MockCuckooClock.cfg

@@ -0,0 +1,22 @@
+#!/usr/bin/env python
+PACKAGE='cuckoo_time_translator'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("assumedDelay",          double_t, 0,
+        "Assumed deterministic transport delay in seconds.",
+         0, 0.0, 1)
+
+gen.add("delay",          double_t, 0,
+        "Actually simulated deterministic transport delay in seconds.",
+         0, 0.0, 1)
+
+gen.add("delaySigma",          double_t, 0,
+        "Simulated random transport delay in seconds (exponential distribution with lambda=1/delaySigma).",
+         0, 0.0, 1)
+
+
+
+exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "MockCuckooClock"))

cuckoo_time_translator/src/MockCuckooDeviceDriver.cpp

@@ -1,10 +1,11 @@
 #include "MockCuckooDeviceDriver.h"
 
+#include <random>
+
 #include <cuckoo_time_translator/KalmanOwt.h>
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-parameter"
-#include "ros/ros.h"
-#include "std_msgs/String.h"
+#include <ros/ros.h>
 #pragma GCC diagnostic pop
 
 namespace cuckoo_time_translator {
@@ -19,28 +20,50 @@ KalmanOwtConfig getMockKalmanConfig() {
   return kc;
 }
 
-
 MockCuckooDeviceDriver::MockCuckooDeviceDriver(ros::NodeHandle & nh) :
-  cuckooClock(kWrappingNumber, kFreq, kSkew, ros::Duration(kOffset)),
-  translator(WrappingClockParameters{kWrappingNumber, kFreq}, nh.getNamespace(), Defaults().setFilterConfig(getMockKalmanConfig()))
+  cuckooClock_(kWrappingNumber, kFreq, kSkew, ros::Duration(kOffset)),
+  translator_(WrappingClockParameters{kWrappingNumber, kFreq}, nh.getNamespace(), Defaults().setFilterConfig(getMockKalmanConfig()))
 {
+  srv_.setCallback(boost::bind(&MockCuckooDeviceDriver::dynamicReconfigureCallback, this, _1, _2));
+
+  nh.param("delay", delaySeconds_, delaySeconds_);
+  nh.param("delaySigma", delaySigmaSeconds_, delaySigmaSeconds_);
+  nh.param("assumedDelay", assumedDelaySeconds_, assumedDelaySeconds_);
+  ROS_INFO("Using initial delay=%g s, delaySigma=%g s, and assumedDelay=%g s.", delaySeconds_, delaySigmaSeconds_, assumedDelaySeconds_);
 }
 
 
 void MockCuckooDeviceDriver::step() {
   ros::Time receiveTime;
   uint32_t deviceTime;
 
-  cuckooClock.getNewSimulatedMeasurementTimes(deviceTime, receiveTime);
+  static std::default_random_engine generator;
+
+  cuckooClock_.getNewSimulatedMeasurementTimes(deviceTime, receiveTime);
 
-  translator.update(deviceTime, receiveTime);
+  receiveTime += ros::Duration(delaySeconds_);
+  if(delaySigmaSeconds_){
+    std::exponential_distribution<double> delayDistribution(1/delaySigmaSeconds_);
+    receiveTime += ros::Duration(delayDistribution(generator));
+  }
+
+  translator_.update(deviceTime, receiveTime, -assumedDelaySeconds_);
 }
 } // namespace cuckoo_time_translator
 
 using namespace cuckoo_time_translator;
 
-int main(int argc, char **argv)
-{
+
+
+
+void MockCuckooDeviceDriver::dynamicReconfigureCallback(const MockCuckooClockConfig &config, int) {
+  assumedDelaySeconds_ = config.assumedDelay;
+  delaySeconds_ = config.delay;
+  delaySigmaSeconds_ = config.delaySigma;
+  ROS_INFO("Using updated delay=%g s, delaySigma=%g s, and assumedDelay=%g s.", delaySeconds_, delaySigmaSeconds_, assumedDelaySeconds_);
+}
+
+int main(int argc, char **argv) {
   ros::init(argc, argv, "mock_cuckoo_device_driver");
 
   ros::NodeHandle nh;
@@ -58,4 +81,3 @@ int main(int argc, char **argv)
 
   return 0;
 }
-

cuckoo_time_translator/src/MockCuckooDeviceDriver.h

@@ -4,9 +4,11 @@
 #include <cuckoo_time_translator/DeviceTimeTranslator.h>
 #include "MockCuckooClock.h"
 
-namespace ros {
-class NodeHandle;
-}
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#include <dynamic_reconfigure/server.h>
+#include <cuckoo_time_translator/MockCuckooClockConfig.h>
+#pragma GCC diagnostic pop
 
 namespace cuckoo_time_translator {
 
@@ -21,8 +23,15 @@ class MockCuckooDeviceDriver {
 
   void step();
  private:
-  MockCuckooClock cuckooClock;
-  DeviceTimeUnwrapperAndTranslator<TimestampUnwrapperEventOnly> translator;
+  void dynamicReconfigureCallback(const MockCuckooClockConfig &config, int);
+
+  MockCuckooClock cuckooClock_;
+  DeviceTimeUnwrapperAndTranslator<TimestampUnwrapperEventOnly> translator_;
+  double delaySeconds_ = 0;
+  double delaySigmaSeconds_ = 0;
+  double assumedDelaySeconds_ = 0;
+
+  dynamic_reconfigure::Server<MockCuckooClockConfig> srv_;
 };
 
 }

cuckoo_time_translator_python/python/cuckoo_time_translator/device_time_bags.py

@@ -2,6 +2,7 @@
 
 import pickle
 import rosbag
+import numpy as np
 
 from timestamp_series import TimestampSeries
 from timestamp_series import TimestampSeries
@@ -35,13 +36,17 @@ def __init__(self, bagFile, topic, invalidate=False):
     bag = rosbag.Bag(bagFile)
 
     self.translated_hw_times = TimestampSeries()
+    self.translated_hw_times_without_offset = TimestampSeries()
     self.receive_times = TimestampSeries()
     self.raw_hw_times = TimestampSeries()
     for topic, msg, t in bag.read_messages(topics=[topic]):
       self.translated_hw_times.append(msg.header.stamp.to_sec())
+      self.translated_hw_times_without_offset.append(msg.header.stamp.to_sec() - msg.offset_secs)
       self.receive_times.append(msg.receive_time.to_sec())
       self.raw_hw_times.append(float(msg.event_stamp))
 
+    self.zeroOffsetAllTheTime = np.linalg.norm(np.array(self.translated_hw_times_without_offset) - np.array(self.translated_hw_times)) < 1e-9
+
     bag.close()
     # Save data to file
     pickle.dump(self.__dict__, open(eventsFile, "wb"))

cuckoo_time_translator_python/scripts/ctt_introspect.py

@@ -83,7 +83,9 @@ def addToPlot(times, label, color):
     if not args.dontPlotReceiveTimes:
       addToPlot(ds.receive_times, "receive times", "r")
     if not args.dontPlotPreTranslated:
-      addToPlot(ds.translated_hw_times, "pre-translated", "g")
+      addToPlot(ds.translated_hw_times, "pre-translated" + (" (+ zero offset)" if ds.zeroOffsetAllTheTime else " + offset"), "g")
+      if not ds.zeroOffsetAllTheTime:
+          addToPlot(ds.translated_hw_times_without_offset, "pre-translated without offset", "b")
 
     owtColors = ['m', 'grey', 'cyan', 'k', 'orange']
     for i, owt in enumerate(hwOwts):