Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf into devel

include/core/frame/frame_base.h

@@ -139,17 +139,17 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         bool isConstrainedBy(const FactorBaseConstPtr& _factor) const;
 
         template <class C>
-        CaptureBaseConstPtr getCaptureOfType() const;
+        std::shared_ptr<const C> getCaptureOfType() const;
         template <class C>
-        CaptureBasePtr getCaptureOfType();
+        std::shared_ptr<C> getCaptureOfType();
 
         CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
         CaptureBasePtr getCaptureOfType(const std::string& type);
 
         template <class C>
-        CaptureBaseConstPtrList getCapturesOfType() const;
+        std::list<std::shared_ptr<const C>> getCapturesOfType() const;
         template <class C>
-        CaptureBasePtrList getCapturesOfType();
+        std::list<std::shared_ptr<C>> getCapturesOfType();
 
         CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
         CaptureBasePtrList getCapturesOfType(const std::string& type);
@@ -290,40 +290,52 @@ inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
 }
 
 template <class C>
-inline CaptureBaseConstPtr FrameBase::getCaptureOfType() const
+inline std::shared_ptr<const C> FrameBase::getCaptureOfType() const
 {
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            return capture_ptr;
+    {
+        auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
+        if (cap_derived)
+            return cap_derived;
+    }
     return nullptr;
 }
 
 template <class C>
-inline CaptureBasePtr FrameBase::getCaptureOfType()
+inline std::shared_ptr<C> FrameBase::getCaptureOfType()
 {
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            return capture_ptr;
+    {
+        auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
+        if (cap_derived)
+            return cap_derived;
+    }
     return nullptr;
 }
 
 template <class C>
-inline CaptureBaseConstPtrList FrameBase::getCapturesOfType() const
+inline std::list<std::shared_ptr<const C>> FrameBase::getCapturesOfType() const
 {
-    CaptureBaseConstPtrList captures;
+    std::list<std::shared_ptr<const C>> captures;
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            captures.push_back(capture_ptr);
+    {
+        auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
+        if (cap_derived)
+            captures.push_back(cap_derived);
+    }
     return captures;
 }
 
 template <class C>
-inline CaptureBasePtrList FrameBase::getCapturesOfType()
+inline std::list<std::shared_ptr<C>> FrameBase::getCapturesOfType()
 {
-    CaptureBasePtrList captures;
+    std::list<std::shared_ptr<C>> captures;
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            captures.push_back(capture_ptr);
+    {
+        auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
+        if (cap_derived)
+            captures.push_back(cap_derived);
+    }
     return captures;
 }