controller::Pr2Odometry Class Reference

#include <pr2_odometry.h>

Inheritance diagram for controller::Pr2Odometry:

List of all members.

Public Member Functions
bool	init (pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
	Initializes and loads odometry information from the param server.
	Pr2Odometry ()
	Constructor for the odometry.
void	publish ()
	Publishes the currently computed odometry information.
void	publishOdometer ()
	Publishes the currently computed odometer information.
void	publishState ()
	Publishes the odometry state information.
void	publishTransform ()
	Publishes the currently computed odometry information to tf.
void	starting ()
void	update ()
	(a) Updates positions of the caster and wheels. Called every timestep in realtime
	~Pr2Odometry ()
	Destructor for the odometry.
Private Member Functions
void	computeBaseVelocity ()
	Computes the base velocity from the caster positions and wheel speeds.
OdomMatrix16x16	findWeightMatrix (const OdomMatrix16x1 &residual, const std::string &weight_type)
	Finds the weight matrix from the iterative least squares residuals.
double	getCorrectedWheelSpeed (const 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.
bool	isInputValid ()
OdomMatrix3x1	iterativeLeastSquares (const OdomMatrix16x3 &lhs, const OdomMatrix16x1 &rhs, const std::string &weight_type, const int &max_iter)
	Function used to compute the most likely solution to the odometry using iterative least squares.
void	populateCovariance (const 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
std::string	base_footprint_frame_
	The topic name of the base footprint frame.
BaseKinematics	base_kin_
	class where the robot's information is computed and stored
double	base_link_floor_z_offset_
std::string	base_link_frame_
	The topic name of the base link frame.
double	caster_calibration_multiplier_
OdomMatrix16x3	cbv_lhs_
OdomMatrix16x1	cbv_rhs_
OdomMatrix3x1	cbv_soln_
double	cov_x_theta_
double	cov_x_y_
double	cov_y_theta_
ros::Time	current_time_
boost::scoped_ptr < realtime_tools::RealtimePublisher < pr2_mechanism_controllers::DebugInfo > >	debug_publisher_
double	expected_odometer_publish_time_
	The time that the odometry is expected to be published next.
double	expected_publish_time_
	The time that the odometry is expected to be published next.
double	expected_state_publish_time_
	The time that the odometry is expected to be published next.
OdomMatrix16x3	fit_lhs_
OdomMatrix16x1	fit_residual_
OdomMatrix16x1	fit_rhs_
OdomMatrix3x1	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_odometer_publish_time_
	The last time the odometry information was published.
ros::Time	last_publish_time_
	The last time the odometry information was published.
ros::Time	last_state_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::Time	last_transform_publish_time_
	The last time the odometry information was published.
boost::scoped_ptr < realtime_tools::RealtimePublisher < pr2_mechanism_controllers::OdometryMatrix > >	matrix_publisher_
ros::NodeHandle	node_
geometry_msgs::Point	odom_
	Matricies used in the computation of the iterative least squares and related functions.
std::string	odom_frame_
	The topic name of the published odometry.
double	odom_publish_rate_
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.
double	odometer_publish_rate_
boost::scoped_ptr < realtime_tools::RealtimePublisher < pr2_mechanism_controllers::Odometer > >	odometer_publisher_
	The RealtimePublisher that does the realtime publishing of the odometry.
boost::scoped_ptr < realtime_tools::RealtimePublisher < nav_msgs::Odometry > >	odometry_publisher_
	The RealtimePublisher that does the realtime publishing of the odometry.
OdomMatrix16x1	odometry_residual_
double	odometry_residual_max_
	Maximum residual from the iteritive least squares.
bool	publish_odom_
bool	publish_odometer_
bool	publish_state_
bool	publish_tf_
	enable or disable tf publishing
int	sequence_
double	sigma_theta_
double	sigma_x_
double	sigma_y_
double	state_publish_rate_
boost::scoped_ptr < realtime_tools::RealtimePublisher < pr2_mechanism_controllers::BaseOdometryState > >	state_publisher_
	The RealtimePublisher that does the realtime publishing of the odometry state.
std::string	tf_prefix_
boost::scoped_ptr < realtime_tools::RealtimePublisher < tf::tfMessage > >	transform_publisher_
	Publishes the transform between the odometry frame and the base frame.
bool	verbose_
OdomMatrix16x16	w_fit
OdomMatrix16x16	weight_matrix_

---

Detailed Description

Definition at line 67 of file pr2_odometry.h.

---

Constructor & Destructor Documentation

controller::Pr2Odometry::Pr2Odometry

(

)

Constructor for the odometry.

Definition at line 49 of file pr2_odometry.cpp.

controller::Pr2Odometry::~Pr2Odometry

(

)

Destructor for the odometry.

Definition at line 54 of file pr2_odometry.cpp.

---

Member Function Documentation

void controller::Pr2Odometry::computeBaseVelocity

(

)

[private]

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

Definition at line 364 of file pr2_odometry.cpp.

OdomMatrix16x16 controller::Pr2Odometry::findWeightMatrix	(	const OdomMatrix16x1 &	residual,
		const std::string &	weight_type
	)		[private]

Finds the weight matrix from the iterative least squares residuals.

Definition at line 480 of file pr2_odometry.cpp.

double controller::Pr2Odometry::getCorrectedWheelSpeed

(

const 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 400 of file pr2_odometry.cpp.

void controller::Pr2Odometry::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 354 of file pr2_odometry.cpp.

void controller::Pr2Odometry::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 264 of file pr2_odometry.cpp.

void controller::Pr2Odometry::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 271 of file pr2_odometry.cpp.

bool controller::Pr2Odometry::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 58 of file pr2_odometry.cpp.

bool controller::Pr2Odometry::isInputValid

(

)

[private]

Definition at line 172 of file pr2_odometry.cpp.

OdomMatrix3x1 controller::Pr2Odometry::iterativeLeastSquares	(	const OdomMatrix16x3 &	lhs,
		const OdomMatrix16x1 &	rhs,
		const std::string &	weight_type,
		const int &	max_iter
	)		[private]

Function used to compute the most likely solution to the odometry using iterative least squares.

Definition at line 411 of file pr2_odometry.cpp.

void controller::Pr2Odometry::populateCovariance	(	const double &	residual,
		nav_msgs::Odometry &	msg
	)		[private]

populate the covariance part of the odometry message

Definition at line 293 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publish

(

)

Publishes the currently computed odometry information.

Definition at line 558 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishOdometer

(

)

Publishes the currently computed odometer information.

Definition at line 527 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishState

(

)

Publishes the odometry state information.

Definition at line 540 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishTransform

(

)

Publishes the currently computed odometry information to tf.

Definition at line 571 of file pr2_odometry.cpp.

void controller::Pr2Odometry::starting

(

)

[virtual]

Reimplemented from pr2_controller_interface::Controller.

Definition at line 187 of file pr2_odometry.cpp.

void controller::Pr2Odometry::update

(

void

)

[virtual]

(a) Updates positions of the caster and wheels. Called every timestep in realtime

Implements pr2_controller_interface::Controller.

Definition at line 197 of file pr2_odometry.cpp.

void controller::Pr2Odometry::updateOdometry

(

)

[private]

Finds and stores the latest odometry information.

Definition at line 241 of file pr2_odometry.cpp.

---

Member Data Documentation

std::string controller::Pr2Odometry::base_footprint_frame_ [private]

The topic name of the base footprint frame.

Definition at line 242 of file pr2_odometry.h.

BaseKinematics controller::Pr2Odometry::base_kin_ [private]

class where the robot's information is computed and stored

Definition at line 132 of file pr2_odometry.h.

double controller::Pr2Odometry::base_link_floor_z_offset_ [private]

Definition at line 301 of file pr2_odometry.h.

std::string controller::Pr2Odometry::base_link_frame_ [private]

The topic name of the base link frame.

Definition at line 237 of file pr2_odometry.h.

double controller::Pr2Odometry::caster_calibration_multiplier_ [private]

Definition at line 327 of file pr2_odometry.h.

OdomMatrix16x3 controller::Pr2Odometry::cbv_lhs_ [private]

Definition at line 329 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::cbv_rhs_ [private]

Definition at line 330 of file pr2_odometry.h.

OdomMatrix3x1 controller::Pr2Odometry::cbv_soln_ [private]

Definition at line 333 of file pr2_odometry.h.

double controller::Pr2Odometry::cov_x_theta_ [private]

Definition at line 299 of file pr2_odometry.h.

double controller::Pr2Odometry::cov_x_y_ [private]

Definition at line 299 of file pr2_odometry.h.

double controller::Pr2Odometry::cov_y_theta_ [private]

Definition at line 299 of file pr2_odometry.h.

ros::Time controller::Pr2Odometry::current_time_ [private]

Definition at line 197 of file pr2_odometry.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::DebugInfo> > controller::Pr2Odometry::debug_publisher_ [private]

Definition at line 313 of file pr2_odometry.h.

double controller::Pr2Odometry::expected_odometer_publish_time_ [private]

The time that the odometry is expected to be published next.

Definition at line 272 of file pr2_odometry.h.

double controller::Pr2Odometry::expected_publish_time_ [private]

The time that the odometry is expected to be published next.

Definition at line 267 of file pr2_odometry.h.

double controller::Pr2Odometry::expected_state_publish_time_ [private]

The time that the odometry is expected to be published next.

Definition at line 277 of file pr2_odometry.h.

OdomMatrix16x3 controller::Pr2Odometry::fit_lhs_ [private]

Definition at line 329 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::fit_residual_ [private]

Definition at line 330 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::fit_rhs_ [private]

Definition at line 331 of file pr2_odometry.h.

OdomMatrix3x1 controller::Pr2Odometry::fit_soln_ [private]

Definition at line 333 of file pr2_odometry.h.

int controller::Pr2Odometry::ils_max_iterations_ [private]

Number of iterations used during iterative least squares.

Definition at line 222 of file pr2_odometry.h.

std::string controller::Pr2Odometry::ils_weight_type_ [private]

The type of weighting used in findWeightMatrix.

Definition at line 217 of file pr2_odometry.h.

ros::Time controller::Pr2Odometry::last_odometer_publish_time_ [private]

The last time the odometry information was published.

Definition at line 257 of file pr2_odometry.h.

ros::Time controller::Pr2Odometry::last_publish_time_ [private]

The last time the odometry information was published.

Definition at line 247 of file pr2_odometry.h.

ros::Time controller::Pr2Odometry::last_state_publish_time_ [private]

The last time the odometry information was published.

Definition at line 262 of file pr2_odometry.h.

ros::Time controller::Pr2Odometry::last_time_ [private]

Stores the last update time and the current update time.

Definition at line 197 of file pr2_odometry.h.

ros::Time controller::Pr2Odometry::last_transform_publish_time_ [private]

The last time the odometry information was published.

Definition at line 252 of file pr2_odometry.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::OdometryMatrix> > controller::Pr2Odometry::matrix_publisher_ [private]

Definition at line 312 of file pr2_odometry.h.

ros::NodeHandle controller::Pr2Odometry::node_ [private]

Definition at line 127 of file pr2_odometry.h.

geometry_msgs::Point controller::Pr2Odometry::odom_ [private]

Matricies used in the computation of the iterative least squares and related functions.

Point that stores the current translational position (x,y) and angular position (z)

Definition at line 207 of file pr2_odometry.h.

std::string controller::Pr2Odometry::odom_frame_ [private]

The topic name of the published odometry.

Definition at line 232 of file pr2_odometry.h.

double controller::Pr2Odometry::odom_publish_rate_ [private]

Definition at line 321 of file pr2_odometry.h.

geometry_msgs::Twist controller::Pr2Odometry::odom_vel_ [private]

Twist that remembers the current translational velocities (vel.vx, vel.vy) and angular position (ang_vel.vz)

Definition at line 212 of file pr2_odometry.h.

double controller::Pr2Odometry::odometer_angle_ [private]

Total angular distance traveled by the base as computed by the odometer.

Definition at line 192 of file pr2_odometry.h.

double controller::Pr2Odometry::odometer_distance_ [private]

Total distance traveled by the base as computed by the odometer.

Definition at line 187 of file pr2_odometry.h.

double controller::Pr2Odometry::odometer_publish_rate_ [private]

Definition at line 323 of file pr2_odometry.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::Odometer> > controller::Pr2Odometry::odometer_publisher_ [private]

The RealtimePublisher that does the realtime publishing of the odometry.

Definition at line 287 of file pr2_odometry.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher <nav_msgs::Odometry> > controller::Pr2Odometry::odometry_publisher_ [private]

The RealtimePublisher that does the realtime publishing of the odometry.

Definition at line 282 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::odometry_residual_ [private]

Definition at line 330 of file pr2_odometry.h.

double controller::Pr2Odometry::odometry_residual_max_ [private]

Maximum residual from the iteritive least squares.

Definition at line 227 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_odom_ [private]

Definition at line 319 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_odometer_ [private]

Definition at line 319 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_state_ [private]

Definition at line 319 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_tf_ [private]

enable or disable tf publishing

Definition at line 304 of file pr2_odometry.h.

int controller::Pr2Odometry::sequence_ [private]

Definition at line 315 of file pr2_odometry.h.

double controller::Pr2Odometry::sigma_theta_ [private]

Definition at line 299 of file pr2_odometry.h.

double controller::Pr2Odometry::sigma_x_ [private]

Definition at line 299 of file pr2_odometry.h.

double controller::Pr2Odometry::sigma_y_ [private]

Definition at line 299 of file pr2_odometry.h.

double controller::Pr2Odometry::state_publish_rate_ [private]

Definition at line 325 of file pr2_odometry.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::BaseOdometryState> > controller::Pr2Odometry::state_publisher_ [private]

The RealtimePublisher that does the realtime publishing of the odometry state.

Definition at line 292 of file pr2_odometry.h.

std::string controller::Pr2Odometry::tf_prefix_ [private]

Definition at line 335 of file pr2_odometry.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher <tf::tfMessage> > controller::Pr2Odometry::transform_publisher_ [private]

Publishes the transform between the odometry frame and the base frame.

Definition at line 297 of file pr2_odometry.h.

bool controller::Pr2Odometry::verbose_ [private]

Definition at line 319 of file pr2_odometry.h.

OdomMatrix16x16 controller::Pr2Odometry::w_fit [private]

Definition at line 332 of file pr2_odometry.h.

OdomMatrix16x16 controller::Pr2Odometry::weight_matrix_ [private]

Definition at line 332 of file pr2_odometry.h.

---

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

• pr2_odometry.h
• pr2_odometry.cpp