sensor enable/disable

include/core/sensor/sensor_base.h

@@ -116,6 +116,7 @@ class SensorBase : public NodeStateBlocks
     friend ProcessorBase;
 
   private:
+    bool                 enabled_;  ///< Sensor enabled
     HardwareBaseWPtr     hardware_ptr_;
     ProcessorBasePtrList processor_list_;
     static unsigned int  sensor_id_count_;      ///< Object counter (acts as simple ID factory)
@@ -199,6 +200,13 @@ class SensorBase : public NodeStateBlocks
     StateBlockPtr      getIntrinsic();
 
   public:
+
+    // enable/disable the sensor
+    bool isEnabled() const;
+    void enable();
+    void disable();
+
+    // Set the state block as fixed or unfixed
     void fixExtrinsics();
     void unfixExtrinsics();
     void fixIntrinsics();
@@ -346,4 +354,19 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
     hardware_ptr_ = _hw_ptr;
 }
 
+inline bool SensorBase::isEnabled() const
+{
+    return enabled_;
+}
+
+inline void SensorBase::enable()
+{
+    enabled_ = true;
+}
+
+inline void SensorBase::disable()
+{
+    enabled_ = false;
+}
+
 }  // namespace wolf

schema/sensor/SensorBase.schema

@@ -3,4 +3,10 @@ follow: TypeAndPlugin.schema
 name:
   _mandatory: true
   _type: string
-  _doc: The sensor's name. It has to be unique.
+  _doc: The sensor's name. It has to be unique.
\ No newline at end of file
+
+enabled:
+  _type: bool
+  _mandatory: false
+  _default: true
+  _doc: Whether the sensor is enabled or not. If not, captures will not be processed when calling process().
\ No newline at end of file

src/sensor/sensor_base.cpp

@@ -34,6 +34,7 @@ SensorBase::SensorBase(const std::string& _type, const TypeComposite& _state_typ
                       _state_types,
                       VectorComposite(_params["states"], "value", false, _state_types.getKeys()),
                       PriorComposite(_params["states"], "prior", true, _state_types.getKeys())),
+      enabled_(_params["enabled"].as<bool>()),
       hardware_ptr_(),
       sensor_id_(++sensor_id_count_),  // simple ID factory
       state_block_dynamic_(),
@@ -246,6 +247,19 @@ StateBlockPtr SensorBase::getIntrinsic()
 
 bool SensorBase::process(const CaptureBasePtr capture_ptr)
 {
+    if (not enabled_) return false;
+    if (not capture_ptr) return false;
+    if (capture_ptr->getSensor() and capture_ptr->getSensor() != shared_from_this_sensor())
+    {
+        WOLF_WARN("SensorBase::process: Capture already has a sensor configured different from this. Ignoring capture.");
+        return false;
+    }
+    if (not capture_ptr->getTimeStamp().ok())
+    {
+        WOLF_WARN("SensorBase::process: Capture has an invalid timestamp. Ignoring capture.");
+        return false;
+    }
+
     capture_ptr->setSensor(shared_from_this_sensor());
 
     for (const auto& processor : processor_list_) processor->captureCallback(capture_ptr);

test/gtest_factor_base.cpp

@@ -24,6 +24,8 @@
 using namespace wolf;
 using namespace Eigen;
 
+std::string wolf_dir = _WOLF_CODE_DIR;
+
 class FactorBaseTest : public testing::Test
 {
   public:
@@ -36,6 +38,8 @@ class FactorBaseTest : public testing::Test
     {
         YAML::Node rand_params;
         rand_params["name"]                         = "Sensor0";
+        rand_params["type"]                         = "SensorDummyPo2d";
+        rand_params["plugin"]                       = "core";
         rand_params["states"]["P"]["value"]         = Vector2d::Random();
         rand_params["states"]["P"]["prior"]["mode"] = "initial_guess";
         rand_params["states"]["P"]["dynamic"]       = false;
@@ -45,9 +49,9 @@ class FactorBaseTest : public testing::Test
         rand_params["noise_p_std"]                  = 0.1;
         rand_params["noise_o_std"]                  = 0.1;
 
-        S0                  = SensorDummyPo2d::create(rand_params, {});
+        S0                  = SensorDummyPo2d::create(rand_params, {wolf_dir});
         rand_params["name"] = "Sensor1";
-        S1                  = SensorDummyPo2d::create(rand_params, {});
+        S1                  = SensorDummyPo2d::create(rand_params, {wolf_dir});
         F0                  = std::make_shared<FrameBase>(0.0,
                                          TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
                                          VectorComposite{{'P', Vector2d::Random()}, {'O', Vector1d::Random()}},