From 954432241d31b8d2a2412be1187f568efe10bebd Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 21 Jan 2025 17:25:42 +0100
Subject: [PATCH] [skip ci] wip
---
include/core/processor/motion_provider.h | 25 +++++++++++++-----------
src/problem/problem.cpp | 18 ++++++++++++-----
src/processor/motion_provider.cpp | 4 ++--
src/processor/processor_base.cpp | 5 +++++
test/gtest_motion_provider.cpp | 6 +++---
5 files changed, 37 insertions(+), 21 deletions(-)
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index 4b2ddb5f2..3a2eb71de 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -45,7 +45,7 @@ class MotionProvider
VectorComposite getOdometry() const;
void setOdometry(const VectorComposite&);
- bool isStateGetter() const;
+ bool isStateProvider() const;
int getOrder() const;
void setOrder(int);
@@ -66,7 +66,8 @@ class MotionProvider
protected:
TypeComposite state_types_;
- YAML::Node params_motion_provider_;
+ bool is_state_provider_;
+ double state_provider_order_;
private:
VectorComposite odometry_;
@@ -74,7 +75,9 @@ class MotionProvider
};
inline MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params)
- : state_types_(_state_types), params_motion_provider_(Clone(_params))
+ : state_types_(_state_types),
+ is_state_provider_(_params["state_provider"].as<bool>()),
+ state_provider_order_(_params["state_provider_order"].as<double>())
{
checkTypeComposite(_state_types);
}
@@ -86,23 +89,23 @@ namespace wolf
{
inline MotionProvider::~MotionProvider() {}
-inline bool MotionProvider::isStateGetter() const
+inline bool MotionProvider::isStateProvider() const
{
- return params_motion_provider_["state_provider"].as<bool>();
+ return is_state_provider_;
}
inline int MotionProvider::getOrder() const
{
- if (not isStateGetter())
- throw std::runtime_error("MotionProvider::getOrder: motion provider not state getter, doesn't have order");
- return params_motion_provider_["state_provider_order"].as<int>();
+ if (not isStateProvider())
+ throw std::runtime_error("MotionProvider::getOrder: not a motion provider, doesn't have order");
+ return state_provider_order_;
}
inline void MotionProvider::setOrder(int _order)
{
- if (not isStateGetter())
- throw std::runtime_error("MotionProvider::getOrder: motion provider not state getter, cannot have order");
- params_motion_provider_["state_provider_order"] = _order;
+ if (not isStateProvider())
+ throw std::runtime_error("MotionProvider::getOrder: not a motion provider, cannot have order");
+ state_provider_order_ = _order;
}
} /* namespace wolf */
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6d4bbe712..6dd69ff8e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -163,17 +163,18 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node,
}
// SENSORS ===============================================================================
- // load plugin if sensor is not registered
WOLF_DEBUG("Installing sensors...");
for (auto sensor_n : _param_node["sensors"]) problem->installSensor(sensor_n);
// PROCESSORS ===============================================================================
- // load plugin if processor is not registered
WOLF_DEBUG("Installing processors...");
- for (auto processor_n : _param_node["processors"]) problem->installProcessor(processor_n);
+ for (auto processor_n : _param_node["processors"])
+ {
+ WOLF_DEBUG("Installing processor with the following YAML::Node:\n", processor_n);
+ problem->installProcessor(processor_n);
+ }
// MAP (optional) ===============================================================================
- // Default MapBase
WOLF_DEBUG("Creating map...");
// load plugin if map is not registered
if (not FactoryMapNode::isCreatorRegistered(_param_node["map"]["type"].as<std::string>()))
@@ -313,16 +314,21 @@ ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node, st
}
// Create processor (with derived class validation)
ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(processor_type, _processor_node, _schema_folders);
+ WOLF_DEBUG("Processor created: ", prc_ptr->getName());
if (not prc_ptr) throw std::runtime_error("Processor could not be created.");
// Dimension check
+ WOLF_DEBUG("Checking dimension compatible: ", prc_ptr->getName());
if (prc_ptr->getDimCompatible() != 0 and prc_ptr->getDimCompatible() != this->getDim())
throw std::runtime_error("Processor not compatible with the Problem dimension.");
// Link
+ WOLF_DEBUG("Configuring: ", prc_ptr->getName());
prc_ptr->configure(sen_ptr);
+ WOLF_DEBUG("linking: ", prc_ptr->getName(), " with ", sen_ptr->getName());
prc_ptr->link(sen_ptr);
+ WOLF_DEBUG("Processor installed correctly - ", prc_ptr->getName());
return prc_ptr;
}
@@ -374,6 +380,7 @@ ProcessorBasePtr Problem::installProcessor(SensorBasePtr _sensor_corr
prc_ptr->configure(_sensor_corresponding);
prc_ptr->link(_sensor_corresponding);
+ WOLF_DEBUG("Processor installed: ", prc_ptr->getName());
return prc_ptr;
}
@@ -771,8 +778,9 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
{
+ WOLF_DEBUG("Adding MotionProvider processor with priority = ", _motion_provider_ptr->getOrder());
// Check if is state getter
- if (not _motion_provider_ptr->isStateGetter())
+ if (not _motion_provider_ptr->isStateProvider())
{
WOLF_WARN("adding a MotionProvider processor with state_provider=false. Not adding this processor");
return;
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index c943858c9..4a11336ee 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -25,9 +25,9 @@ using namespace wolf;
void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr)
{
- WOLF_DEBUG_COND(not isStateGetter(),
+ WOLF_DEBUG_COND(not isStateProvider(),
"MotionProvider processor with 'state_provider'=false. Not adding this processor");
- if (isStateGetter())
+ if (isStateProvider())
{
_prb_ptr->addMotionProvider(_motion_ptr);
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 10b3e1a58..b519c7f61 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -146,14 +146,19 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
if (_sen_ptr)
{
+ WOLF_DEBUG("adding processor to sensor");
_sen_ptr->addProcessor(shared_from_this());
+ WOLF_DEBUG("set sensor to processor");
this->setSensor(_sen_ptr);
+ WOLF_DEBUG("set problem to processor");
this->setProblem(_sen_ptr->getProblem());
+ WOLF_DEBUG("done!");
}
else
{
WOLF_WARN("Linking Processor ", id(), " to a nullptr");
}
+ WOLF_DEBUG("done!!");
}
void ProcessorBase::setProblem(ProblemPtr _problem)
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index cc6aa5361..abf74cb9f 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -93,9 +93,9 @@ TEST_F(MotionProviderTest, install)
ASSERT_TRUE(mp3 != nullptr);
// well configured
- ASSERT_FALSE(mp1->isStateGetter());
- ASSERT_TRUE(mp2->isStateGetter());
- ASSERT_TRUE(mp3->isStateGetter());
+ ASSERT_FALSE(mp1->isStateProvider());
+ ASSERT_TRUE(mp2->isStateProvider());
+ ASSERT_TRUE(mp3->isStateProvider());
ASSERT_EQ(mp2->getOrder(), 1);
ASSERT_EQ(mp3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
ASSERT_TRUE(mp1->getStateTypes().has('P'));
--
GitLab