Skip to content
Snippets Groups Projects
Commit 23e2b7b7 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Improve check() [Cherry picked]

Cherry picked from adapting-to-multiple-processor-motion-285
parent 1261502a
No related branches found
No related tags found
1 merge request!340Resolve "Problem::check() more exhaustive"
...@@ -1694,7 +1694,7 @@ bool Problem::check(int verbose_level) const ...@@ -1694,7 +1694,7 @@ bool Problem::check(int verbose_level) const
if ( Fo ) // case FRAME: if ( Fo ) // case FRAME:
{ {
if (verbose_level > 0) if (verbose_level > 0)
cout << " --> Frm" << Fo->id() << " <- "; cout << " ( --> Frm" << Fo->id() << " <- ";
bool found = false; bool found = false;
for (auto cby : Fo->getConstrainedByList()) for (auto cby : Fo->getConstrainedByList())
{ {
...@@ -1703,7 +1703,7 @@ bool Problem::check(int verbose_level) const ...@@ -1703,7 +1703,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby); found = found || (c == cby);
} }
if (verbose_level > 0) if (verbose_level > 0)
cout << endl; cout << ")";
// check constrained_by pointer in constrained frame // check constrained_by pointer in constrained frame
// is_consistent = is_consistent && found; // is_consistent = is_consistent && found;
...@@ -1719,7 +1719,7 @@ bool Problem::check(int verbose_level) const ...@@ -1719,7 +1719,7 @@ bool Problem::check(int verbose_level) const
if ( Co ) // case CAPTURE: if ( Co ) // case CAPTURE:
{ {
if (verbose_level > 0) if (verbose_level > 0)
cout << " --> Cap" << Co->id() << " <- "; cout << " ( --> Cap" << Co->id() << " <- ";
bool found = false; bool found = false;
for (auto cby : Co->getConstrainedByList()) for (auto cby : Co->getConstrainedByList())
{ {
...@@ -1728,7 +1728,7 @@ bool Problem::check(int verbose_level) const ...@@ -1728,7 +1728,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby); found = found || (c == cby);
} }
if (verbose_level > 0) if (verbose_level > 0)
cout << endl; cout << ")";
// check constrained_by pointer in constrained frame // check constrained_by pointer in constrained frame
// is_consistent = is_consistent && found; // is_consistent = is_consistent && found;
...@@ -1744,7 +1744,7 @@ bool Problem::check(int verbose_level) const ...@@ -1744,7 +1744,7 @@ bool Problem::check(int verbose_level) const
if ( fo ) // case FEATURE: if ( fo ) // case FEATURE:
{ {
if (verbose_level > 0) if (verbose_level > 0)
cout << " --> Ftr" << fo->id() << " <- "; cout << " ( --> Ftr" << fo->id() << " <- ";
bool found = false; bool found = false;
for (auto cby : fo->getConstrainedByList()) for (auto cby : fo->getConstrainedByList())
{ {
...@@ -1753,7 +1753,7 @@ bool Problem::check(int verbose_level) const ...@@ -1753,7 +1753,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby); found = found || (c == cby);
} }
if (verbose_level > 0) if (verbose_level > 0)
cout << endl; cout << ")";
// check constrained_by pointer in constrained feature // check constrained_by pointer in constrained feature
// is_consistent = is_consistent && found; // is_consistent = is_consistent && found;
...@@ -1768,7 +1768,7 @@ bool Problem::check(int verbose_level) const ...@@ -1768,7 +1768,7 @@ bool Problem::check(int verbose_level) const
if ( Lo ) // case LANDMARK: if ( Lo ) // case LANDMARK:
{ {
if (verbose_level > 0) if (verbose_level > 0)
cout << " --> Lmk" << Lo->id() << " <- "; cout << " ( --> Lmk" << Lo->id() << " <- ";
bool found = false; bool found = false;
for (auto cby : Lo->getConstrainedByList()) for (auto cby : Lo->getConstrainedByList())
{ {
...@@ -1777,7 +1777,7 @@ bool Problem::check(int verbose_level) const ...@@ -1777,7 +1777,7 @@ bool Problem::check(int verbose_level) const
found = found || (c == cby); found = found || (c == cby);
} }
if (verbose_level > 0) if (verbose_level > 0)
cout << endl; cout << ")";
// check constrained_by pointer in constrained landmark // check constrained_by pointer in constrained landmark
// is_consistent = is_consistent && found; // is_consistent = is_consistent && found;
...@@ -1787,6 +1787,9 @@ bool Problem::check(int verbose_level) const ...@@ -1787,6 +1787,9 @@ bool Problem::check(int verbose_level) const
//Clear inconsistency_explanation //Clear inconsistency_explanation
std::stringstream().swap(inconsistency_explanation); std::stringstream().swap(inconsistency_explanation);
} }
if (verbose_level > 0)
cout << endl;
if (verbose_level > 0) if (verbose_level > 0)
{ {
cout << " -> Prb @ " << c->getProblem().get() << endl; cout << " -> Prb @ " << c->getProblem().get() << endl;
...@@ -1824,38 +1827,102 @@ bool Problem::check(int verbose_level) const ...@@ -1824,38 +1827,102 @@ bool Problem::check(int verbose_level) const
cout << " (lp @ " << lp.get() << ")"; cout << " (lp @ " << lp.get() << ")";
} }
} }
bool found_here;
std::vector<StateBlockPtr> sb_vec;
// find in own Frame // find in own Frame
found = found || (std::find_if(F->getStateBlockMap().begin(), F->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != F->getStateBlockMap().end()); sb_vec = F->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " F" << F->id();
found = found || found_here;
// find in own Capture // find in own Capture
found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end()); sb_vec = C->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " C" << C->id();
found = found || found_here;
// Find in other Captures of the own Frame
if (!found_here)
for (auto FC : F->getCaptureList())
{
sb_vec = FC->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " F" << F->id() << ".C" << FC->id();
found = found || found_here;
}
// find in own Sensor // find in own Sensor
if (S) if (S)
{ {
auto sb_vec = S->getStateBlockVec(); sb_vec = S->getStateBlockVec();
found = found || (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end()); found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " S" << S->id();
found = found || found_here;
} }
// find in constrained Frame // find in constrained Frame
if (Fo) if (Fo){
found = found || (std::find_if(Fo->getStateBlockMap().begin(), Fo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Fo->getStateBlockMap().end()); sb_vec = Fo->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Fo" << Fo->id();
found = found || found_here;
}
// find in constrained Capture // find in constrained Capture
if (Co) if (Co)
found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end()); {
sb_vec = Co->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Co" << Co->id();
found = found || found_here;
}
// Find in other Captures of the constrained Frame
if (!found_here && Fo)
for (auto FoC : Fo->getCaptureList())
{
sb_vec = FoC->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Fo" << Fo->id() << ".C" << FoC->id();
found = found || found_here;
}
// find in constrained Feature // find in constrained Feature
if (fo) if (fo)
{ {
// find in constrained feature's Frame // find in constrained feature's Frame
FrameBasePtr foF = fo->getFrame(); auto foF = fo->getFrame();
found = found || (std::find_if(foF->getStateBlockMap().begin(), foF->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != foF->getStateBlockMap().end()); sb_vec = foF->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " foF" << foF->id();
found = found || found_here;
// find in constrained feature's Capture // find in constrained feature's Capture
CaptureBasePtr foC = fo->getCapture(); auto foC = fo->getCapture();
found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end()); sb_vec = foC->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " foC" << foC->id();
found = found || found_here;
// find in constrained feature's Sensor // find in constrained feature's Sensor
SensorBasePtr foS = fo->getCapture()->getSensor(); auto foS = fo->getCapture()->getSensor();
found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end()); sb_vec = foS->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " foS" << foS->id();
found = found || found_here;
} }
// find in constrained landmark // find in constrained landmark
if (Lo) if (Lo)
found = found || (std::find_if(Lo->getStateBlockMap().begin(), Lo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Lo->getStateBlockMap().end()); {
sb_vec = Lo->getStateBlockVec();
found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
if (found_here) cout << " Lo" << Lo->id();
found = found || found_here;
}
if (verbose_level > 0) if (verbose_level > 0)
{ {
if (found) if (found)
......
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