Skip to content
Snippets Groups Projects
Commit a8c7a454 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Print sen and prc names in print()

parent 414e1820
No related branches found
No related tags found
2 merge requests!313WIP: Resolve "Processor constructors and creators requiring a sensor pointer?",!305WIP: Resolve "Do not use default YAML params"
Pipeline #4141 passed
...@@ -199,7 +199,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // ...@@ -199,7 +199,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
{ {
SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name); SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr) if (sen_ptr == nullptr)
throw std::runtime_error("Sensor not found. Cannot bind Processor."); throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, in must match in sensor and processor!");
if (_params_filename == "") if (_params_filename == "")
return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr); return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
else else
...@@ -217,7 +217,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // ...@@ -217,7 +217,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
{ {
SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name); SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr) if (sen_ptr == nullptr)
throw std::runtime_error("Sensor not found. Cannot bind Processor."); throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, in must match in sensor and processor!");
ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr); ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr);
prc_ptr->configure(sen_ptr); prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr); prc_ptr->link(sen_ptr);
...@@ -813,7 +813,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) ...@@ -813,7 +813,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// Sensors ======================================================================================= // Sensors =======================================================================================
for (auto S : getHardware()->getSensorList()) for (auto S : getHardware()->getSensorList())
{ {
cout << " S" << S->id() << " " << S->getType(); cout << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]"); if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
if (depth < 2) if (depth < 2)
cout << " -- " << S->getProcessorList().size() << "p"; cout << " -- " << S->getProcessorList().size() << "p";
...@@ -860,7 +860,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) ...@@ -860,7 +860,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{ {
if (p->isMotion()) if (p->isMotion())
{ {
std::cout << " pm" << p->id() << " " << p->getType() << endl; std::cout << " pm" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p); ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin()) if (pm->getOrigin())
cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F") cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
...@@ -873,17 +873,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) ...@@ -873,17 +873,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
} }
else else
{ {
cout << " pt" << p->id() << " " << p->getType() << endl; cout << " pt" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p); ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
if (pt) if (pt)
{ {
// ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt);
// if (ptt)
// {
// if (ptt->getPrevOrigin())
// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
// << ptt->getPrevOrigin()->getFrame()->id() << endl;
// }
if (pt->getOrigin()) if (pt->getOrigin())
cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F") cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
<< pt->getOrigin()->getFrame()->id() << endl; << pt->getOrigin()->getFrame()->id() << endl;
......
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