PositionTimepointBuffer Class Reference

#include <Localizer.hpp>

Public Member Functions
	PositionTimepointBuffer (unsigned int p_size=200)
void	add (PositionTimepoint const &element)
PositionTimepoint	get_last_by_time (rclcpp::Time &timestamp)
PositionTimepoint	get_last ()
bool	is_empty ()

Private Attributes
boost::circular_buffer< PositionTimepoint >	data
std::shared_mutex	m_mutex
unsigned int	dataSize = 0

Detailed Description

Definition at line 27 of file Localizer.hpp.

Constructor & Destructor Documentation

PositionTimepointBuffer()

PositionTimepointBuffer::PositionTimepointBuffer ( unsigned int  p_size = 200 )

inline

Definition at line 34 of file Localizer.hpp.

                                                     :
      data(p_size), dataSize(p_size) {}

Member Function Documentation

add()

void PositionTimepointBuffer::add ( PositionTimepoint const &  element )

inline

Definition at line 37 of file Localizer.hpp.

                                             {
    m_mutex.lock();
 
    if(data.full()) {
      data.pop_front();
    }
    data.push_back(element);
    
    m_mutex.unlock();
  }

Referenced by Localizer::add_position_timepoint(), Localizer::add_state(), and Localizer::init().

Here is the caller graph for this function:

get_last()

PositionTimepoint PositionTimepointBuffer::get_last ( )

inline

Definition at line 76 of file Localizer.hpp.

                               {
    m_mutex.lock_shared();
    auto returnPoint = data.back();
    m_mutex.unlock_shared();
 
    return returnPoint;
  }

Referenced by Localizer::add_state(), and Localizer::get_latest_position_timepoint().

Here is the caller graph for this function:

get_last_by_time()

PositionTimepoint PositionTimepointBuffer::get_last_by_time ( rclcpp::Time &  timestamp )

inline

Definition at line 48 of file Localizer.hpp.

                                                            {
    m_mutex.lock_shared();
 
    if(data.empty()) {
      log_warning("[Localizer] Position History Lookup failed. ");
      throw std::runtime_error("Localizer.hpp/PositionTimepointBuffer: Lookup failed!");
    }
 
    PositionTimepoint& element = data.back();
    for(int i = data.size() - 2; i >= 0; --i) {
      if(data[i].timestamp < timestamp) {
        if(std::abs((timestamp - data[i].timestamp).nanoseconds()) < std::abs((timestamp - element.timestamp).nanoseconds())) {
          element = data[i];
        }
        else {
          // pass (element remains element)
        }
        m_mutex.unlock_shared();
        return element;
      }
      element = data[i];
    }
 
    m_mutex.unlock_shared();
    log_warning("[Localizer] Position History Lookup fall back: ", std::to_string(data.at(data.size() - 1).timestamp.nanoseconds()), "  ", std::to_string(timestamp.nanoseconds()), " - Use oldest \n");
    return element;
  }

References galaxis::utils::log_warning(), and PositionTimepoint::timestamp.

Referenced by Localizer::get_position_timepoint_by_time().

Here is the call graph for this function:

Here is the caller graph for this function:

is_empty()

bool PositionTimepointBuffer::is_empty ( )

inline

Definition at line 84 of file Localizer.hpp.

                  {
    m_mutex.lock_shared();
    bool isEmpty = data.empty();
    m_mutex.unlock_shared();
 
    return isEmpty;
  }

Referenced by Localizer::add_state(), and Localizer::is_empty().

Here is the caller graph for this function:

Member Data Documentation

data

boost::circular_buffer<PositionTimepoint> PositionTimepointBuffer::data

private

Definition at line 29 of file Localizer.hpp.

dataSize

unsigned int PositionTimepointBuffer::dataSize = 0

private

Definition at line 31 of file Localizer.hpp.

m_mutex

std::shared_mutex PositionTimepointBuffer::m_mutex

private

Definition at line 30 of file Localizer.hpp.

---

The documentation for this class was generated from the following file:

• nodes/Autopilot/include/Localization/Localizer.hpp