#include <ros_filter.h>

Public Member Functions
bool	getFilteredOdometryMessage (nav_msgs::Odometry &message)
	Retrieves the EKF's output for broadcasting.
void	imuCallback (const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, const std::vector< int > &updateVector, const bool differential)
	Callback method for receiving all IMU messages.
void	loadParams ()
	Loads all parameters from file.
void	odometryCallback (const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName, const std::vector< int > &updateVector, const bool differential)
	Callback method for receiving all odometry messages.
void	poseCallback (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const std::vector< int > &updateVector, const bool differential)
	Callback method for receiving all pose messages.
	RosFilter ()
	Constructor.
void	run ()
	Main run method.
void	setPoseCallback (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
	Callback method for manually setting/resetting the internal pose estimate.
void	twistCallback (const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const std::vector< int > &updateVector)
	Callback method for receiving all twist messages.
	~RosFilter ()
Protected Member Functions
std::vector< int >	loadUpdateConfig (const std::string &topicName)
	Loads fusion settings from the config file.
bool	lookupTransformSafe (const std::string &targetFrame, const std::string &sourceFrame, const ros::Time &time, tf::StampedTransform &targetFrameTrans)
	Method for safely obtaining transforms.
bool	preparePose (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const bool differential, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance)
	Prepares a pose message for integration into the filter.
bool	prepareTwist (const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance)
	Prepares a twist message for integration into the filter.
void	quatToRPY (const tf::Quaternion &quat, double &roll, double &pitch, double &yaw)
	Utility method for converting quaternion to RPY.
Protected Attributes
std::string	baseLinkFrameId_
	tf frame name for the robot's body frame
std::ofstream	debugStream_
	Used for outputting debug messages.
Filter	filter_
double	frequency_
	The frequency of the run loop.
std::vector< ros::Subscriber >	imuTopicSubs_
	Vector to hold our IMU message filter subscriber objects so they don't go out of scope.
std::map< std::string, ros::Time >	lastMessageTimes_
	Store the last time a message from each topic was received.
std::string	mapFrameId_
	tf frame name for the robot's map (world-fixed) frame
ros::NodeHandle	nh_
	Node handle.
ros::NodeHandle	nhLocal_
	Local node handle (for params)
std::string	odomFrameId_
	tf frame name for the robot's odometry (world-fixed) frame
std::vector< ros::Subscriber >	odomTopicSubs_
	Vector to hold our odometry message filter subscriber objects so they don't go out of scope.
std::map< std::string, poseMFPtr >	poseMessageFilters_
	Vector to hold our pose message filters so they don't go out of scope.
std::vector< poseMFSubPtr >	poseTopicSubs_
	Vector to hold our pose message filter subscriber objects so they don't go out of scope.
std::map< std::string, tf::Transform >	previousMeasurements_
	Stores the last measurement from a given topic for differential integration.
std::map< std::string, tf::Transform >	previousStates_
	Stores the last state estimate at the time the previous measurement from this sensor was captured.
ros::Subscriber	setPoseSub_
	Subscribes to the set_pose topic (usually published from rviz) - a geometry_msgs/PoseWithCovarianceStamped.
tf::TransformListener	tfListener_
	Transform listener for managing coordinate transforms.
std::string	tfPrefix_
	tf prefix
std::map< std::string, twistMFPtr >	twistMessageFilters_
	Vector to hold our twist message filters so they don't go out of scope.
std::vector< twistMFSubPtr >	twistTopicSubs_
	Vector to hold our twist message filter subscriber objects so they don't go out of scope.
geometry_msgs::TransformStamped	worldBaseLinkTransMsg_
	Message that contains our latest transform (i.e., state)
std::string	worldFrameId_
	tf frame name that is the parent frame of the transform that this node will calculate and broadcast.

Detailed Description

template<class Filter>
class RobotLocalization::RosFilter< Filter >

Definition at line 86 of file ros_filter.h.

Constructor & Destructor Documentation

template<class Filter >

RobotLocalization::RosFilter< Filter >::RosFilter ( ) [inline]

Constructor.

The RosFilter constructor makes sure that anyone using this template is doing so with the correct object type

Definition at line 95 of file ros_filter.h.

template<class Filter >

RobotLocalization::RosFilter< Filter >::~RosFilter ( ) [inline]

Definition at line 103 of file ros_filter.h.

Member Function Documentation

template<class Filter >

bool RobotLocalization::RosFilter< Filter >::getFilteredOdometryMessage ( nav_msgs::Odometry & message ) [inline]

Retrieves the EKF's output for broadcasting.

Parameters:

[out] message - The standard ROS odometry message to be filled

Returns:: true if the filter is initialized, false otherwise

Definition at line 115 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::imuCallback	(	const sensor_msgs::Imu::ConstPtr &	msg,
		const std::string &	topicName,
		const std::vector< int > &	updateVector,
		const bool	differential
	)		`[inline]`

Callback method for receiving all IMU messages.

Parameters:

[in]	msg	- The ROS IMU message to take in.
[in]	topicName	- The name of the IMU data topic (we support many)
[in]	updateVector	- Specifies which variables we want to update from this measurement
[in]	differential	- Whether we integrate the pose portions of this message differentially

This method really just separates out the absolute orientation and velocity data into two new messages and sends them to their respective pose and twist callbacks.

Definition at line 632 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::loadParams ( ) [inline]

Loads all parameters from file.

Definition at line 174 of file ros_filter.h.

template<class Filter >

std::vector<int> RobotLocalization::RosFilter< Filter >::loadUpdateConfig ( const std::string & topicName ) [inline, protected]

Loads fusion settings from the config file.

Parameters:

[in] topicName - The name of the topic for which to load settings

Returns:: The boolean vector of update settings for each variable for this topic

Definition at line 1095 of file ros_filter.h.

template<class Filter >

bool RobotLocalization::RosFilter< Filter >::lookupTransformSafe	(	const std::string &	targetFrame,
		const std::string &	sourceFrame,
		const ros::Time &	time,
		tf::StampedTransform &	targetFrameTrans
	)		`[inline, protected]`

Method for safely obtaining transforms.

Parameters:

[in]	targetFrame	- The target frame of the desired transform
[in]	sourceFrame	- The source frame of the desired transform
[in]	time	- The time at which we want the transform
[out]	targetFrameTrans	- The resulting stamped transform object

Returns:: Sets the value of targetFrameTrans and returns true if sucessful, false otherwise.

This method attempts to obtain a transform from the sourceFrame to the targetFrame at the specific time. If no transform is available at that time, it attempts to simply obtain the latest transform. If that still fails, then the method checks to see if the transform is going from a given frame_id to itself. If any of these checks succeed, the method sets the value of targetFrameTrans and returns true, otherwise it returns false.

Definition at line 1138 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::odometryCallback	(	const nav_msgs::Odometry::ConstPtr &	msg,
		const std::string &	topicName,
		const std::vector< int > &	updateVector,
		const bool	differential
	)		`[inline]`

Callback method for receiving all odometry messages.

Parameters:

[in]	msg	- The ROS odometry message to take in.
[in]	topicName	- The name of the odometry topic (we support many)
[in]	updateVector	- Specifies which variables we want to update from this measurement
[in]	differential	- Whether we integrate the pose portions of this message differentially

This method really just separates out the pose and twist data into two new messages, and passes them to their respective callbacks

Definition at line 700 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::poseCallback	(	const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		const std::vector< int > &	updateVector,
		const bool	differential
	)		`[inline]`

Callback method for receiving all pose messages.

Parameters:

[in]	msg	- The ROS stamped pose with covariance message to take in
[in]	topicName	- The name of the pose topic (we support many)
[in]	targetFrame	- The tf frame name into which we will transform this measurement
[in]	updateVector	- Specifies which variables we want to update from this measurement
[in]	differential	- Whether we integrate the pose portions of this message differentially

Definition at line 747 of file ros_filter.h.

template<class Filter >

bool RobotLocalization::RosFilter< Filter >::preparePose	(	const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		const bool	differential,
		std::vector< int > &	updateVector,
		Eigen::VectorXd &	measurement,
		Eigen::MatrixXd &	measurementCovariance
	)		`[inline, protected]`

Prepares a pose message for integration into the filter.

Parameters:

[in]	msg	- The pose message to prepare
[in]	topicName	- The name of the topic over which this message was received
[in]	targetFrame	- The target tf frame
	differential	- Whether we're carrying out differential integration
[in,out]	updateVector	- The update vector for the data source
[out]	measurement	- The pose data converted to a measurement
[out]	measurementCovariance	- The covariance of the converted measurement

Returns:: true indicates that the measurement was successfully prepared, false otherwise

Definition at line 1202 of file ros_filter.h.

template<class Filter >

bool RobotLocalization::RosFilter< Filter >::prepareTwist	(	const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		std::vector< int > &	updateVector,
		Eigen::VectorXd &	measurement,
		Eigen::MatrixXd &	measurementCovariance
	)		`[inline, protected]`

Prepares a twist message for integration into the filter.

Parameters:

[in]	msg	- The twist message to prepare
[in]	topicName	- The name of the topic over which this message was received
[in]	targetFrame	- The target tf frame
[in,out]	updateVector	- The update vector for the data source
[out]	measurement	- The twist data converted to a measurement
[out]	measurementCovariance	- The covariance of the converted measurement

Returns:: true indicates that the measurement was successfully prepared, false otherwise

Definition at line 1515 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::quatToRPY	(	const tf::Quaternion &	quat,
		double &	roll,
		double &	pitch,
		double &	yaw
	)		`[inline, protected]`

Utility method for converting quaternion to RPY.

Parameters:

[in]	quat	- The quaternion to convert
[out]	roll	- The converted roll
[out]	pitch	- The converted pitch
[out]	yaw	- The converted yaw

Definition at line 1672 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::run ( ) [inline]

Main run method.

Definition at line 968 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::setPoseCallback ( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & msg ) [inline]

Callback method for manually setting/resetting the internal pose estimate.

Parameters:

[in] msg - The ROS stamped pose with covariance message to take in

Definition at line 835 of file ros_filter.h.

template<class Filter >

void RobotLocalization::RosFilter< Filter >::twistCallback	(	const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		const std::vector< int > &	updateVector
	)		`[inline]`

Callback method for receiving all twist messages.

Parameters:

[in]	msg	- The ROS stamped twist with covariance message to take in.
[in]	topicName	- The name of the twist topic (we support many)
[in]	targetFrame	- The tf frame name into which we will transform this measurement
[in]	updateVector	- Specifies which variables we want to update from this measurement

Definition at line 890 of file ros_filter.h.

Member Data Documentation

template<class Filter >

std::string RobotLocalization::RosFilter< Filter >::baseLinkFrameId_ [protected]

tf frame name for the robot's body frame

Definition at line 1688 of file ros_filter.h.

template<class Filter >

std::ofstream RobotLocalization::RosFilter< Filter >::debugStream_ [protected]

Used for outputting debug messages.

Definition at line 1789 of file ros_filter.h.

template<class Filter >

Filter RobotLocalization::RosFilter< Filter >::filter_ [protected]

Definition at line 1089 of file ros_filter.h.

template<class Filter >

double RobotLocalization::RosFilter< Filter >::frequency_ [protected]

The frequency of the run loop.

Definition at line 1680 of file ros_filter.h.

template<class Filter >

std::vector<ros::Subscriber> RobotLocalization::RosFilter< Filter >::imuTopicSubs_ [protected]

Vector to hold our IMU message filter subscriber objects so they don't go out of scope.

Definition at line 1761 of file ros_filter.h.

template<class Filter >

std::map<std::string, ros::Time> RobotLocalization::RosFilter< Filter >::lastMessageTimes_ [protected]

Store the last time a message from each topic was received.

If we're getting messages rapidly, we may accidentally get an older message arriving after a newer one. This variable keeps track of the most recent message time for each subscribed message topic. We also use it when listening to odometry messages to determine if we should be using messages from that topic.

Definition at line 1711 of file ros_filter.h.

template<class Filter >

std::string RobotLocalization::RosFilter< Filter >::mapFrameId_ [protected]

tf frame name for the robot's map (world-fixed) frame

Definition at line 1696 of file ros_filter.h.

template<class Filter >

ros::NodeHandle RobotLocalization::RosFilter< Filter >::nh_ [protected]

Node handle.

Definition at line 1769 of file ros_filter.h.

template<class Filter >

ros::NodeHandle RobotLocalization::RosFilter< Filter >::nhLocal_ [protected]

Local node handle (for params)

Definition at line 1773 of file ros_filter.h.

template<class Filter >

std::string RobotLocalization::RosFilter< Filter >::odomFrameId_ [protected]

tf frame name for the robot's odometry (world-fixed) frame

Definition at line 1692 of file ros_filter.h.

template<class Filter >

std::vector<ros::Subscriber> RobotLocalization::RosFilter< Filter >::odomTopicSubs_ [protected]

Vector to hold our odometry message filter subscriber objects so they don't go out of scope.

Definition at line 1738 of file ros_filter.h.

template<class Filter >

std::map<std::string, poseMFPtr> RobotLocalization::RosFilter< Filter >::poseMessageFilters_ [protected]

Vector to hold our pose message filters so they don't go out of scope.

Definition at line 1747 of file ros_filter.h.

template<class Filter >

std::vector<poseMFSubPtr> RobotLocalization::RosFilter< Filter >::poseTopicSubs_ [protected]

Vector to hold our pose message filter subscriber objects so they don't go out of scope.

Definition at line 1743 of file ros_filter.h.

template<class Filter >

std::map<std::string, tf::Transform> RobotLocalization::RosFilter< Filter >::previousMeasurements_ [protected]

Stores the last measurement from a given topic for differential integration.

To carry out differential integration, we have to (1) transform that into the target frame (probably the frame specified by odomFrameId_), (2) "subtract" the previous measurement from the current measurement, and then (3) transform it again by the previous state estimate. This holds the measurements used for step (2).

Definition at line 1722 of file ros_filter.h.

template<class Filter >

std::map<std::string, tf::Transform> RobotLocalization::RosFilter< Filter >::previousStates_ [protected]

Stores the last state estimate at the time the previous measurement from this sensor was captured.

To carry out differential integration, we have to (1) transform that into the target frame (probably the frame specified by odomFrameId_), (2) "subtract" the previous measurement from the current measurement, and then (3) transform it again by the previous state estimate. This holds the measurements used for step (3).

Definition at line 1733 of file ros_filter.h.

template<class Filter >

ros::Subscriber RobotLocalization::RosFilter< Filter >::setPoseSub_ [protected]

Subscribes to the set_pose topic (usually published from rviz) - a geometry_msgs/PoseWithCovarianceStamped.

Definition at line 1765 of file ros_filter.h.

template<class Filter >

tf::TransformListener RobotLocalization::RosFilter< Filter >::tfListener_ [protected]

Transform listener for managing coordinate transforms.

Definition at line 1785 of file ros_filter.h.

template<class Filter >

std::string RobotLocalization::RosFilter< Filter >::tfPrefix_ [protected]

tf prefix

Definition at line 1684 of file ros_filter.h.

template<class Filter >

std::map<std::string, twistMFPtr> RobotLocalization::RosFilter< Filter >::twistMessageFilters_ [protected]

Vector to hold our twist message filters so they don't go out of scope.

Definition at line 1756 of file ros_filter.h.

template<class Filter >

std::vector<twistMFSubPtr> RobotLocalization::RosFilter< Filter >::twistTopicSubs_ [protected]

Vector to hold our twist message filter subscriber objects so they don't go out of scope.

Definition at line 1752 of file ros_filter.h.

template<class Filter >

geometry_msgs::TransformStamped RobotLocalization::RosFilter< Filter >::worldBaseLinkTransMsg_ [protected]

Message that contains our latest transform (i.e., state)

We use the vehicle's latest state in a number of places, and often use it as a transform, so this is the most convenient variable to use as our global "current state" object

Definition at line 1781 of file ros_filter.h.

template<class Filter >

std::string RobotLocalization::RosFilter< Filter >::worldFrameId_ [protected]

tf frame name that is the parent frame of the transform that this node will calculate and broadcast.

Definition at line 1701 of file ros_filter.h.

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

ros_filter.h

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

template<class Filter> class RobotLocalization::RosFilter< Filter >

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

template<class Filter>
class RobotLocalization::RosFilter< Filter >