diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 8d426eacab111c81b435e48e630d39a802e02a6f..a0c5a62bb1fe60ebcd1ee814c05199e25303a4fc 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -157,16 +157,16 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
CaptureBaseConstPtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr);
- CaptureBaseConstPtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
+ CaptureBaseConstPtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr, const std::string& type) const;
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
- CaptureBaseConstPtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
+ CaptureBaseConstPtrList getCapturesOf(const SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
- FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
+ FactorBaseConstPtr getFactorOf(const ProcessorBaseConstPtr _processor_ptr) const;
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
- FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
+ FactorBaseConstPtr getFactorOf(const ProcessorBaseConstPtr _processor_ptr, const std::string& type) const;
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
unsigned int getHits() const;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index fc93357dd46a948c4d225e982b8ade7bf4682af9..52eab3b6b239922b5afbfe08e84361acd2c0e98e 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -92,6 +92,7 @@ class Buffer
public:
typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator
+ typedef typename std::map<TimeStamp,T>::const_iterator ConstIterator; // buffer iterator
Buffer(){};
~Buffer(void){};
@@ -101,27 +102,28 @@ public:
* Select from the buffer the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
- T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
/**\brief Select an element iterator from the buffer
*
* Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
+ ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
- T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
- T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance);
+ T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
- T selectFirst();
+ T selectFirst() const;
- T selectLast();
+ T selectLast() const;
/**\brief Buffer size
*
*/
- SizeStd size(void);
+ SizeStd size(void) const;
/**\brief Add a element to the buffer
*
@@ -132,6 +134,7 @@ public:
*
* elements are ordered from most recent to oldest
*/
+ const std::map<TimeStamp,T>& getContainer() const;
std::map<TimeStamp,T>& getContainer();
/**\brief Remove all elements in the buffer with a time stamp older than the specified
@@ -152,7 +155,7 @@ public:
/**\brief is the buffer empty ?
*
*/
- bool empty();
+ bool empty() const;
protected:
/**\brief Check time tolerance
@@ -160,14 +163,19 @@ protected:
* Check if the time distance between two time stamps is smaller than
* the time tolerance.
*/
- static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
+ static bool checkTimeTolerance(const TimeStamp& _time_stamp1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance);
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the minimum time tolerance of the two frames.
*/
- static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2);
+ static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
+ const double& _time_tolerance1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance2);
protected:
@@ -454,6 +462,42 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&...
/////////////////////////////////////////////////////////////////////////////////////////
+template <typename T>
+typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+{
+ Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+ bool post_exists = (post != container_.end());
+
+ bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+ if (prev_exists)
+ {
+ Buffer<T>::ConstIterator prev = std::prev(post);
+
+ bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+ if (prev_ok && !post_ok)
+ return prev;
+
+ else if (!prev_ok && post_ok)
+ return post;
+
+ else if (prev_ok && post_ok)
+ {
+ if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+ return post;
+ else
+ return prev;
+ }
+ }
+ else if (post_ok)
+ return post;
+
+ return container_.end();
+}
+
template <typename T>
typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
{
@@ -491,12 +535,12 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st
}
template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
if (container_.empty())
return nullptr;
- Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance);
+ auto 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
@@ -508,7 +552,7 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
}
template <typename T>
-T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty())
@@ -529,7 +573,7 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
template <typename T>
-T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty())
@@ -549,7 +593,7 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
}
template <typename T>
-T Buffer<T>::selectFirst()
+T Buffer<T>::selectFirst() const
{
// There is no element
if (container_.empty())
@@ -560,7 +604,7 @@ T Buffer<T>::selectFirst()
}
template <typename T>
-T Buffer<T>::selectLast()
+T Buffer<T>::selectLast() const
{
// There is no element
if (container_.empty())
@@ -576,6 +620,12 @@ void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
container_.emplace(_time_stamp, _element);
}
+template <typename T>
+const std::map<TimeStamp,T>& Buffer<T>::getContainer() const
+{
+ return container_;
+}
+
template <typename T>
std::map<TimeStamp,T>& Buffer<T>::getContainer()
{
@@ -589,13 +639,13 @@ inline void Buffer<T>::clear()
}
template <typename T>
-inline bool Buffer<T>::empty()
+inline bool Buffer<T>::empty() const
{
return container_.empty();
}
template <typename T>
-inline SizeStd Buffer<T>::size(void)
+inline SizeStd Buffer<T>::size(void) const
{
return container_.size();
}
@@ -628,8 +678,8 @@ inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
template <typename T>
inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
- const TimeStamp& _time_stamp2,
- const double& _time_tolerance)
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
bool pass = time_diff <= _time_tolerance;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index f295cdbca724a4e4ee89b8d2aa5c52ae487b537a..bba64203c3b7285208e54ab4f6b98c3d86e47846 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -230,7 +230,7 @@ CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type)
return captures;
}
-CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr, const std::string& type) const
{
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
@@ -264,7 +264,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr)
return nullptr;
}
-CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
+CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBaseConstPtr _sensor_ptr) const
{
CaptureBaseConstPtrList captures;
for (auto capture_ptr : getCaptureList())
@@ -282,7 +282,7 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
return captures;
}
-FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
+FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBaseConstPtr _processor_ptr, const std::string& type) const
{
for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
@@ -308,7 +308,7 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, cons
return nullptr;
}
-FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
+FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBaseConstPtr _processor_ptr) const
{
for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)