Merge branch 'master' into fix/wrong_parameter_name_in_default

Author: HannesSommer

cuckoo_time_translator/cfg/DeviceTimeTranslator.cfg

@@ -10,12 +10,21 @@ gen.add("switch_time",          double_t, 0,
          0, 0.0, 36000)
 
 algo_enum = gen.enum([ 
-                       gen.const("ReceiveTimeOnly",       int_t, 0, "No device time. Receive time only is used."),
-                       gen.const("ConvexHull", int_t, 1, "Convex hull"),
-                       gen.const("Kalman",        int_t, 2, "Kalman filter"), 
+                       gen.const("ReceiveTimePassThrough",
+                           int_t, 0,
+                           "Receive time is passed through as translated time. Device time is ignored."),
+                       gen.const("ConvexHull",
+                           int_t, 1,
+                           "Convex hull translator"),
+                       gen.const("Kalman",
+                           int_t, 2,
+                           "Kalman filter based translator"),
+                       gen.const("DeviceTimePassThrough",
+                           int_t, 3,
+                           "Device time is passed through as translated time. Receive time is ignored."), 
                      ],
                      "Device timestamp filter algorithm")
 
-gen.add("filter_algo", int_t, 0, "The filter algorithm used for device timestamps", 0, 0, 3, edit_method=algo_enum)
+gen.add("filter_algo", int_t, 0, "The translation algorithm used for device timestamps", 0, 0, 3, edit_method=algo_enum)
 
 exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "DeviceTimeTranslator"))

cuckoo_time_translator/include/cuckoo_time_translator/DeviceTimeTranslator.h

@@ -18,9 +18,11 @@ class DeviceTimeTranslatorConfig;
 //TODO (c++11) use enum class
 struct FilterAlgorithm {
   enum Type {
-    ReceiveTimeOnly,
+    ReceiveTimeOnly = 0, // for backwards compatibility
+    ReceiveTimePassThrough = 0,
     ConvexHull,
     Kalman,
+    DeviceTimePassThrough
   } type;
 
   FilterAlgorithm(Type t) : type(t) {}

cuckoo_time_translator/include/cuckoo_time_translator/OwtFactory.h

@@ -32,7 +32,8 @@ class OwtFactoryImpl : public OwtFactory, public Config {
 
 typedef OwtFactoryImpl<FilterAlgorithm::Kalman, KalmanOwt, KalmanOwt::Config> KalmanOwtFactory;
 typedef OwtFactoryImpl<FilterAlgorithm::ConvexHull, ConvexHullOwt> ConvexHullOwtFactory;
-typedef OwtFactoryImpl<FilterAlgorithm::ReceiveTimeOnly, NopOwt> ReceiveTimeOnlyOwtFactory;
+typedef OwtFactoryImpl<FilterAlgorithm::ReceiveTimePassThrough, ReceiveTimePassThroughOwt> ReceiveTimePassThroughOwtFactory;
+typedef OwtFactoryImpl<FilterAlgorithm::DeviceTimePassThrough, DeviceTimePassThroughOwt> DeviceTimePassThroughOwtFactory;
 }
 
 #endif /* HA1414207_EF1A_4368_BD21_A56BFFA7D0BB */

cuckoo_time_translator/src/DeviceTimeTranslator.cpp

@@ -57,9 +57,10 @@ NS::NS(const char* nameSpace, bool appendDeviceTimeSubnamespace)
 class Defaults::Impl {
  public:
   Impl(){
-    regOwtFactory(new ReceiveTimeOnlyOwtFactory);
+    regOwtFactory(new ReceiveTimePassThroughOwtFactory);
     regOwtFactory(new KalmanOwtFactory);
     regOwtFactory(new ConvexHullOwtFactory);
+    regOwtFactory(new DeviceTimePassThroughOwtFactory);
   }
 
   Impl(const Impl& other){
@@ -143,8 +144,8 @@ std::unique_ptr<OneWayTranslator> Defaults::Impl::createOwt(FilterAlgorithm fa)
   if(e != owtFactoryReg_.end()){
     return e->second->createOwt();
   } else {
-    ROS_ERROR("Unknown device time filter algorithm : %u. Falling back to no filter (NopOwt).", static_cast<unsigned>(fa));
-    return std::unique_ptr<OneWayTranslator>(new NopOwt);
+    ROS_ERROR("Unknown device time filter algorithm : %u. Falling back to default translator (ReceiveTimePassThroughOwt).", static_cast<unsigned>(fa));
+    return std::unique_ptr<OneWayTranslator>(new ReceiveTimePassThroughOwt);
   }
 }
 
@@ -225,7 +226,9 @@ class DeviceTimeTranslator::Impl {
       switchingTimeSeconds_ = expectedSwitchingTimeSeconds;
       somethingWasUpdated = true;
       timeTranslator_ = defaults_.createOwt(expectedAlgo_);
-      if(dynamic_cast<NopOwt*>(timeTranslator_.get()) == nullptr && shouldSwitch(expectedSwitchingTimeSeconds)){
+      if(dynamic_cast<ReceiveTimePassThroughOwtFactory*>(timeTranslator_.get()) == nullptr
+          && dynamic_cast<DeviceTimePassThroughOwtFactory*>(timeTranslator_.get()) == nullptr
+          && shouldSwitch(expectedSwitchingTimeSeconds)){
         timeTranslator_ = std::unique_ptr<OneWayTranslator>(new SwitchingOwt(expectedSwitchingTimeSeconds, [this]() { return defaults_.createOwt(expectedAlgo_); }));
       }
       switchingTimeSeconds_ = expectedSwitchingTimeSeconds;
@@ -252,7 +255,8 @@ class DeviceTimeTranslator::Impl {
   }
 
   std::unique_ptr<OneWayTranslator> timeTranslator_;
-  FilterAlgorithm currentAlgo_ = FilterAlgorithm::ReceiveTimeOnly, expectedAlgo_ = FilterAlgorithm::ReceiveTimeOnly;
+  FilterAlgorithm currentAlgo_ = FilterAlgorithm::ReceiveTimePassThrough;
+  FilterAlgorithm expectedAlgo_ = FilterAlgorithm::ReceiveTimePassThrough;
   Defaults defaults_;
   ros::Publisher deviceTimePub_;
   ros::NodeHandle nh_;
@@ -282,8 +286,11 @@ DeviceTimeTranslator::DeviceTimeTranslator(const NS & nameSpace, const Defaults
   pImpl_->getDeviceTimePub() = pImpl_->getNh().advertise<DeviceTimestamp>("", 5);
   pImpl_->getConfigSrv().setCallback(boost::bind(&DeviceTimeTranslator::configCallback, this, _1, _2));
 
-  if(pImpl_->getExpectedAlgo() == FilterAlgorithm::ReceiveTimeOnly){
-    ROS_WARN("Current %s/filterAlgo setting (=%u ~ ReceiveTimeOnly) causes the sensor's hardware clock to be ignore. Instead the receive time in the driver is used as timestamp.", nameSpace.toString().c_str(), unsigned(FilterAlgorithm::ReceiveTimeOnly));
+  if(pImpl_->getExpectedAlgo() == FilterAlgorithm::ReceiveTimePassThrough){
+    ROS_WARN("Current %s/filterAlgo setting (=%u ~ ReceiveTimePassThrough) causes the sensor's hardware clock to be ignore. Instead the receive time in the driver is used as translated timestamp.", nameSpace.toString().c_str(), unsigned(FilterAlgorithm::ReceiveTimePassThrough));
+  }
+  if(pImpl_->getExpectedAlgo() == FilterAlgorithm::DeviceTimePassThrough){
+    ROS_WARN("Current %s/filterAlgo setting (=%u ~ DeviceTimePassThrough) causes the receive time to be ignore. Instead the device time is passed through as translated timestamp.", nameSpace.toString().c_str(), unsigned(FilterAlgorithm::DeviceTimePassThrough));
   }
 }
 

cuckoo_time_translator/src/OwtFactory.cpp

@@ -49,6 +49,7 @@ Defaults & Defaults::setFilterConfig(const KalmanOwtConfig& c) {
 }
 
 template class OwtFactoryImpl<FilterAlgorithm::ConvexHull, ConvexHullOwt>;
-template class OwtFactoryImpl<FilterAlgorithm::ReceiveTimeOnly, NopOwt>;
+template class OwtFactoryImpl<FilterAlgorithm::ReceiveTimePassThrough, ReceiveTimePassThroughOwt>;
+template class OwtFactoryImpl<FilterAlgorithm::DeviceTimePassThrough, DeviceTimePassThroughOwt>;
 
 }

cuckoo_time_translator/test/TestDeviceTimeTranslator.cpp

@@ -30,7 +30,7 @@ TEST(DeviceTimeTranslator, DefaultDeviceTimeUnwrapperAndTranslatorWithTransmitTi
 
 TEST(DeviceTimeTranslator, UnwrappedDeviceTimeTranslatorNoTranslation) {
   UnwrappedDeviceTimeTranslator translator({100.0}, "");
-  translator.setFilterAlgorithm(FilterAlgorithm::ReceiveTimeOnly);
+  translator.setFilterAlgorithm(FilterAlgorithm::ReceiveTimePassThrough);
   EXPECT_EQ(10.0, translator.update(100ul, ros::Time(10.0)).toSec());
   EXPECT_EQ(110.0, translator.update(1000ul, ros::Time(110.0)).toSec());
   EXPECT_EQ(120.0, translator.update(0ul, ros::Time(120.0)).toSec());

cuckoo_time_translator/test/TestDeviceTimeTranslatorNode.cpp

@@ -77,10 +77,10 @@ TEST(DeviceTimeTranslator, ReceiveStamps) {
   for(auto & m : msgs){
     owt.updateAndTranslateToLocalTimestamp(RemoteTime(m.timePair.remote / MockCuckooDeviceDriver::kFreq), m.timePair.local);
   }
-  EXPECT_NEAR(expectedOffset.toSec(), owt.getOffset(), 0.5);
+  EXPECT_NEAR(expectedOffset.toSec(), owt.getOffset(), 0.8);
   EXPECT_NEAR(MockCuckooDeviceDriver::kSkew, owt.getSkew(), 0.01);
 
-  EXPECT_EQ(FilterAlgorithm::ReceiveTimeOnly, msgs.back().filterAlgo.type);
+  EXPECT_EQ(FilterAlgorithm::ReceiveTimePassThrough, msgs.back().filterAlgo.type);
 
   clear();
   ASSERT_EQ(msgs.size(), 0);

cuckoo_time_translator_algorithms/include/cuckoo_time_translator/OneWayTranslator.h

@@ -41,13 +41,34 @@ class OneWayTranslator {
   virtual OneWayTranslator* cloneImpl() const = 0;
 };
 
+
+/**
+ * The receive time pass through One Way Translator does ignore the device time and yields the receive time.
+ * This implies that translateToLocalTimestamp is not implemented (throws) because no receive time is available.
+ */
+class ReceiveTimePassThroughOwt : public OneWayTranslator {
+ public:
+  virtual ~ReceiveTimePassThroughOwt();
+  virtual LocalTime translateToLocalTimestamp(RemoteTime remoteTimeTics) const override;
+  virtual LocalTime updateAndTranslateToLocalTimestamp(RemoteTime remoteTimeTics, LocalTime localTimeSecs) override;
+  virtual bool isReadyToTranslate() const override;
+  virtual void reset() override;
+
+  virtual void printNameAndConfig(std::ostream & o) const override;
+  virtual void printState(std::ostream & o) const override;
+ protected:
+  virtual ReceiveTimePassThroughOwt* cloneImpl() const override;
+};
+
+//TODO cleanup: remove all traces of NopOwt.
+typedef ReceiveTimePassThroughOwt NopOwt; // For backwards compatibility
+
 /**
- * The No Operation One Way Translator does no translation at all.
- * I.e. it passes the receive time through.
+ * The device time pass through One Way Translator does ignore the receive time and yields the device time.
  */
-class NopOwt : public OneWayTranslator {
+class DeviceTimePassThroughOwt : public OneWayTranslator {
  public:
-  virtual ~NopOwt();
+  virtual ~DeviceTimePassThroughOwt();
   virtual LocalTime translateToLocalTimestamp(RemoteTime remoteTimeTics) const override;
   virtual LocalTime updateAndTranslateToLocalTimestamp(RemoteTime remoteTimeTics, LocalTime localTimeSecs) override;
   virtual bool isReadyToTranslate() const override;
@@ -56,9 +77,10 @@ class NopOwt : public OneWayTranslator {
   virtual void printNameAndConfig(std::ostream & o) const override;
   virtual void printState(std::ostream & o) const override;
  protected:
-  virtual NopOwt* cloneImpl() const override;
+  virtual DeviceTimePassThroughOwt* cloneImpl() const override;
 };
 
+
 } /* namespace cuckoo_time_translator */
 
 #endif /* H3A613172_9ECF_45BC_8D47_C711DE333A50 */

cuckoo_time_translator_algorithms/src/OneWayTranslator.cpp

@@ -18,34 +18,64 @@ std::unique_ptr<OneWayTranslator> OneWayTranslator::clone() const {
   return std::unique_ptr<OneWayTranslator>(cloneImpl());
 }
 
-NopOwt::~NopOwt() {
+ReceiveTimePassThroughOwt::~ReceiveTimePassThroughOwt() {
 }
 
-LocalTime NopOwt::translateToLocalTimestamp(RemoteTime /*remoteTimeTics*/) const {
-  AASSERT(false, "translateToLocalTimestamp is not implemented in NopOwt!");
+LocalTime ReceiveTimePassThroughOwt::translateToLocalTimestamp(RemoteTime /*remoteTimeTics*/) const {
+  AASSERT(false, "translateToLocalTimestamp is not implemented in ReceiveTimePassThroughOwt!");
   throw ""; // dummy;
 }
 
-LocalTime NopOwt::updateAndTranslateToLocalTimestamp(RemoteTime /*remoteTimeTics*/, LocalTime localTimeSecs) {
+LocalTime ReceiveTimePassThroughOwt::updateAndTranslateToLocalTimestamp(RemoteTime /*remoteTimeTics*/, LocalTime localTimeSecs) {
   return localTimeSecs;
 }
 
-bool NopOwt::isReadyToTranslate() const {
+bool ReceiveTimePassThroughOwt::isReadyToTranslate() const {
   return true;
 }
 
-void NopOwt::printNameAndConfig(std::ostream& o) const {
-  o << "NopOwt()";
+void ReceiveTimePassThroughOwt::printNameAndConfig(std::ostream& o) const {
+  o << "ReceiveTimePassThroughOwt()";
 }
 
-void NopOwt::printState(std::ostream& /*o*/) const {
+void ReceiveTimePassThroughOwt::printState(std::ostream& /*o*/) const {
 }
 
-void NopOwt::reset() {
+void ReceiveTimePassThroughOwt::reset() {
 }
 
-NopOwt* NopOwt::cloneImpl() const {
-  return new NopOwt();
+ReceiveTimePassThroughOwt* ReceiveTimePassThroughOwt::cloneImpl() const {
+  return new ReceiveTimePassThroughOwt();
 }
 
+DeviceTimePassThroughOwt::~DeviceTimePassThroughOwt() {
+}
+
+LocalTime DeviceTimePassThroughOwt::translateToLocalTimestamp(RemoteTime remoteTimeTics) const {
+  return LocalTime(static_cast<double>(remoteTimeTics));
+}
+
+LocalTime DeviceTimePassThroughOwt::updateAndTranslateToLocalTimestamp(RemoteTime /*remoteTimeTics*/, LocalTime localTimeSecs) {
+  return localTimeSecs;
+}
+
+bool DeviceTimePassThroughOwt::isReadyToTranslate() const {
+  return true;
+}
+
+void DeviceTimePassThroughOwt::printNameAndConfig(std::ostream& o) const {
+  o << "DeviceTimePassThroughOwt()";
+}
+
+void DeviceTimePassThroughOwt::printState(std::ostream& /*o*/) const {
+}
+
+void DeviceTimePassThroughOwt::reset() {
+}
+
+DeviceTimePassThroughOwt* DeviceTimePassThroughOwt::cloneImpl() const {
+  return new DeviceTimePassThroughOwt();
+}
+
+
 } /* namespace cuckoo_time_translator */

cuckoo_time_translator_python/scripts/ctt_introspect.py

@@ -69,7 +69,7 @@
 
     if baselineOwt:
       base_times = np.array(baselineOwt.apply(ds.raw_hw_times, ds.receive_times))
-      info("Baseline owt after translation: " + baselineOwt.getConfigAndStateString())
+      info("Baseline OWT after translation: " + baselineOwt.getConfigAndStateString())
 
     delaysToPlot = []
     labels = []