From e548d9d8fcad75ccfc17ac04298544c3aa397c96 Mon Sep 17 00:00:00 2001
From: pepms <jmarti@iri.upc.edu>
Date: Thu, 10 Apr 2025 17:41:15 +0200
Subject: [PATCH] sensor enable/disable
---
include/core/sensor/sensor_base.h | 23 +++++++++++++++++++++++
schema/sensor/SensorBase.schema | 8 +++++++-
src/sensor/sensor_base.cpp | 14 ++++++++++++++
test/gtest_factor_base.cpp | 8 ++++++--
4 files changed, 50 insertions(+), 3 deletions(-)
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index a9b60dce5..6333d3a66 100644
--- a/include/core/sensor/sensor_base.h
+++ b/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
diff --git a/schema/sensor/SensorBase.schema b/schema/sensor/SensorBase.schema
index e9aa53fb7..dbef1627a 100644
--- a/schema/sensor/SensorBase.schema
+++ b/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.
\ No newline at end of file
+ _doc: The sensor's name. It has to be unique.
+
+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
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index d49a2ef24..8c67c9119 100644
--- a/src/sensor/sensor_base.cpp
+++ b/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);
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 1d90a4fe5..b8647f6a7 100644
--- a/test/gtest_factor_base.cpp
+++ b/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()}},
--
GitLab