#include <pr2_odometry.h>

Inheritance diagram for controller::Pr2Odometry:

Public Member Functions
bool	init (pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
	Initializes and loads odometry information from the param server. More...

	Pr2Odometry ()
	Constructor for the odometry. More...

void	publish ()
	Publishes the currently computed odometry information. More...

void	publishOdometer ()
	Publishes the currently computed odometer information. More...

void	publishState ()
	Publishes the odometry state information. More...

void	publishTransform ()
	Publishes the currently computed odometry information to tf. More...

void	starting ()

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

	~Pr2Odometry ()
	Destructor for the odometry. More...

Public Member Functions inherited from pr2_controller_interface::Controller
	Controller ()

bool	getController (const std::string &name, int sched, ControllerType *&c)

bool	initRequest (ControllerProvider cp, pr2_mechanism_model::RobotState robot, ros::NodeHandle &n)

bool	isRunning ()

void	starting (const ros::Time &time)

bool	startRequest ()

void	stopping (const ros::Time &time)

virtual void	stopping ()

bool	stopRequest ()

void	update (const ros::Time &time, const ros::Duration &period)

void	updateRequest ()

virtual	~Controller ()

Private Member Functions
void	computeBaseVelocity ()
	Computes the base velocity from the caster positions and wheel speeds. More...

OdomMatrix16x16	findWeightMatrix (const OdomMatrix16x1 &residual)
	Finds the weight matrix from the iterative least squares residuals. More...

double	getCorrectedWheelSpeed (const int &index)
	Computes the wheel's speed adjusted for the attached caster's rotational velocity. More...

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. More...

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. More...

void	getOdometryMessage (nav_msgs::Odometry &msg)
	Builds the odometry message and prepares it for sending. More...

bool	isInputValid ()

OdomMatrix3x1	iterativeLeastSquares (const OdomMatrix16x3 &lhs, const OdomMatrix16x1 &rhs, const int &max_iter)
	Function used to compute the most likely solution to the odometry using iterative least squares. More...

void	populateCovariance (const double &residual, nav_msgs::Odometry &msg)
	populate the covariance part of the odometry message More...

void	updateOdometry ()
	Finds and stores the latest odometry information. More...

Private Attributes
std::string	base_footprint_frame_
	The topic name of the base footprint frame. More...

BaseKinematics	base_kin_
	class where the robot's information is computed and stored More...

double	base_link_floor_z_offset_

std::string	base_link_frame_
	The topic name of the base link frame. More...

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. More...

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

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

OdomMatrix16x3	fit_lhs_

OdomMatrix16x1	fit_residual_

OdomMatrix16x1	fit_rhs_

OdomMatrix3x1	fit_soln_

int	ils_max_iterations_
	Number of iterations used during iterative least squares. More...

ros::Time	last_odometer_publish_time_
	The last time the odometry information was published. More...

ros::Time	last_publish_time_
	The last time the odometry information was published. More...

ros::Time	last_state_publish_time_
	The last time the odometry information was published. More...

ros::Time	last_time_
	Stores the last update time and the current update time. More...

ros::Time	last_transform_publish_time_
	The last time the odometry information was published. More...

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_mechanism_controllers::OdometryMatrix > >	matrix_publisher_

ros::NodeHandle	node_

geometry_msgs::Point	odom_
	Point that stores the current translational position (x,y) and angular position (z) More...

std::string	odom_frame_
	The topic name of the published odometry. More...

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) More...

double	odometer_angle_
	Total angular distance traveled by the base as computed by the odometer. More...

double	odometer_distance_
	Total distance traveled by the base as computed by the odometer. More...

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. More...

boost::scoped_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > >	odometry_publisher_
	The RealtimePublisher that does the realtime publishing of the odometry. More...

OdomMatrix16x1	odometry_residual_

double	odometry_residual_max_
	Maximum residual from the iteritive least squares. More...

bool	publish_odom_

bool	publish_odometer_

bool	publish_state_

bool	publish_tf_
	enable or disable tf publishing More...

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. More...

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. More...

bool	verbose_

OdomMatrix16x16	w_fit

OdomMatrix16x16	weight_matrix_

Additional Inherited Members
Public Attributes inherited from pr2_controller_interface::Controller
std::vector< std::string >	after_list_

	AFTER_ME

std::vector< std::string >	before_list_

	BEFORE_ME

	CONSTRUCTED

	INITIALIZED

	RUNNING

enum pr2_controller_interface::Controller:: { ... }	state_

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

OdomMatrix16x16 controller::Pr2Odometry::findWeightMatrix ( const OdomMatrix16x1 & residual )

private

Finds the weight matrix from the iterative least squares residuals.

Definition at line 452 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 380 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 329 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 257 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 265 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 162 of file pr2_odometry.cpp.

OdomMatrix3x1 controller::Pr2Odometry::iterativeLeastSquares	(	const OdomMatrix16x3 &	lhs,
		const OdomMatrix16x1 &	rhs,
		const int &	max_iter
	)

private

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

Definition at line 392 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 284 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publish ( )

Publishes the currently computed odometry information.

Definition at line 499 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishOdometer ( )

Publishes the currently computed odometer information.

Definition at line 464 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishState ( )

Publishes the odometry state information.

Definition at line 477 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishTransform ( )

Publishes the currently computed odometry information to tf.

Definition at line 512 of file pr2_odometry.cpp.

void controller::Pr2Odometry::starting ( )

virtual

Reimplemented from pr2_controller_interface::Controller.

Definition at line 179 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 189 of file pr2_odometry.cpp.

void controller::Pr2Odometry::updateOdometry ( )

private

Finds and stores the latest odometry information.

Definition at line 231 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 232 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 291 of file pr2_odometry.h.

std::string controller::Pr2Odometry::base_link_frame_

private

The topic name of the base link frame.

Definition at line 227 of file pr2_odometry.h.

double controller::Pr2Odometry::caster_calibration_multiplier_

private

Definition at line 317 of file pr2_odometry.h.

OdomMatrix16x3 controller::Pr2Odometry::cbv_lhs_

private

Definition at line 319 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::cbv_rhs_

private

Definition at line 320 of file pr2_odometry.h.

OdomMatrix3x1 controller::Pr2Odometry::cbv_soln_

private

Definition at line 323 of file pr2_odometry.h.

double controller::Pr2Odometry::cov_x_theta_

private

Definition at line 289 of file pr2_odometry.h.

double controller::Pr2Odometry::cov_x_y_

private

Definition at line 289 of file pr2_odometry.h.

double controller::Pr2Odometry::cov_y_theta_

private

Definition at line 289 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 303 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 262 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 257 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 267 of file pr2_odometry.h.

OdomMatrix16x3 controller::Pr2Odometry::fit_lhs_

private

Definition at line 319 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::fit_residual_

private

Definition at line 320 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::fit_rhs_

private

Definition at line 321 of file pr2_odometry.h.

OdomMatrix3x1 controller::Pr2Odometry::fit_soln_

private

Definition at line 323 of file pr2_odometry.h.

int controller::Pr2Odometry::ils_max_iterations_

private

Number of iterations used during iterative least squares.

Definition at line 212 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 247 of file pr2_odometry.h.

ros::Time controller::Pr2Odometry::last_publish_time_

private

The last time the odometry information was published.

Definition at line 237 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 252 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 242 of file pr2_odometry.h.

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

private

Definition at line 302 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

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

Definition at line 202 of file pr2_odometry.h.

std::string controller::Pr2Odometry::odom_frame_

private

The topic name of the published odometry.

Definition at line 222 of file pr2_odometry.h.

double controller::Pr2Odometry::odom_publish_rate_

private

Definition at line 311 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 207 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 313 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 277 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 272 of file pr2_odometry.h.

OdomMatrix16x1 controller::Pr2Odometry::odometry_residual_

private

Definition at line 320 of file pr2_odometry.h.

double controller::Pr2Odometry::odometry_residual_max_

private

Maximum residual from the iteritive least squares.

Definition at line 217 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_odom_

private

Definition at line 309 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_odometer_

private

Definition at line 309 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_state_

private

Definition at line 309 of file pr2_odometry.h.

bool controller::Pr2Odometry::publish_tf_

private

enable or disable tf publishing

Definition at line 294 of file pr2_odometry.h.

int controller::Pr2Odometry::sequence_

private

Definition at line 305 of file pr2_odometry.h.

double controller::Pr2Odometry::sigma_theta_

private

Definition at line 289 of file pr2_odometry.h.

double controller::Pr2Odometry::sigma_x_

private

Definition at line 289 of file pr2_odometry.h.

double controller::Pr2Odometry::sigma_y_

private

Definition at line 289 of file pr2_odometry.h.

double controller::Pr2Odometry::state_publish_rate_

private

Definition at line 315 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 282 of file pr2_odometry.h.

std::string controller::Pr2Odometry::tf_prefix_

private

Definition at line 325 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 287 of file pr2_odometry.h.

bool controller::Pr2Odometry::verbose_

private

Definition at line 309 of file pr2_odometry.h.

OdomMatrix16x16 controller::Pr2Odometry::w_fit

private

Definition at line 322 of file pr2_odometry.h.

OdomMatrix16x16 controller::Pr2Odometry::weight_matrix_

private

Definition at line 322 of file pr2_odometry.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation