#include <ros_filter.h>

Inheritance diagram for RobotLocalization::RosFilter< T >:

Public Member Functions
void	accelerationCallback (const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const std::vector< int > &updateVector, const double mahalanobisThresh)
	Callback method for receiving all acceleration (IMU) messages.
void	enqueueMeasurement (const std::string &topicName, const Eigen::VectorXd &measurement, const Eigen::MatrixXd &measurementCovariance, const std::vector< int > &updateVector, const double mahalanobisThresh, const ros::Time &time)
	Adds a measurement to the queue of measurements to be processed.
void	forceTwoD (Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance, std::vector< int > &updateVector)
	Method for zeroing out 3D variables within measurements.
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)
	Callback method for receiving all IMU messages.
void	integrateMeasurements (const double currentTime)
	Processes all measurements in the measurement queue, in temporal order.
void	loadParams ()
	Loads all parameters from file.
void	odometryCallback (const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName)
	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, const bool relative, const bool imuData, const double mahalanobisThresh)
	Callback method for receiving all pose messages.
	RosFilter (std::vector< double > args=std::vector< double >())
	Constructor.
void	run ()
	Main run method.
void	setPoseCallback (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
	Callback method for manually setting/resetting the internal pose estimate.
bool	setPoseSrvCallback (robot_localization::SetPose::Request &request, robot_localization::SetPose::Response &)
	Service callback for manually setting/resetting the internal pose estimate.
std::string	tfFailureReasonString (const tf::FilterFailureReason reason)
	Converts tf message filter failures to strings.
void	transformImuFailureCallback (const sensor_msgs::Imu::ConstPtr &msg, const tf::FilterFailureReason reason, const std::string &topicName, const std::string &targetFrame)
	Callback method for reporting failed IMU message transforms.
void	transformPoseFailureCallback (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const tf::FilterFailureReason reason, const std::string &topicName, const std::string &targetFrame)
	Callback method for reporting failed Pose message transforms.
void	transformTwistFailureCallback (const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, const tf::FilterFailureReason reason, const std::string &topicName, const std::string &targetFrame)
	Callback method for reporting failed Twist message transforms.
void	twistCallback (const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const std::vector< int > &updateVector, const double mahalanobisThresh)
	Callback method for receiving all twist messages.
	~RosFilter ()
	Destructor.
Protected Member Functions
void	addDiagnostic (const int errLevel, const std::string &topicAndClass, const std::string &message, const bool staticDiag)
	Adds a diagnostic message to the accumulating map and updates the error level.
void	aggregateDiagnostics (diagnostic_updater::DiagnosticStatusWrapper &wrapper)
	Aggregates all diagnostics so they can be published.
void	copyCovariance (const double *arr, Eigen::MatrixXd &covariance, const std::string &topicName, const std::vector< int > &updateVector, const size_t offset, const size_t dimension)
	Utility method for copying covariances from ROS covariance arrays to Eigen matrices.
void	copyCovariance (const Eigen::MatrixXd &covariance, double *arr, const size_t dimension)
	Utility method for copying covariances from Eigen matrices to ROS covariance arrays.
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	prepareAcceleration (const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, std::vector< int > &updateVector, Eigen::VectorXd &measurement, Eigen::MatrixXd &measurementCovariance)
	Prepares an IMU message's linear acceleration for integration into the filter.
bool	preparePose (const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, const std::string &topicName, const std::string &targetFrame, const bool differential, const bool relative, const bool imuData, 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.
Protected Attributes
std::map< std::string, imuMFPtr >	accelerationMessageFilters_
	Vector to hold our acceleration (represented as IMU) message filters so they don't go out of scope.
std::string	baseLinkFrameId_
	tf frame name for the robot's body frame
std::ofstream	debugStream_
	Used for outputting debug messages.
diagnostic_updater::Updater	diagnosticUpdater_
	Used for updating the diagnostics.
int	dynamicDiagErrorLevel_
	The max (worst) dynamic diagnostic level.
std::map< std::string, std::string >	dynamicDiagnostics_
	This object accumulates dynamic diagnostics, e.g., diagnostics relating to sensor data.
T	filter_
	Our filter (EKF, UKF, etc.)
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, tf::Transform >	initialMeasurements_
	Stores the first measurement from each topic for relative measurements.
std::map< std::string, ros::Time >	lastMessageTimes_
	Store the last time a message from each topic was received.
ros::Time	lastSetPoseTime_
	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
MeasurementQueue	measurementQueue_
	We process measurements by queueing them up in callbacks and processing them all at once within each iteration.
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, Eigen::MatrixXd >	previousMeasurementCovariances_
	We also need the previous covariance matrix for differential data.
std::map< std::string, tf::Transform >	previousMeasurements_
	Stores the last measurement from a given topic for differential integration.
bool	printDiagnostics_
	Whether or not we print diagnostic messages to the /diagnostics topic.
std::map< std::string, bool >	removeGravitationalAcc_
	If including acceleration for each IMU input, whether or not we remove acceleration due to gravity.
ros::ServiceServer	setPoseSrv_
	Service that allows another node to change the current state and recieve a confirmation. Uses a custom SetPose service.
ros::Subscriber	setPoseSub_
	Subscribes to the set_pose topic (usually published from rviz). Message type is geometry_msgs/PoseWithCovarianceStamped.
std::vector< std::string >	stateVariableNames_
	Contains the state vector variable names in string format.
int	staticDiagErrorLevel_
	The max (worst) static diagnostic level.
std::map< std::string, std::string >	staticDiagnostics_
	This object accumulates static diagnostics, e.g., diagnostics relating to the configuration parameters.
tf::TransformListener	tfListener_
	Transform listener for managing coordinate transforms.
std::string	tfPrefix_
	tf prefix
ros::Duration	tfTimeOffset_
	For future (or past) dating the world_frame->base_link_frame transform.
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.
bool	twoDMode_
	Whether or not we're in 2D mode.
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 T>
class RobotLocalization::RosFilter< T >

Definition at line 73 of file ros_filter.h.

Constructor & Destructor Documentation

template<typename T >

RobotLocalization::RosFilter< T >::RosFilter ( std::vector< double > args = std::vector<double>() )

Constructor.

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

Definition at line 41 of file ros_filter.cpp.

template<typename T >

RobotLocalization::RosFilter< T >::~RosFilter ( )

Destructor.

Clears out the message filters and topic subscribers.

Definition at line 71 of file ros_filter.cpp.

Member Function Documentation

template<typename T >

void RobotLocalization::RosFilter< T >::accelerationCallback	(	const sensor_msgs::Imu::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		const std::vector< int > &	updateVector,
		const double	mahalanobisThresh
	)

Callback method for receiving all acceleration (IMU) messages.

Parameters:

[in]	msg	- The ROS IMU message to take in.
[in]	topicName	- The name of the IMU topic
[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]	mahalanobisThresh	- Threshold, expressed as a Mahalanobis distance, for outliter rejection

Definition at line 83 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::addDiagnostic	(	const int	errLevel,
		const std::string &	topicAndClass,
		const std::string &	message,
		const bool	staticDiag
	)		`[protected]`

Adds a diagnostic message to the accumulating map and updates the error level.

Parameters:

[in]	errLevel	- The error level of the diagnostic
[in]	topicAndClass	- The sensor topic (if relevant) and class of diagnostic
[in]	message	- Details of the diagnostic

Definition at line 1652 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::aggregateDiagnostics ( diagnostic_updater::DiagnosticStatusWrapper & wrapper ) [protected]

Aggregates all diagnostics so they can be published.

Parameters:

[in] wrapper - The diagnostic status wrapper to update

Definition at line 1670 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::copyCovariance	(	const double *	arr,
		Eigen::MatrixXd &	covariance,
		const std::string &	topicName,
		const std::vector< int > &	updateVector,
		const size_t	offset,
		const size_t	dimension
	)		`[protected]`

Utility method for copying covariances from ROS covariance arrays to Eigen matrices.

This method copies the covariances and also does some data validation, which is why it requires more parameters than just the covariance containers.

Parameters:

[in]	arr	- The source array for the covariance data
[in]	covariance	- The destination matrix for the covariance data
[in]	topicName	- The name of the source data topic (for debug purposes)
[in]	updateVector	- The update vector for the source topic
[in]	offset	- The "starting" location within the array/update vector
[in]	dimension	- The number of values to copy, starting at the offset

Definition at line 1718 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::copyCovariance	(	const Eigen::MatrixXd &	covariance,
		double *	arr,
		const size_t	dimension
	)		`[protected]`

Utility method for copying covariances from Eigen matrices to ROS covariance arrays.

Parameters:

[in]	covariance	- The source matrix for the covariance data
[in]	arr	- The destination array
[in]	dimension	- The number of values to copy

Definition at line 1780 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::enqueueMeasurement	(	const std::string &	topicName,
		const Eigen::VectorXd &	measurement,
		const Eigen::MatrixXd &	measurementCovariance,
		const std::vector< int > &	updateVector,
		const double	mahalanobisThresh,
		const ros::Time &	time
	)

Adds a measurement to the queue of measurements to be processed.

Parameters:

[in]	topicName	- The name of the measurement source (only used for debugging)
[in]	measurement	- The measurement to enqueue
[in]	measurementCovariance	- The covariance of the measurement
[in]	updateVector	- The boolean vector that specifies which variables to update from this measurement
[in]	mahalanobisThresh	- Threshold, expressed as a Mahalanobis distance, for outliter rejection
[in]	time	- The time of arrival (in seconds)

Definition at line 154 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::forceTwoD	(	Eigen::VectorXd &	measurement,
		Eigen::MatrixXd &	measurementCovariance,
		std::vector< int > &	updateVector
	)

Method for zeroing out 3D variables within measurements.

Parameters:

[out]	measurement	- The measurement whose 3D variables will be zeroed out
[out]	measurementCovariance	- The covariance of the measurement
[out]	updateVector	- The boolean update vector of the measurement

If we're in 2D mode, then for every measurement from every sensor, we call this. It sets the 3D variables to 0, gives those variables tiny variances, and sets their updateVector values to 1.

Definition at line 173 of file ros_filter.cpp.

template<typename T >

bool RobotLocalization::RosFilter< T >::getFilteredOdometryMessage ( nav_msgs::Odometry & message )

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 203 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::imuCallback	(	const sensor_msgs::Imu::ConstPtr &	msg,
		const std::string &	topicName
	)

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)

This method really just separates out the absolute orientation and velocity data into two new messages and adds them to their respective pose and twist callback message filters.

Definition at line 261 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::integrateMeasurements ( const double currentTime )

Processes all measurements in the measurement queue, in temporal order.

Parameters:

[in] currentTime - The time at which to carry out integration (the current time)

Definition at line 332 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::loadParams ( )

Loads all parameters from file.

Definition at line 383 of file ros_filter.cpp.

template<typename T >

std::vector< int > RobotLocalization::RosFilter< T >::loadUpdateConfig ( const std::string & topicName ) [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 1794 of file ros_filter.cpp.

template<typename T >

bool RobotLocalization::RosFilter< T >::lookupTransformSafe	(	const std::string &	targetFrame,
		const std::string &	sourceFrame,
		const ros::Time &	time,
		tf::StampedTransform &	targetFrameTrans
	)		`[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 1830 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::odometryCallback	(	const nav_msgs::Odometry::ConstPtr &	msg,
		const std::string &	topicName
	)

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)

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 1182 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::poseCallback	(	const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		const std::vector< int > &	updateVector,
		const bool	differential,
		const bool	relative,
		const bool	imuData,
		const double	mahalanobisThresh
	)

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
[in]	mahalanobisThresh	- Threshold, expressed as a Mahalanobis distance, for outliter rejection

Definition at line 1225 of file ros_filter.cpp.

template<typename T >

bool RobotLocalization::RosFilter< T >::prepareAcceleration	(	const sensor_msgs::Imu::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		std::vector< int > &	updateVector,
		Eigen::VectorXd &	measurement,
		Eigen::MatrixXd &	measurementCovariance
	)		`[protected]`

Prepares an IMU message's linear acceleration for integration into the filter.

Parameters:

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

Definition at line 1882 of file ros_filter.cpp.

template<typename T >

bool RobotLocalization::RosFilter< T >::preparePose	(	const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		const bool	differential,
		const bool	relative,
		const bool	imuData,
		std::vector< int > &	updateVector,
		Eigen::VectorXd &	measurement,
		Eigen::MatrixXd &	measurementCovariance
	)		`[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
[in]	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 2011 of file ros_filter.cpp.

template<typename T >

bool RobotLocalization::RosFilter< T >::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
	)		`[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 2381 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::run ( )

Main run method.

Definition at line 1301 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::setPoseCallback ( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & msg )

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 1444 of file ros_filter.cpp.

template<typename T >

bool RobotLocalization::RosFilter< T >::setPoseSrvCallback	(	robot_localization::SetPose::Request &	request,
		robot_localization::SetPose::Response &
	)

Service callback for manually setting/resetting the internal pose estimate.

Parameters:

[in]	request	- Custom service request with pose information
[out]	response	- N/A

Returns:: boolean true if successful, false if not

Definition at line 1493 of file ros_filter.cpp.

template<typename T >

std::string RobotLocalization::RosFilter< T >::tfFailureReasonString ( const tf::FilterFailureReason reason )

Converts tf message filter failures to strings.

Parameters:

[in] reason - The failure reason object

Returns:: a string explanation of the failure

Definition at line 1504 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::transformImuFailureCallback	(	const sensor_msgs::Imu::ConstPtr &	msg,
		const tf::FilterFailureReason	reason,
		const std::string &	topicName,
		const std::string &	targetFrame
	)

Callback method for reporting failed IMU message transforms.

Parameters:

[in]	msg	- The ROS IMU message that failed
[in]	reason	- The reason for failure
[in]	topicName	- The name of the IMU topic
[in]	targetFrame	- The tf target frame into which we attempted to transform the message

Definition at line 1526 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::transformPoseFailureCallback	(	const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &	msg,
		const tf::FilterFailureReason	reason,
		const std::string &	topicName,
		const std::string &	targetFrame
	)

Callback method for reporting failed Pose message transforms.

Parameters:

[in]	msg	- The ROS stamped pose with covariance message that failed
[in]	reason	- The reason for failure
[in]	topicName	- The name of the pose topic
[in]	targetFrame	- The tf target frame into which we attempted to transform the message

Definition at line 1544 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::transformTwistFailureCallback	(	const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &	msg,
		const tf::FilterFailureReason	reason,
		const std::string &	topicName,
		const std::string &	targetFrame
	)

Callback method for reporting failed Twist message transforms.

Parameters:

[in]	msg	- The ROS stamped twist with covariance message that failed
[in]	reason	- The reason for failure
[in]	topicName	- The name of the twist topic
[in]	targetFrame	- The tf target frame into which we attempted to transform the message

Definition at line 1562 of file ros_filter.cpp.

template<typename T >

void RobotLocalization::RosFilter< T >::twistCallback	(	const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &	msg,
		const std::string &	topicName,
		const std::string &	targetFrame,
		const std::vector< int > &	updateVector,
		const double	mahalanobisThresh
	)

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
[in]	mahalanobisThresh	- Threshold, expressed as a Mahalanobis distance, for outliter rejection

Definition at line 1580 of file ros_filter.cpp.

Member Data Documentation

template<class T >

std::map<std::string, imuMFPtr> RobotLocalization::RosFilter< T >::accelerationMessageFilters_ [protected]

Vector to hold our acceleration (represented as IMU) message filters so they don't go out of scope.

Definition at line 376 of file ros_filter.h.

template<class T >

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

tf frame name for the robot's body frame

Definition at line 380 of file ros_filter.h.

template<class T >

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

Used for outputting debug messages.

Definition at line 398 of file ros_filter.h.

template<class T >

diagnostic_updater::Updater RobotLocalization::RosFilter< T >::diagnosticUpdater_ [protected]

Used for updating the diagnostics.

Definition at line 406 of file ros_filter.h.

template<class T >

int RobotLocalization::RosFilter< T >::dynamicDiagErrorLevel_ [protected]

The max (worst) dynamic diagnostic level.

Definition at line 402 of file ros_filter.h.

template<class T >

std::map<std::string, std::string> RobotLocalization::RosFilter< T >::dynamicDiagnostics_ [protected]

This object accumulates dynamic diagnostics, e.g., diagnostics relating to sensor data.

The values are considered transient and are cleared at every iteration.

Definition at line 394 of file ros_filter.h.

template<class T >

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

Our filter (EKF, UKF, etc.)

Definition at line 410 of file ros_filter.h.

template<class T >

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

The frequency of the run loop.

Definition at line 414 of file ros_filter.h.

template<class T >

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

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

Definition at line 419 of file ros_filter.h.

template<class T >

std::map<std::string, tf::Transform> RobotLocalization::RosFilter< T >::initialMeasurements_ [protected]

Stores the first measurement from each topic for relative measurements.

When a given sensor is being integrated in relative mode, its first measurement is effectively treated as an offset, and future measurements have this first measurement removed before they are fused. This variable stores the initial measurements. Note that this is different from using differential mode, as in differential mode, pose data is converted to twist data, resulting in boundless error growth for the variables being fused. With relative measurements, the vehicle will start with a 0 heading and position, but the measurements are still fused absolutely.

Definition at line 431 of file ros_filter.h.

template<class T >

std::map<std::string, ros::Time> RobotLocalization::RosFilter< T >::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 441 of file ros_filter.h.

template<class T >

ros::Time RobotLocalization::RosFilter< T >::lastSetPoseTime_ [protected]

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

If we receive a setPose message to reset the filter, we can get in strange situations wherein we process the pose reset, but then even after another spin cycle or two, we can get a new message with a time stamp that is *older* than the setPose message's time stamp. This means we have to check the message's time stamp against the lastSetPoseTime_.

Definition at line 450 of file ros_filter.h.

template<class T >

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

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

Definition at line 454 of file ros_filter.h.

template<class T >

MeasurementQueue RobotLocalization::RosFilter< T >::measurementQueue_ [protected]

We process measurements by queueing them up in callbacks and processing them all at once within each iteration.

Definition at line 460 of file ros_filter.h.

template<class T >

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

Node handle.

Definition at line 464 of file ros_filter.h.

template<class T >

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

Local node handle (for params)

Definition at line 468 of file ros_filter.h.

template<class T >

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

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

Definition at line 472 of file ros_filter.h.

template<class T >

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

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

Definition at line 476 of file ros_filter.h.

template<class T >

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

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

Definition at line 480 of file ros_filter.h.

template<class T >

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

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

Definition at line 484 of file ros_filter.h.

template<class T >

std::map<std::string, Eigen::MatrixXd> RobotLocalization::RosFilter< T >::previousMeasurementCovariances_ [protected]

We also need the previous covariance matrix for differential data.

Definition at line 498 of file ros_filter.h.

template<class T >

std::map<std::string, tf::Transform> RobotLocalization::RosFilter< T >::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 494 of file ros_filter.h.

template<class T >

bool RobotLocalization::RosFilter< T >::printDiagnostics_ [protected]

Whether or not we print diagnostic messages to the /diagnostics topic.

Definition at line 502 of file ros_filter.h.

template<class T >

std::map<std::string, bool> RobotLocalization::RosFilter< T >::removeGravitationalAcc_ [protected]

If including acceleration for each IMU input, whether or not we remove acceleration due to gravity.

Definition at line 506 of file ros_filter.h.

template<class T >

ros::ServiceServer RobotLocalization::RosFilter< T >::setPoseSrv_ [protected]

Service that allows another node to change the current state and recieve a confirmation. Uses a custom SetPose service.

Definition at line 516 of file ros_filter.h.

template<class T >

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

Subscribes to the set_pose topic (usually published from rviz). Message type is geometry_msgs/PoseWithCovarianceStamped.

Definition at line 511 of file ros_filter.h.

template<class T >

std::vector<std::string> RobotLocalization::RosFilter< T >::stateVariableNames_ [protected]

Contains the state vector variable names in string format.

Definition at line 520 of file ros_filter.h.

template<class T >

int RobotLocalization::RosFilter< T >::staticDiagErrorLevel_ [protected]

The max (worst) static diagnostic level.

Definition at line 524 of file ros_filter.h.

template<class T >

std::map<std::string, std::string> RobotLocalization::RosFilter< T >::staticDiagnostics_ [protected]

This object accumulates static diagnostics, e.g., diagnostics relating to the configuration parameters.

The values are treated as static and always reported (i.e., this object is never cleared)

Definition at line 387 of file ros_filter.h.

template<class T >

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

Transform listener for managing coordinate transforms.

Definition at line 528 of file ros_filter.h.

template<class T >

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

tf prefix

Definition at line 532 of file ros_filter.h.

template<class T >

ros::Duration RobotLocalization::RosFilter< T >::tfTimeOffset_ [protected]

For future (or past) dating the world_frame->base_link_frame transform.

Definition at line 536 of file ros_filter.h.

template<class T >

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

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

Definition at line 540 of file ros_filter.h.

template<class T >

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

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

Definition at line 545 of file ros_filter.h.

template<class T >

bool RobotLocalization::RosFilter< T >::twoDMode_ [protected]

Whether or not we're in 2D mode.

If this is true, the filter binds all 3D variables (Z, roll, pitch, and their respective velocities) to 0 for every measurement.

Definition at line 553 of file ros_filter.h.

template<class T >

geometry_msgs::TransformStamped RobotLocalization::RosFilter< T >::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 561 of file ros_filter.h.

template<class T >

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

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

Definition at line 565 of file ros_filter.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

template<class T> class RobotLocalization::RosFilter< T >

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

template<class T>
class RobotLocalization::RosFilter< T >