#include <rosie_odometry.h>

Inheritance diagram for controller::RosieOdometry:

Public Member Functions
bool	init (pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
	Initializes and loads odometry information from the param server.
bool	initXml (pr2_mechanism_model::RobotState robot_state, TiXmlElement config)
	Loads Odometry's information from the xml description file and param server.
void	publish ()
	Publishes the currently computed odometry information.
	RosieOdometry ()
	Constructor for the odometry.
void	starting ()
void	update ()
	(a) Updates positions of the caster and wheels. Called every timestep in realtime
	~RosieOdometry ()
	Destructor for the odometry.
Private Member Functions
void	computeBaseVelocity ()
	Computes the base velocity from the caster positions and wheel speeds.
Eigen::MatrixXf	findWeightMatrix (Eigen::MatrixXf residual, std::string weight_type)
	Finds the weight matrix from the iterative least squares residuals.
double	getCorrectedWheelSpeed (int index)
	Computes the wheel's speed adjusted for the attached caster's rotational velocity.
void	getOdometry (double &x, double &y, double &yaw, double &vx, double &vy, double &vw)
	Takes the current odometery information and stores it into the six double parameters.
void	getOdometry (geometry_msgs::Point &odom, geometry_msgs::Twist &odom_vel)
	Takes the current odometry information and stores it into the Point and Twist parameters.
void	getOdometryMessage (nav_msgs::Odometry &msg)
	Builds the odometry message and prepares it for sending.
Eigen::MatrixXf	iterativeLeastSquares (Eigen::MatrixXf lhs, Eigen::MatrixXf rhs, std::string weight_type, int max_iter)
	Function used to compute the most likely solution to the odometry using iterative least squares.
void	populateCovariance (double residual, nav_msgs::Odometry &msg)
	populate the covariance part of the odometry message
void	updateOdometry ()
	Finds and stores the latest odometry information.
Private Attributes
BaseKinematics	base_kin_
	class where the robot's information is computed and stored
std::string	base_link_frame_
	The topic name of the base link frame.
Eigen::MatrixXf	cbv_lhs_
Eigen::MatrixXf	cbv_rhs_
	Matricies used in the computation of the iterative least squares and related functions.
Eigen::MatrixXf	cbv_soln_
double	cov_x_theta_
double	cov_x_y_
double	cov_y_theta_
ros::Time	current_time_
double	expected_publish_time_
	The time that the odometry is expected to be published next.
Eigen::MatrixXf	fit_lhs_
Eigen::MatrixXf	fit_residual_
Eigen::MatrixXf	fit_rhs_
Eigen::MatrixXf	fit_soln_
int	ils_max_iterations_
	Number of iterations used during iterative least squares.
std::string	ils_weight_type_
	The type of weighting used in findWeightMatrix.
ros::Time	last_publish_time_
	The last time the odometry information was published.
ros::Time	last_time_
	Stores the last update time and the current update time.
ros::NodeHandle	node_
geometry_msgs::Point	odom_
	Point that stores the current translational position (x,y) and angular position (z)
std::string	odom_frame_
	The topic name of the published odometry.
geometry_msgs::Twist	odom_vel_
	Twist that remembers the current translational velocities (vel.vx, vel.vy) and angular position (ang_vel.vz)
double	odometer_angle_
	Total angular distance traveled by the base as computed by the odometer.
double	odometer_distance_
	Total distance traveled by the base as computed by the odometer.
boost::scoped_ptr < realtime_tools::RealtimePublisher < nav_msgs::Odometry > >	odometry_publisher_
	The RealtimePublisher that does the realtime publishing of the odometry.
Eigen::MatrixXf	odometry_residual_
double	odometry_residual_max_
	Maximum residual from the iteritive least squares.
double	sigma_theta_
double	sigma_x_
double	sigma_y_
boost::scoped_ptr < realtime_tools::RealtimePublisher < tf::tfMessage > >	transform_publisher_
	Publishes the transform between the odometry frame and the base frame.
Eigen::MatrixXf	weight_matrix_

Detailed Description

Definition at line 61 of file rosie_odometry.h.

Constructor & Destructor Documentation

controller::RosieOdometry::RosieOdometry ( )

Constructor for the odometry.

Definition at line 45 of file rosie_odometry.cpp.

controller::RosieOdometry::~RosieOdometry ( )

Destructor for the odometry.

Definition at line 49 of file rosie_odometry.cpp.

Member Function Documentation

void controller::RosieOdometry::computeBaseVelocity ( ) [private]

Computes the base velocity from the caster positions and wheel speeds.

Definition at line 219 of file rosie_odometry.cpp.

Eigen::MatrixXf controller::RosieOdometry::findWeightMatrix	(	Eigen::MatrixXf	residual,
		std::string	weight_type
	)		`[private]`

Finds the weight matrix from the iterative least squares residuals.

Definition at line 295 of file rosie_odometry.cpp.

double controller::RosieOdometry::getCorrectedWheelSpeed ( int index ) [private]

Computes the wheel's speed adjusted for the attached caster's rotational velocity.

Parameters:

index The index of the wheel

Definition at line 253 of file rosie_odometry.cpp.

void controller::RosieOdometry::getOdometry	(	double &	x,
		double &	y,
		double &	yaw,
		double &	vx,
		double &	vy,
		double &	vw
	)		`[private]`

Takes the current odometery information and stores it into the six double parameters.

Parameters:

x	X component of the current odom position
y	Y component of the current odom position
yaw	Yaw (theta) component of the current odom position
vx	X component of the current odom velocity
vy	Y component of the current odom velocity
vw	Angular velocity (omega) component of the current odom velocity

Definition at line 209 of file rosie_odometry.cpp.

void controller::RosieOdometry::getOdometry	(	geometry_msgs::Point &	odom,
		geometry_msgs::Twist &	odom_vel
	)		`[private]`

Takes the current odometry information and stores it into the Point and Twist parameters.

Parameters:

odom	Point into which the odometry position is placed (z is theta)
odom_vel	into which the odometry velocity is placed

Definition at line 143 of file rosie_odometry.cpp.

void controller::RosieOdometry::getOdometryMessage ( nav_msgs::Odometry & msg ) [private]

Builds the odometry message and prepares it for sending.

Parameters:

msg	The nav_msgs::Odometry into which the odometry values are placed

Definition at line 150 of file rosie_odometry.cpp.

bool controller::RosieOdometry::init	(	pr2_mechanism_model::RobotState *	robot_state,
		ros::NodeHandle &	node
	)		`[virtual]`

Initializes and loads odometry information from the param server.

Parameters:

robot_state	The robot's current state
config	Tiny xml element pointing to this controller

Returns:: Successful init

Implements pr2_controller_interface::Controller.

Definition at line 53 of file rosie_odometry.cpp.

bool controller::RosieOdometry::initXml	(	pr2_mechanism_model::RobotState *	robot_state,
		TiXmlElement *	config
	)

Loads Odometry's information from the xml description file and param server.

Parameters:

robot_state	The robot's current state
config	Tiny xml element pointing to this controller

Returns:: Successful init

Definition at line 101 of file rosie_odometry.cpp.

Eigen::MatrixXf controller::RosieOdometry::iterativeLeastSquares	(	Eigen::MatrixXf	lhs,
		Eigen::MatrixXf	rhs,
		std::string	weight_type,
		int	max_iter
	)		`[private]`