Made MockCuckooDeviceDriver more flexible and interactive.

* support random delay simulation
* dynamic reconfigure for some parameters

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_;
 };
 
 }