Merge pull request #43 from ethz-asl/cleanup/overall

Cleanup/overall

Author: HannesSommer

README.md

@@ -20,14 +20,21 @@ Howevere, this step is beyond this repository. If you need that the following re
 * [ethz-asl/kalibr](https://github.com/ethz-asl/kalibr/)
 * [ethz-asl/oomact](https://github.com/ethz-asl/oomact/)
 
+
 ### Why call it translation rather than synchronization? 
 Because **synchronization** refers to what one does to clocks to make them run at the same speed and yield similar times when read at the same time (as for instance NTP and PTP do to computer clocks), whereas the algorithms in this repository are trying to only **translate** the time of one clock for a given event into the another clock's time for the *same* event and not touch the clocks at all.
 
 
+### Dependencies
+* [caktin_simple](https://github.com/catkin/catkin_simple)
+* ros-*-cmake-modules
+* ros-*-rosconsole-bridge
+* ros-*-dynamic-reconfigure
+
+
 ### Examples for how to use it in a ROS sensor driver:
 See [usage examples](https://github.com/ethz-asl/cuckoo_time_translator/wiki#usage-examples-ros-sensor-drivers).
 
 
-
 ## Acknowledgments
 This work is supported in part by the European Union's Seventh Framework Programme (FP7/2007-2013) under grant #610603 (EUROPA2) and the European Union's Horizon 2020 research and innovation programme under grant agreement No 644227 (FLOURISH).

cuckoo_time_translator/test/TestDefaults.cpp

@@ -15,9 +15,9 @@ TEST(Defaults, KalmanDefaults) {
   ASSERT_TRUE(k);
 
   KalmanOwtConfig kc;
-  EXPECT_EQ(kc.updateRate, k->getConfig().updateRate);
+  EXPECT_EQ(kc.updateCooldownSecs, k->getConfig().updateCooldownSecs);
 
-  kc.updateRate += 1.0;
+  kc.updateCooldownSecs += 1.0;
 
   d.setFilterConfig(kc);
 
@@ -26,5 +26,5 @@ TEST(Defaults, KalmanDefaults) {
   k = dynamic_cast<KalmanOwt*>(uk.get());
   ASSERT_TRUE(k);
 
-  EXPECT_EQ(kc.updateRate, k->getConfig().updateRate);
+  EXPECT_EQ(kc.updateCooldownSecs, k->getConfig().updateCooldownSecs);
 }

cuckoo_time_translator_algorithms/include/cuckoo_time_translator/KalmanOwt.h

@@ -15,7 +15,7 @@ struct KalmanOwtConfig {
   double sigmaInitSkew = 1e-3;
   double sigmaOffset = 2e-3;
   double sigmaSkew = 2e-6;
-  double updateRate = 0.5;
+  double updateCooldownSecs = 0.5;
   double outlierThreshold = 1.0;
 };
 
@@ -51,7 +51,7 @@ class KalmanOwt : public OneWayTranslator
   Eigen::Matrix<double, 1, 2> H_;
   bool isInitialized_;
   double lastUpdateDeviceTime_;
-  double dt_;
+  double lastUpdateDt_;
 };
 
 }

cuckoo_time_translator_algorithms/src/KalmanOwt.cpp

@@ -12,7 +12,7 @@ KalmanOwt::KalmanOwt(Config config) :
   R_ (0),
   isInitialized_(false),
   lastUpdateDeviceTime_(0),
-  dt_(0)
+  lastUpdateDt_(0)
 {
 }
 
@@ -27,7 +27,7 @@ LocalTime KalmanOwt::translateToLocalTimestamp(const RemoteTime remoteTimeTics)
 
 void KalmanOwt::printNameAndConfig(std::ostream & out) const {
   out << "Kalman("
-      << "updateRate=" << config.updateRate
+      << "updateCooldownSecs=" << config.updateCooldownSecs
       << ", "
       << "sigmaSkew=" << config.sigmaSkew
       << ", "
@@ -41,7 +41,7 @@ void KalmanOwt::printNameAndConfig(std::ostream & out) const {
       << ")";
 }
 void KalmanOwt::printState(std::ostream & out) const {
-  out << "offset=" << std::fixed << x_(0) << ", skew=" << x_(1) << ", dt=" << dt_;
+  out << "offset=" << std::fixed << x_(0) << ", skew=" << x_(1) << ", dt=" << lastUpdateDt_;
 }
 
 void KalmanOwt::reset() {
@@ -56,15 +56,16 @@ LocalTime KalmanOwt::updateAndTranslateToLocalTimestamp(const RemoteTime remoteT
 
   const double dt = remoteTimeTics - lastUpdateDeviceTime_;
 
-  if(dt >= config.updateRate){
-    dt_=dt;
-    Eigen::Matrix2d F;
-    F << 1, dt_, 0, 1;
+  if(dt >= config.updateCooldownSecs){
+    // Save some values for later usage
+    lastUpdateDeviceTime_ = remoteTimeTics;
+    lastUpdateDt_ = dt;
 
     // Prediction
+    Eigen::Matrix2d F;
+    F << 1, dt, 0, 1;
     x_ = F * x_;
-    P_ = F * P_ * F.transpose() + dt_ * Q_;
-    lastUpdateDeviceTime_ = remoteTimeTics;
+    P_ = F * P_ * F.transpose() + dt * Q_;
 
     // Update
     const double S = H_ * P_ * H_.transpose() + R_;

cuckoo_time_translator_python/CMakeLists.txt

@@ -29,30 +29,33 @@ find_package(Boost REQUIRED COMPONENTS python)
 include_directories(${PYTHON_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 
 catkin_python_setup()
+
 cs_add_library(${PROJECT_NAME}
   src/module.cpp
 )
+
 string(REGEX REPLACE "_python$" "" LIB_OUTPUT_DIR ${CATKIN_PACKAGE_PYTHON_DESTINATION})
-set_target_properties(${PROJECT_NAME}
-    PROPERTIES
-        LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${LIB_OUTPUT_DIR}
+set_target_properties(${PROJECT_NAME} PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${LIB_OUTPUT_DIR}
 )
 
 if (APPLE)
-    set_target_properties(${PROJECT_NAME}
-        PROPERTIES
-            SUFFIX ".so" # This is apparently needed for Darvin. But why?
-    )
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    SUFFIX ".so" # This is apparently needed for Darvin. But why
+  )
 endif ()
 
 target_link_libraries(${PROJECT_NAME} ${PYTHON_LIBRARY} ${Boost_LIBRARIES})
 
-cs_install()
+install(FILES ${CATKIN_DEVEL_PREFIX}/${LIB_OUTPUT_DIR}/lib${PROJECT_NAME}.so
+  DESTINATION ${LIB_OUTPUT_DIR}
+)
+
 cs_export()
 
 if(CATKIN_ENABLE_TESTING)
-catkin_add_nosetests(
-  test/Test.py
-  DEPENDENCIES ${PROJECT_NAME}
-)
+  catkin_add_nosetests(
+    test/Test.py
+    DEPENDENCIES ${PROJECT_NAME}
+  )
 endif()

cuckoo_time_translator_python/python/cuckoo_time_translator/__init__.py

@@ -1 +0,0 @@
-from libcuckoo_time_translator_python import *

cuckoo_time_translator_python/python/cuckoo_time_translator/algorithms.py

@@ -0,0 +1 @@
+from .libcuckoo_time_translator_python import *

cuckoo_time_translator_python/python/cuckoo_time_translator/batch_algo.py

@@ -3,24 +3,26 @@
 import numpy as np
 import sys
 
+
 def chunks(l, n):
     """Yield successive n-sized chunks from l."""
     for i in range(0, len(l), n):
         yield l[i:i + n]
 
-def printDelayStat(delays, name, outlierLimit = None, file = None, chunkSizes = ()):
+
+def printDelayStat(delays, name, outlierLimit=None, file=None, chunkSizes=()):
     if delays is not None and len(delays):
         if outlierLimit:
             delays = delays[delays < outlierLimit]
-            
-        delays = np.array(delays) * 1e3 # convert to ms
-        
+
+        delays = np.array(delays) * 1e3  # convert to ms
+
         files = [sys.stdout]
         if file: files.append(file)
         for f in files:
-            print("%s: mean=%f ms, std=%f ms" % (name, np.mean(delays), np.std(delays)), file = f)
+            print("%s: mean=%f ms, std=%f ms" % (name, np.mean(delays), np.std(delays)), file=f)
             for n in chunkSizes:
-                print("std(%s over %d chunks) = %g ms" %(name, n, np.std([ np.mean(c) for c in chunks(delays, n)])), file = f)
+                print("std(%s over %d chunks) = %g ms" % (name, n, np.std([ np.mean(c) for c in chunks(delays, n)])), file=f)
 
     else :
         print("No %s data!" % name)

cuckoo_time_translator_python/python/cuckoo_time_translator/device_time_bags.py

@@ -1,7 +1,4 @@
-from __future__ import print_function
-
 import os
-import sys
 
 import pickle
 import rosbag
@@ -12,6 +9,7 @@
 
 DefaultTopic = '/device_time'
 
+
 def guessTopics(bagFile):
   with rosbag.Bag(bagFile) as bag:
     found = []
@@ -21,8 +19,10 @@ def guessTopics(bagFile):
 
   return found
 
+
 class DeviceTimeStream():
-  def __init__(self, bagFile, topic, invalidate = False):
+
+  def __init__(self, bagFile, topic, invalidate=False):
     eventsFile = os.path.realpath(bagFile) + topic.replace("/", "_") + ".p"
     if os.path.exists(eventsFile):
         if invalidate:
@@ -31,14 +31,14 @@ def __init__(self, bagFile, topic, invalidate = False):
             verbose("Loading from " + eventsFile)
             self.__dict__ = pickle.load(open(eventsFile, "rb"))
             return
-    
+
     bag = rosbag.Bag(bagFile)
 
-    self.filtered_hw_times = TimestampSeries()
+    self.translated_hw_times = TimestampSeries()
     self.receive_times = TimestampSeries()
     self.raw_hw_times = TimestampSeries()
     for topic, msg, t in bag.read_messages(topics=[topic]):
-      self.filtered_hw_times.append(msg.header.stamp.to_sec())
+      self.translated_hw_times.append(msg.header.stamp.to_sec())
       self.receive_times.append(msg.receive_time.to_sec())
       self.raw_hw_times.append(float(msg.event_stamp))
 

cuckoo_time_translator_python/python/cuckoo_time_translator/plotting.py

@@ -2,7 +2,6 @@
 
 from __future__ import print_function
 
-import sys
 import os
 import numpy as np
 import matplotlib
@@ -11,10 +10,12 @@
 
 from tools import *
 
-def show(block = True):
+
+def show(block=True):
     plt.show(block)
 
-def save(fig, legend, fileName, fileFormat = 'pdf', overwrite = False):
+
+def save(fig, legend, fileName, fileFormat='pdf', overwrite=False):
     fullFileName = fileName + '.' + fileFormat if not fileName.endswith(fileFormat) else fileName
     if not overwrite and os.path.exists(fullFileName):
         warn("Output file %s exists already! Not overwriting!" % fullFileName)
@@ -24,19 +25,19 @@ def save(fig, legend, fileName, fileFormat = 'pdf', overwrite = False):
     extra = ()
     if legend:
         extra = (legend,)
-        
+
     fig.savefig(fullFileName, bbox_inches='tight', format=fileFormat)
 
 
-def plotMultiDelays(x, delays, xLabel,  labels = None, title = None, markersize = 1.5, fileName = None, overwrite = None, colors = None, block = False, show = False):
+def plotMultiDelays(x, delays, xLabel, labels=None, title=None, markersize=1.5, fileName=None, overwrite=None, colors=None, block=False, show=False):
     if not colors:
         defaultColors = ['r', 'g', 'b']
         colors = list(reversed(defaultColors[:len(delays)]))
-    
+
     yLabel = "offset [msec]"
-        
+
     import matplotlib.gridspec as gridspec
-    
+
     figsize = (5, 2.5)
 
     breakX = False
@@ -47,28 +48,27 @@ def plotMyDelays(ax):
         if labels:
             for i, (d, l) in enumerate(zip(delays, labels)):
                 assert(len(xA) == len(d))
-                ax.plot(xA, np.asarray(d) * 1000, linestyle='-', marker='.', markersize= markersize, label = l, color = colors[i])
+                ax.plot(xA, np.asarray(d) * 1000, linestyle='-', marker='.', markersize=markersize, label=l, color=colors[i])
         else:
             for i, d in enumerate(delays):
                 assert(len(xA) == len(d))
-                ax.plot(xA, np.asarray(d) * 1000, linestyle='-', marker='.', markersize= markersize, color = colors[i])
+                ax.plot(xA, np.asarray(d) * 1000, linestyle='-', marker='.', markersize=markersize, color=colors[i])
 
-    fig, ax = plt.subplots(1, 1, figsize= figsize)
+    fig, ax = plt.subplots(1, 1, figsize=figsize)
     plotMyDelays(ax)
     ax.set_xlabel(xLabel)
     ax.set_ylabel(yLabel)
 
     if title:
         ax.set_title(title)
-    
+
     legend = None
     if labels:
         legend = ax.legend(loc='upper right', shadow=False)
 
-    
     if fileName:
-        save(fig, legend, fileName, overwrite = overwrite)
-        
+        save(fig, legend, fileName, overwrite=overwrite)
+
     if show:
       show(block)
     return fig