Skip to content
Snippets Groups Projects
Commit 137b508d authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

minor: more verbose warnings and errors

parent 19219298
No related branches found
No related tags found
No related merge requests found
...@@ -166,7 +166,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr) ...@@ -166,7 +166,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
// Warning if adding an already added // Warning if adding an already added
if (factors_.count(fac_ptr) != 0) 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; return;
} }
...@@ -206,7 +206,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr) ...@@ -206,7 +206,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
// Warning if removing a missing factor // Warning if removing a missing factor
if (factors_.count(fac_ptr) == 0) 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; return;
} }
...@@ -230,7 +230,7 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr) ...@@ -230,7 +230,7 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
// Warning if adding an already added state block // Warning if adding an already added state block
if (state_blocks_.count(state_ptr) != 0) 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; return;
} }
...@@ -256,7 +256,7 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr) ...@@ -256,7 +256,7 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
// Warning if removing a missing state block // Warning if removing a missing state block
if (state_blocks_.count(state_ptr) == 0) 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; return;
} }
...@@ -323,8 +323,10 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state ...@@ -323,8 +323,10 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state
auto it = state_blocks_.find(state_ptr); auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end()) 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 !"); throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
}
return it->second; return it->second;
} }
...@@ -333,8 +335,10 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state ...@@ -333,8 +335,10 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state
auto it = state_blocks_.find(state_ptr); auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end()) 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 !"); throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
}
return it->second.data(); return it->second.data();
} }
...@@ -343,8 +347,10 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) ...@@ -343,8 +347,10 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
auto it = state_blocks_.find(state_ptr); auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end()) 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 !"); throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
}
return it->second.data(); return it->second.data();
} }
...@@ -365,7 +371,7 @@ bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const ...@@ -365,7 +371,7 @@ bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_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) bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment