diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 5570e0b9390424162c5fa5b4de070abbd18d508f..8e1c44860a8bb08e14f0e5488d3429e751050dc4 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -68,11 +68,11 @@ class Problem : public std::enable_shared_from_this<Problem>
friend MotionProvider;
protected:
- HardwareBasePtr hardware_ptr_;
- TrajectoryBasePtr trajectory_ptr_;
- MapBasePtr map_ptr_;
- TreeManagerBasePtr tree_manager_;
- std::map<int, MotionProviderPtr> motion_provider_map_;
+ HardwareBasePtr hardware_ptr_;
+ TrajectoryBasePtr trajectory_ptr_;
+ MapBasePtr map_ptr_;
+ TreeManagerBasePtr tree_manager_;
+ std::map<unsigned int, MotionProviderPtr> motion_provider_map_;
SizeEigen dim_;
StateKeys frame_keys_;
@@ -199,8 +199,8 @@ class Problem : public std::enable_shared_from_this<Problem>
void removeMotionProvider(MotionProviderPtr proc);
public:
- std::map<int, MotionProviderConstPtr> getMotionProviderMap() const;
- std::map<int, MotionProviderPtr> getMotionProviderMap();
+ std::map<unsigned int, MotionProviderConstPtr> getMotionProviderMap() const;
+ std::map<unsigned int, MotionProviderPtr> getMotionProviderMap();
// Trajectory branch ----------------------------------
TrajectoryBaseConstPtr getTrajectory() const;
@@ -430,14 +430,14 @@ inline bool Problem::isFirstFrameApplied() const
return first_frame_status_ == 2;
}
-inline std::map<int, MotionProviderConstPtr> Problem::getMotionProviderMap() const
+inline std::map<unsigned int, MotionProviderConstPtr> Problem::getMotionProviderMap() const
{
- std::map<int, MotionProviderConstPtr> map_const;
+ std::map<unsigned int, MotionProviderConstPtr> map_const;
for (auto&& pair : motion_provider_map_) map_const[pair.first] = pair.second;
return map_const;
}
-inline std::map<int, MotionProviderPtr> Problem::getMotionProviderMap()
+inline std::map<unsigned int, MotionProviderPtr> Problem::getMotionProviderMap()
{
return motion_provider_map_;
}
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index 3a2eb71de4eaeae9228c0e40b86fc045a5fd5314..ca19d80baa2b501204a48534053d3fe0fa268bb3 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -45,9 +45,9 @@ class MotionProvider
VectorComposite getOdometry() const;
void setOdometry(const VectorComposite&);
- bool isStateProvider() const;
- int getOrder() const;
- void setOrder(int);
+ bool isStateProvider() const;
+ unsigned int getOrder() const;
+ void setOrder(unsigned int);
public:
TypeComposite getStateTypes() const
@@ -67,21 +67,13 @@ class MotionProvider
protected:
TypeComposite state_types_;
bool is_state_provider_;
- double state_provider_order_;
+ unsigned int state_provider_order_;
private:
VectorComposite odometry_;
mutable std::mutex mut_odometry_;
};
-inline MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::Node& _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);
-}
-
} // namespace wolf
///// IMPLEMENTATION ///////
@@ -94,14 +86,14 @@ inline bool MotionProvider::isStateProvider() const
return is_state_provider_;
}
-inline int MotionProvider::getOrder() const
+inline unsigned int MotionProvider::getOrder() const
{
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)
+inline void MotionProvider::setOrder(unsigned int _order)
{
if (not isStateProvider())
throw std::runtime_error("MotionProvider::getOrder: not a motion provider, cannot have order");
diff --git a/schema/processor/MotionProvider.schema b/schema/processor/MotionProvider.schema
index 3881ba7120d2c2f21d4ccf002e8cdf5ef38342a4..95313cec713c5fb6fcae1c3b4314ec4643e91b5e 100644
--- a/schema/processor/MotionProvider.schema
+++ b/schema/processor/MotionProvider.schema
@@ -5,5 +5,5 @@ state_provider:
state_provider_order:
_mandatory: $state_provider
- _type: double
- _doc: The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased).
\ No newline at end of file
+ _type: unsigned int
+ _doc: The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same order number.
\ No newline at end of file
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6dd69ff8e66ef9166f152513750f79c65391faa1..eab9e22109eee86b69e17894d08b23d490dd18f3 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -168,13 +168,9 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node,
// PROCESSORS ===============================================================================
WOLF_DEBUG("Installing processors...");
- for (auto processor_n : _param_node["processors"])
- {
- WOLF_DEBUG("Installing processor with the following YAML::Node:\n", processor_n);
- problem->installProcessor(processor_n);
- }
+ for (auto processor_n : _param_node["processors"]) problem->installProcessor(processor_n);
- // MAP (optional) ===============================================================================
+ // MAP ===============================================================================
WOLF_DEBUG("Creating map...");
// load plugin if map is not registered
if (not FactoryMapNode::isCreatorRegistered(_param_node["map"]["type"].as<std::string>()))
@@ -230,6 +226,9 @@ SensorBasePtr Problem::installSensor(const YAML::Node& _sensor_node, std::vector
// Link
sen_ptr->link(getHardware());
+
+ WOLF_DEBUG("Sensor installed correctly - ", sen_ptr->getName());
+
return sen_ptr;
}
@@ -272,6 +271,8 @@ SensorBasePtr Problem::installSensor(const std::string& _params_yaml_filen
// Link
sen_ptr->link(getHardware());
+ WOLF_DEBUG("Sensor installed correctly - ", sen_ptr->getName());
+
return sen_ptr;
}
@@ -314,20 +315,17 @@ 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;
@@ -380,7 +378,7 @@ ProcessorBasePtr Problem::installProcessor(SensorBasePtr _sensor_corr
prc_ptr->configure(_sensor_corresponding);
prc_ptr->link(_sensor_corresponding);
- WOLF_DEBUG("Processor installed: ", prc_ptr->getName());
+ WOLF_DEBUG("Processor installed correctly - ", prc_ptr->getName());
return prc_ptr;
}
@@ -782,18 +780,19 @@ void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
// Check if is state getter
if (not _motion_provider_ptr->isStateProvider())
{
- WOLF_WARN("adding a MotionProvider processor with state_provider=false. Not adding this processor");
+ WOLF_DEBUG(
+ "adding a MotionProvider processor with state_provider=false. Not adding this processor as state provider "
+ "in Problem.");
return;
}
// check duplicated priority
- while (motion_provider_map_.count(_motion_provider_ptr->getOrder()) == 1)
+ if (motion_provider_map_.count(_motion_provider_ptr->getOrder()) == 1)
{
WOLF_ERROR("Adding a MotionProvider processor with priority = ",
_motion_provider_ptr->getOrder(),
- " which is already taken. Trying to add it with priority = ",
- _motion_provider_ptr->getOrder() + 1);
- _motion_provider_ptr->setOrder(_motion_provider_ptr->getOrder() + 1);
+ " which is already taken.");
+ throw std::runtime_error("Problem::addMotionProvider: duplicated priority");
}
// add to map ordered by priority
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index 4a11336ee5f775393bd827c6d3d96f1514a44253..d8d9d5e0af92ac03bd04125db65a8899d1c427d5 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -23,6 +23,14 @@
using namespace wolf;
+MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::Node& _params)
+ : state_types_(_state_types),
+ is_state_provider_(_params["state_provider"].as<bool>()),
+ state_provider_order_(_params["state_provider_order"] ? _params["state_provider_order"].as<unsigned int>() : 0)
+{
+ checkTypeComposite(_state_types);
+}
+
void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr)
{
WOLF_DEBUG_COND(not isStateProvider(),
@@ -37,7 +45,8 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion
void MotionProvider::setOdometry(const VectorComposite& _odom)
{
std::lock_guard<std::mutex> lock(mut_odometry_);
- assert(_odom.has(getStateKeys()) && "MotionProvider::setOdometry(): any key missing in _odom.");
+ if (not _odom.has(getStateKeys()))
+ throw std::runtime_error("MotionProvider::setOdometry(): any key missing in _odom.");
for (auto key : getStateKeys()) odometry_[key] = _odom.at(key); // overwrite/insert only keys of the state_types_
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index b519c7f61d47b6bf938fb01620f2b59877044092..1a58306f42938ead927a976ee61de407c2cd0f6b 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -146,13 +146,9 @@ 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
{
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 1ace7b2c2ce4d2606b5646bbf934bb6fcd378761..853edc8780633951d7ab05c2dd5fc8bb8b3e6f1f 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -104,8 +104,8 @@ TEST(Emplace, Processor)
ASSERT_EQ(P->getHardware(), S->getHardware());
ASSERT_EQ(P->getHardware()->getSensorList().front(), S);
- auto prc = ProcessorBase::emplace<ProcessorOdom2d>(
- S, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
+ auto params_proc = YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml");
+ auto prc = ProcessorBase::emplace<ProcessorOdom2d>(S, params_proc, {wolf_dir});
ASSERT_EQ(P, prc->getProblem());
ASSERT_EQ(S, prc->getSensor());
@@ -117,8 +117,10 @@ TEST(Emplace, Processor)
SensorBasePtr sen2 = SensorBase::emplace<SensorDummyPo2d>(P->getHardware(), params2, {wolf_dir});
- ProcessorOdom2dPtr prc2 = ProcessorBase::emplace<ProcessorOdom2d>(
- sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
+ auto params_proc2 = params_proc;
+ params_proc["state_provider_order"] = 2;
+
+ ProcessorOdom2dPtr prc2 = ProcessorBase::emplace<ProcessorOdom2d>(sen2, params_proc2, {wolf_dir});
ASSERT_EQ(P, prc->getProblem());
ASSERT_EQ(sen2, prc2->getSensor());
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index abf74cb9f7c2bb23f75684fabd7df5d510ad4dd9..bb4cc27a2609df4180e8e74922c586ddd9cba6dd 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -97,7 +97,7 @@ TEST_F(MotionProviderTest, install)
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_EQ(mp3->getOrder(), 2);
ASSERT_TRUE(mp1->getStateTypes().has('P'));
ASSERT_TRUE(mp1->getStateTypes().has('O'));
ASSERT_FALSE(mp1->getStateTypes().has('V'));
@@ -114,13 +114,19 @@ TEST_F(MotionProviderTest, install)
ASSERT_EQ(std::next(problem->getMotionProviderMap().begin())->second, mp3);
}
+TEST_F(MotionProviderTest, duplicated_order)
+{
+ //try to install a processor with the same order --> throw
+ ASSERT_THROW(problem->installProcessor(sen, wolf_dir + "/test/yaml/processor_motion_provider_dummy2.yaml", {wolf_dir}), std::runtime_error);
+}
+
TEST_F(MotionProviderTest, odometry)
{
VectorComposite odom_p({{'P', Vector2d::Zero()}});
VectorComposite odom_pov({{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}, {'V', Vector2d::Zero()}});
// Error: required PO keys to be added
- ASSERT_DEATH({ mp1->setOdometry(odom_p); }, "");
+ ASSERT_THROW(mp1->setOdometry(odom_p), std::runtime_error);
mp1->setOdometry(odom_pov);
mp2->setOdometry(odom_pov);
mp3->setOdometry(odom_pov);
diff --git a/test/yaml/processor_motion_provider_dummy3.yaml b/test/yaml/processor_motion_provider_dummy3.yaml
index 8a769caaa4ef7ac26e5aac77a82594dba9c1c7d1..3414e1820bf47d574a1b5539588923abc9858a8f 100644
--- a/test/yaml/processor_motion_provider_dummy3.yaml
+++ b/test/yaml/processor_motion_provider_dummy3.yaml
@@ -11,4 +11,4 @@ keyframe_vote:
apply_loss_function: false
state_provider: true
-state_provider_order: 1 # same as proc2, will be relegated after it
\ No newline at end of file
+state_provider_order: 2
\ No newline at end of file