From 8a0b1d89051a4f74c99482816476b02a158bd78a Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Mon, 9 Oct 2017 18:17:56 +0200
Subject: [PATCH] add getConstraintOf

---
 src/frame_base.h | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/frame_base.h b/src/frame_base.h
index 408b6b0bc..773eb6ef1 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -135,6 +135,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr);
         void unlinkCapture(CaptureBasePtr _cap_ptr);
 
+        ConstraintBasePtr getConstraintOf(const ProcessorBasePtr _processor_ptr);
+        ConstraintBasePtr getConstraintOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
+
         void getConstraintList(ConstraintBaseList& _ctr_list);
         virtual ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr);
         unsigned int getHits() const;
@@ -323,6 +326,25 @@ inline void FrameBase::unlinkCapture(CaptureBasePtr _cap_ptr)
     capture_list_.remove(_cap_ptr);
 }
 
+inline ConstraintBasePtr
+FrameBase::getConstraintOf(const ProcessorBasePtr _processor_ptr)
+{
+  for (const ConstraintBasePtr& constaint_ptr : getConstrainedByList())
+      if (constaint_ptr->getProcessor() == _processor_ptr)
+          return constaint_ptr;
+  return nullptr;
+}
+
+inline ConstraintBasePtr
+FrameBase::getConstraintOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
+{
+  for (const ConstraintBasePtr& constaint_ptr : getConstrainedByList())
+      if (constaint_ptr->getProcessor() == _processor_ptr &&
+          constaint_ptr->getType() == type)
+          return constaint_ptr;
+  return nullptr;
+}
+
 inline void FrameBase::getConstraintList(ConstraintBaseList& _ctr_list)
 {
     for (auto c_ptr : getCaptureList())
-- 
GitLab