From 03db60335ea2322311baeadc72ac93fc3b1c49cd Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 28 Apr 2022 00:01:42 +0200
Subject: [PATCH] Use Problem::findProcessor()

---
 src/publisher_vision.cpp | 27 +++++++--------------------
 1 file changed, 7 insertions(+), 20 deletions(-)

diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index c0ba11e..6ba1556 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -113,28 +113,15 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_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())
-        {
-            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 (not processor_vision_)
+    auto proc = problem_->findProcessor(processor_name);
+    if (not proc)
         throw std::runtime_error(
                 "PublisherVisionDebug: processor " + processor_name + " not found.");
+    processor_vision_ = std::dynamic_pointer_cast<ProcessorVisualOdometry>(proc);
+    if (not processor_vision_)
+        throw std::runtime_error(
+                "PublisherVisionDebug: processor " + processor_name + " is not of type ProcessorVisualOdometry");
+
 }
 
 void PublisherVisionDebug::initialize(ros::NodeHandle &nh, const std::string &topic)
-- 
GitLab