From 137b508d1f35b7c1c740902e34a003f71c2c73eb Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 28 Sep 2020 17:59:13 +0200
Subject: [PATCH] minor: more verbose warnings and errors
---
src/solver/solver_manager.cpp | 22 ++++++++++++++--------
1 file changed, 14 insertions(+), 8 deletions(-)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 8fff62bb4..123e69ef1 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -166,7 +166,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
// Warning if adding an already added
if (factors_.count(fac_ptr) != 0)
{
- WOLF_WARN("Tried to add a factor that was already added !");
+ WOLF_WARN("Tried to add the factor ", fac_ptr->id(), " that was already added !");
return;
}
@@ -206,7 +206,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
// Warning if removing a missing factor
if (factors_.count(fac_ptr) == 0)
{
- WOLF_WARN("Tried to remove a factor that is missing !");
+ WOLF_WARN("Tried to remove factor ", fac_ptr->id(), " that is missing !");
return;
}
@@ -230,7 +230,7 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
// Warning if adding an already added state block
if (state_blocks_.count(state_ptr) != 0)
{
- WOLF_WARN("Tried to add a StateBlock that was already added !");
+ WOLF_WARN("Tried to add athe StateBloc ", state_ptr, " that was already added !");
return;
}
@@ -256,7 +256,7 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
// Warning if removing a missing state block
if (state_blocks_.count(state_ptr) == 0)
{
- WOLF_WARN("Tried to remove a StateBlock that was not added !");
+ WOLF_WARN("Tried to remove the StateBlock ", state_ptr, " that was not added !");
return;
}
@@ -323,8 +323,10 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
+ {
+ WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+ }
return it->second;
}
@@ -333,8 +335,10 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
+ {
+ WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+ }
return it->second.data();
}
@@ -343,8 +347,10 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
+ {
+ WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+ }
return it->second.data();
}
@@ -365,7 +371,7 @@ bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
- return factors_.count(fac_ptr) and isFactorRegisteredDerived(fac_ptr);
+ return factors_.count(fac_ptr) == 1 and isFactorRegisteredDerived(fac_ptr);
}
bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
--
GitLab