Maintains a history of graph localizations; uses a fixed frame to extrapolate A localization is a PoseStamped whose frame id is of the form 'node42'. More...
#include <localization_buffer.h>
Public Member Functions | |
bool | hasLocalization (const ros::Time &t) const |
boost::optional < geometry_msgs::PoseStamped > | lastLocalization () const |
geometry_msgs::PoseStamped | localizationAt (const ros::Time &t, const ros::Duration &d) const |
geometry_msgs::PoseStamped | localizationAt (const ros::Time &t) const |
LocalizationBuffer (const unsigned buffer_size, const std::string &fixed_frame, const std::string &base_frame, TfPtr tf, const double max_extrapolation) | |
Private Member Functions | |
void | localizationCallback (graph_mapping_msgs::LocalizationDistribution::ConstPtr m) |
Private Attributes | |
const std::string | base_frame_ |
boost::circular_buffer < graph_mapping_msgs::LocalizationDistribution::ConstPtr > | buffer_ |
const std::string | fixed_frame_ |
ros::Subscriber | loc_sub_ |
const double | max_extrapolation_ |
boost::mutex | mutex_ |
ros::NodeHandle | nh_ |
TfPtr | tf_ |
Maintains a history of graph localizations; uses a fixed frame to extrapolate A localization is a PoseStamped whose frame id is of the form 'node42'.
Threadsafe
Definition at line 58 of file localization_buffer.h.
graph_slam::LocalizationBuffer::LocalizationBuffer | ( | const unsigned | buffer_size, | |
const std::string & | fixed_frame, | |||
const std::string & | base_frame, | |||
TfPtr | tf, | |||
const double | max_extrapolation | |||
) |
bool graph_slam::LocalizationBuffer::hasLocalization | ( | const ros::Time & | t | ) | const |
Whether | we can interpolate a localization at the given time |
Definition at line 122 of file localization_buffer.cpp.
boost::optional< gm::PoseStamped > graph_slam::LocalizationBuffer::lastLocalization | ( | ) | const |
Definition at line 138 of file localization_buffer.cpp.
gm::PoseStamped graph_slam::LocalizationBuffer::localizationAt | ( | const ros::Time & | t, | |
const ros::Duration & | d | |||
) | const |
LocalizationExtrapolationException |
Definition at line 94 of file localization_buffer.cpp.
gm::PoseStamped graph_slam::LocalizationBuffer::localizationAt | ( | const ros::Time & | t | ) | const |
LocalizationExtrapolationException |
Definition at line 88 of file localization_buffer.cpp.
void graph_slam::LocalizationBuffer::localizationCallback | ( | graph_mapping_msgs::LocalizationDistribution::ConstPtr | m | ) | [private] |
const std::string graph_slam::LocalizationBuffer::base_frame_ [private] |
Definition at line 82 of file localization_buffer.h.
boost::circular_buffer<graph_mapping_msgs::LocalizationDistribution::ConstPtr> graph_slam::LocalizationBuffer::buffer_ [private] |
Definition at line 87 of file localization_buffer.h.
const std::string graph_slam::LocalizationBuffer::fixed_frame_ [private] |
Definition at line 81 of file localization_buffer.h.
ros::Subscriber graph_slam::LocalizationBuffer::loc_sub_ [private] |
Definition at line 89 of file localization_buffer.h.
const double graph_slam::LocalizationBuffer::max_extrapolation_ [private] |
Definition at line 83 of file localization_buffer.h.
boost::mutex graph_slam::LocalizationBuffer::mutex_ [mutable, private] |
Definition at line 85 of file localization_buffer.h.
ros::NodeHandle graph_slam::LocalizationBuffer::nh_ [private] |
Definition at line 88 of file localization_buffer.h.
TfPtr graph_slam::LocalizationBuffer::tf_ [private] |
Definition at line 86 of file localization_buffer.h.