diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index 32fdd27cd2057c90248458175b556323ec562960..909bba38cf5b29ac07bf9c7d53484cc257cd39f1 100644 --- a/src/publisher_vision.cpp +++ b/src/publisher_vision.cpp @@ -71,7 +71,7 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name, img_transport_(ros::NodeHandle()) { // if user do not provide processor's name, first processor of type PublisherVisionDebug is taken - auto processor_name = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", ""); + auto processor_name = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", ""); show_tracks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_tracks_ID", false); show_landmarks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_landmarks_ID", false); @@ -90,21 +90,32 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name, // search the processor + if (processor_name == "") + throw std::runtime_error( + "PublisherVisionDebug: name of processor of type ProcessorVisualOdometry not provided"); + for (auto sen : _problem->getHardware()->getSensorList()) { for (auto proc : sen->getProcessorList()) { - processor_vision_ = std::dynamic_pointer_cast<ProcessorVisualOdometry>(proc); - if (processor_vision_ and (proc->getName() == processor_name or processor_name == "")) - break; + if (proc->getName() == processor_name) + { + processor_vision_ = std::dynamic_pointer_cast<ProcessorVisualOdometry>(proc); + if (processor_vision_) + break; // + else + { + throw std::runtime_error( + "PublisherVisionDebug: processor " + processor_name + " is not of type ProcessorVisualOdometry"); + break; + } + } } - if (processor_vision_) - break; } if (not processor_vision_) throw std::runtime_error( - "PublisherVisionDebug processor of type ProcessorVisualOdometry with the provided name was not found."); + "PublisherVisionDebug: processor " + processor_name + " not found."); } void PublisherVisionDebug::initialize(ros::NodeHandle &nh, const std::string &topic)