RosRobotLocalizationListener class. More...
#include <ros_robot_localization_listener.h>
Public Member Functions | |
const std::string & | getBaseFrameId () const |
getBaseFrameId Returns the base frame id of the localization listener More... | |
bool | getState (const double time, const std::string &frame_id, Eigen::VectorXd &state, Eigen::MatrixXd &covariance, std::string world_frame_id="") const |
Get a state from the localization estimator. More... | |
bool | getState (const ros::Time &ros_time, const std::string &frame_id, Eigen::VectorXd &state, Eigen::MatrixXd &covariance, const std::string &world_frame_id="") const |
Get a state from the localization estimator. More... | |
const std::string & | getWorldFrameId () const |
getWorldFrameId Returns the world frame id of the localization listener More... | |
RosRobotLocalizationListener () | |
Constructor. More... | |
~RosRobotLocalizationListener () | |
Destructor. More... | |
Private Member Functions | |
void | odomAndAccelCallback (const nav_msgs::Odometry &odom, const geometry_msgs::AccelWithCovarianceStamped &accel) |
Callback for odom and accel. More... | |
Private Attributes | |
message_filters::Subscriber< geometry_msgs::AccelWithCovarianceStamped > | accel_sub_ |
Subscriber to the acceleration state topic (output of a filter node) More... | |
std::string | base_frame_id_ |
Child frame id received from the Odometry message. More... | |
RobotLocalizationEstimator * | estimator_ |
The core state estimator that facilitates inter- and extrapolation between buffered states. More... | |
ros::NodeHandle | nh_ |
A public handle to the ROS node. More... | |
ros::NodeHandle | nh_p_ |
A private handle to the ROS node. More... | |
message_filters::Subscriber< nav_msgs::Odometry > | odom_sub_ |
Subscriber to the odometry state topic (output of a filter node) More... | |
message_filters::TimeSynchronizer< nav_msgs::Odometry, geometry_msgs::AccelWithCovarianceStamped > | sync_ |
Waits for both an Odometry and an Accel message before calling a single callback function. More... | |
tf2_ros::Buffer | tf_buffer_ |
Tf buffer for looking up transforms. More... | |
tf2_ros::TransformListener | tf_listener_ |
Transform listener to fill the tf_buffer. More... | |
std::string | world_frame_id_ |
Frame id received from the odometry message. More... | |
RosRobotLocalizationListener class.
This class wraps the RobotLocalizationEstimator. It listens to topics over which the (filtered) robot state is published (odom and accel) and pushes them into its instance of the RobotLocalizationEstimator. It exposes a getState method to offer the user the estimated state at a requested time. This class offers the option to run this listener without the need to run a separate node. If you do wish to run this functionality in a separate node, consider the robot localization listener node.
Definition at line 58 of file ros_robot_localization_listener.h.
RobotLocalization::RosRobotLocalizationListener::RosRobotLocalizationListener | ( | ) |
Constructor.
The RosRobotLocalizationListener constructor initializes nodehandles, subscribers, a filter for synchronized listening to the topics it subscribes to, and an instance of the RobotLocalizationEstimator.
Definition at line 68 of file ros_robot_localization_listener.cpp.
RobotLocalization::RosRobotLocalizationListener::~RosRobotLocalizationListener | ( | ) |
const std::string & RobotLocalization::RosRobotLocalizationListener::getBaseFrameId | ( | ) | const |
getBaseFrameId Returns the base frame id of the localization listener
Definition at line 526 of file ros_robot_localization_listener.cpp.
bool RobotLocalization::RosRobotLocalizationListener::getState | ( | const double | time, |
const std::string & | frame_id, | ||
Eigen::VectorXd & | state, | ||
Eigen::MatrixXd & | covariance, | ||
std::string | world_frame_id = "" |
||
) | const |
Get a state from the localization estimator.
Requests the predicted state and covariance at a given time from the Robot Localization Estimator.
[in] | time | - time of the requested state |
[in] | frame_id | - frame id of which the state is requested. |
[out] | state | - state at the requested time |
[out] | covariance | - covariance at the requested time |
Definition at line 313 of file ros_robot_localization_listener.cpp.
bool RobotLocalization::RosRobotLocalizationListener::getState | ( | const ros::Time & | ros_time, |
const std::string & | frame_id, | ||
Eigen::VectorXd & | state, | ||
Eigen::MatrixXd & | covariance, | ||
const std::string & | world_frame_id = "" |
||
) | const |
Get a state from the localization estimator.
Overload of getState method for using ros::Time.
[in] | ros_time | - ros time of the requested state |
[in] | frame_id | - frame id of which the state is requested. |
[out] | state | - state at the requested time |
[out] | covariance | - covariance at the requested time |
Definition at line 508 of file ros_robot_localization_listener.cpp.
const std::string & RobotLocalization::RosRobotLocalizationListener::getWorldFrameId | ( | ) | const |
getWorldFrameId Returns the world frame id of the localization listener
Definition at line 531 of file ros_robot_localization_listener.cpp.
|
private |
Callback for odom and accel.
Puts the information from the odom and accel messages in a Robot Localization Estimator state and sets the most recent state of the estimator.
[in] | odometry | message |
[in] | accel | message |
Definition at line 177 of file ros_robot_localization_listener.cpp.
|
private |
Subscriber to the acceleration state topic (output of a filter node)
Definition at line 146 of file ros_robot_localization_listener.h.
|
private |
Child frame id received from the Odometry message.
Definition at line 154 of file ros_robot_localization_listener.h.
|
private |
The core state estimator that facilitates inter- and extrapolation between buffered states.
Definition at line 130 of file ros_robot_localization_listener.h.
|
private |
A public handle to the ROS node.
Definition at line 134 of file ros_robot_localization_listener.h.
|
private |
A private handle to the ROS node.
Definition at line 138 of file ros_robot_localization_listener.h.
|
private |
Subscriber to the odometry state topic (output of a filter node)
Definition at line 142 of file ros_robot_localization_listener.h.
|
private |
Waits for both an Odometry and an Accel message before calling a single callback function.
Definition at line 150 of file ros_robot_localization_listener.h.
|
private |
Tf buffer for looking up transforms.
Definition at line 162 of file ros_robot_localization_listener.h.
|
private |
Transform listener to fill the tf_buffer.
Definition at line 166 of file ros_robot_localization_listener.h.
|
private |
Frame id received from the odometry message.
Definition at line 158 of file ros_robot_localization_listener.h.