Merge branch 'devel' of...

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

include/core/processor/processor_base.h

@@ -97,8 +97,17 @@ public:
     * respecting a defined time tolerances
     */
     T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    
+    /**\brief Select a Pack iterator from the buffer
+    *
+    *  Select from the buffer the iterator pointing to the closest pack (w.r.t. time stamp),
+    * respecting a defined time tolerances
+    */
+    Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
 
     T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    
+    T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance);
 
     T selectFirst();
 
@@ -118,7 +127,7 @@ public:
     *
     * elements are ordered from most recent to oldest
     */
-    const std::map<TimeStamp,T>& getContainer();
+    std::map<TimeStamp,T>& getContainer();
 
     /**\brief Remove all packs in the buffer with a time stamp older than the specified
     *
@@ -486,11 +495,8 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&...
 /////////////////////////////////////////////////////////////////////////////////////////
 
 template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
 {
-    if (container_.empty())
-        return nullptr;
-
     Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
 
     bool prev_exists = (post != container_.begin());
@@ -505,22 +511,39 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
         bool prev_ok = simpleCheckTimeTolerance(prev->first, _time_stamp, _time_tolerance);
 
         if (prev_ok && !post_ok)
-            return prev->second;
+            return prev;
 
         else if (!prev_ok && post_ok)
-            return post->second;
+            return post;
 
         else if (prev_ok && post_ok)
         {
             if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
-                return post->second;
+                return post;
             else
-                return prev->second;
+                return prev;
         }
     }
     else if (post_ok)
-        return post->second;
+        return post;
+
+    return container_.end();
+}
+
+template <typename T>
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+{
+    if (container_.empty())
+        return nullptr;
+
+    Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance);
 
+    // end is returned from selectIterator if an element of the buffer complying with the time stamp
+    // and time tolerance has not been found
+    if (it != container_.end()){
+        return it->second;
+    }
+    
     return nullptr;
 }
 
@@ -542,7 +565,27 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
 
     // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
     return nullptr;
+}
+
+
+template <typename T>
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance)
+{
+    // There is no element
+    if (container_.empty())
+         return nullptr;
 
+    // Checking on rbegin() since packs are ordered in time
+    // Return last pack if is older than time stamp
+    if (container_.rbegin()->first > _time_stamp)
+         return container_.rbegin()->second;
+
+    // Return last pack if despite being newer, it is within the time tolerance
+    if (simpleCheckTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
+        return container_.rbegin()->second;
+
+    // otherwise return nullptr (no pack after the provided ts or within the tolerance was found)
+    return nullptr;
 }
 
 template <typename T>
@@ -574,7 +617,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
 }
 
 template <typename T>
-const std::map<TimeStamp,T>& Buffer<T>::getContainer()
+std::map<TimeStamp,T>& Buffer<T>::getContainer()
 {
     return container_;
 }