#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 ()

virtual void	stopping ()

void	stopping (const ros::Time &time)

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 98 of file pr2_odometry.h.

Constructor & Destructor Documentation

◆ Pr2Odometry()

controller::Pr2Odometry::Pr2Odometry ( )

Constructor for the odometry.

Definition at line 81 of file pr2_odometry.cpp.

◆ ~Pr2Odometry()

controller::Pr2Odometry::~Pr2Odometry ( )

Destructor for the odometry.

Definition at line 86 of file pr2_odometry.cpp.

Member Function Documentation

◆ computeBaseVelocity()

void controller::Pr2Odometry::computeBaseVelocity ( )

private

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

Definition at line 372 of file pr2_odometry.cpp.

◆ findWeightMatrix()

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

private

Finds the weight matrix from the iterative least squares residuals.

Definition at line 484 of file pr2_odometry.cpp.

◆ getCorrectedWheelSpeed()

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

◆ getOdometry() [1/2]

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

◆ getOdometry() [2/2]

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

◆ getOdometryMessage()

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

◆ init()

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

◆ isInputValid()

bool controller::Pr2Odometry::isInputValid ( )

private

Definition at line 194 of file pr2_odometry.cpp.

◆ iterativeLeastSquares()

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

◆ populateCovariance()

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

private

populate the covariance part of the odometry message

Definition at line 316 of file pr2_odometry.cpp.

◆ publish()

void controller::Pr2Odometry::publish ( )

Publishes the currently computed odometry information.

Definition at line 531 of file pr2_odometry.cpp.

◆ publishOdometer()

void controller::Pr2Odometry::publishOdometer ( )

Publishes the currently computed odometer information.

Definition at line 496 of file pr2_odometry.cpp.

◆ publishState()

void controller::Pr2Odometry::publishState ( )

Publishes the odometry state information.

Definition at line 509 of file pr2_odometry.cpp.

◆ publishTransform()

void controller::Pr2Odometry::publishTransform ( )

Publishes the currently computed odometry information to tf.

Definition at line 544 of file pr2_odometry.cpp.

◆ starting()

void controller::Pr2Odometry::starting ( )

virtual

Reimplemented from pr2_controller_interface::Controller.

Definition at line 211 of file pr2_odometry.cpp.

◆ update()

void controller::Pr2Odometry::update ( )

virtual

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

Implements pr2_controller_interface::Controller.

Definition at line 221 of file pr2_odometry.cpp.

◆ updateOdometry()

void controller::Pr2Odometry::updateOdometry ( )

private

Finds and stores the latest odometry information.

Definition at line 263 of file pr2_odometry.cpp.

Member Data Documentation

◆ base_footprint_frame_

std::string controller::Pr2Odometry::base_footprint_frame_

private

The topic name of the base footprint frame.

Definition at line 263 of file pr2_odometry.h.

◆ base_kin_

BaseKinematics controller::Pr2Odometry::base_kin_

private

class where the robot's information is computed and stored

Definition at line 163 of file pr2_odometry.h.

◆ base_link_floor_z_offset_

double controller::Pr2Odometry::base_link_floor_z_offset_

private

Definition at line 322 of file pr2_odometry.h.

◆ base_link_frame_

std::string controller::Pr2Odometry::base_link_frame_

private

The topic name of the base link frame.

Definition at line 258 of file pr2_odometry.h.

◆ caster_calibration_multiplier_

double controller::Pr2Odometry::caster_calibration_multiplier_

private

Definition at line 348 of file pr2_odometry.h.

◆ cbv_lhs_

OdomMatrix16x3 controller::Pr2Odometry::cbv_lhs_

private

Definition at line 350 of file pr2_odometry.h.

◆ cbv_rhs_

OdomMatrix16x1 controller::Pr2Odometry::cbv_rhs_

private

Definition at line 351 of file pr2_odometry.h.

◆ cbv_soln_

OdomMatrix3x1 controller::Pr2Odometry::cbv_soln_

private

Definition at line 354 of file pr2_odometry.h.

◆ cov_x_theta_

double controller::Pr2Odometry::cov_x_theta_

private

Definition at line 320 of file pr2_odometry.h.

◆ cov_x_y_

double controller::Pr2Odometry::cov_x_y_

private

Definition at line 320 of file pr2_odometry.h.

◆ cov_y_theta_

double controller::Pr2Odometry::cov_y_theta_

private

Definition at line 320 of file pr2_odometry.h.

◆ current_time_

ros::Time controller::Pr2Odometry::current_time_

private

Definition at line 228 of file pr2_odometry.h.

◆ debug_publisher_

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

private

Definition at line 334 of file pr2_odometry.h.

◆ expected_odometer_publish_time_

double controller::Pr2Odometry::expected_odometer_publish_time_

private

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

Definition at line 293 of file pr2_odometry.h.

◆ expected_publish_time_

double controller::Pr2Odometry::expected_publish_time_

private

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

Definition at line 288 of file pr2_odometry.h.

◆ expected_state_publish_time_

double controller::Pr2Odometry::expected_state_publish_time_

private

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

Definition at line 298 of file pr2_odometry.h.

◆ fit_lhs_

OdomMatrix16x3 controller::Pr2Odometry::fit_lhs_

private

Definition at line 350 of file pr2_odometry.h.

◆ fit_residual_

OdomMatrix16x1 controller::Pr2Odometry::fit_residual_

private

Definition at line 351 of file pr2_odometry.h.

◆ fit_rhs_

OdomMatrix16x1 controller::Pr2Odometry::fit_rhs_

private

Definition at line 352 of file pr2_odometry.h.

◆ fit_soln_

OdomMatrix3x1 controller::Pr2Odometry::fit_soln_

private

Definition at line 354 of file pr2_odometry.h.

◆ ils_max_iterations_

int controller::Pr2Odometry::ils_max_iterations_

private

Number of iterations used during iterative least squares.

Definition at line 243 of file pr2_odometry.h.

◆ last_odometer_publish_time_

ros::Time controller::Pr2Odometry::last_odometer_publish_time_

private

The last time the odometry information was published.

Definition at line 278 of file pr2_odometry.h.

◆ last_publish_time_

ros::Time controller::Pr2Odometry::last_publish_time_

private

The last time the odometry information was published.

Definition at line 268 of file pr2_odometry.h.

◆ last_state_publish_time_

ros::Time controller::Pr2Odometry::last_state_publish_time_

private

The last time the odometry information was published.

Definition at line 283 of file pr2_odometry.h.

◆ last_time_

ros::Time controller::Pr2Odometry::last_time_

private

Stores the last update time and the current update time.

Definition at line 228 of file pr2_odometry.h.

◆ last_transform_publish_time_

ros::Time controller::Pr2Odometry::last_transform_publish_time_

private

The last time the odometry information was published.

Definition at line 273 of file pr2_odometry.h.

◆ matrix_publisher_

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

private

Definition at line 333 of file pr2_odometry.h.

◆ node_

ros::NodeHandle controller::Pr2Odometry::node_

private

Definition at line 158 of file pr2_odometry.h.

◆ odom_

geometry_msgs::Point controller::Pr2Odometry::odom_

private

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

Definition at line 233 of file pr2_odometry.h.

◆ odom_frame_

std::string controller::Pr2Odometry::odom_frame_

private

The topic name of the published odometry.

Definition at line 253 of file pr2_odometry.h.

◆ odom_publish_rate_

double controller::Pr2Odometry::odom_publish_rate_

private

Definition at line 342 of file pr2_odometry.h.

◆ odom_vel_

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 238 of file pr2_odometry.h.

◆ odometer_angle_

double controller::Pr2Odometry::odometer_angle_

private

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

Definition at line 223 of file pr2_odometry.h.

◆ odometer_distance_

double controller::Pr2Odometry::odometer_distance_

private

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

Definition at line 218 of file pr2_odometry.h.

◆ odometer_publish_rate_

double controller::Pr2Odometry::odometer_publish_rate_

private

Definition at line 344 of file pr2_odometry.h.

◆ odometer_publisher_

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 308 of file pr2_odometry.h.

◆ odometry_publisher_

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 303 of file pr2_odometry.h.

◆ odometry_residual_

OdomMatrix16x1 controller::Pr2Odometry::odometry_residual_

private

Definition at line 351 of file pr2_odometry.h.

◆ odometry_residual_max_

double controller::Pr2Odometry::odometry_residual_max_

private

Maximum residual from the iteritive least squares.

Definition at line 248 of file pr2_odometry.h.

◆ publish_odom_

bool controller::Pr2Odometry::publish_odom_

private

Definition at line 340 of file pr2_odometry.h.

◆ publish_odometer_

bool controller::Pr2Odometry::publish_odometer_

private

Definition at line 340 of file pr2_odometry.h.

◆ publish_state_

bool controller::Pr2Odometry::publish_state_

private

Definition at line 340 of file pr2_odometry.h.

◆ publish_tf_

bool controller::Pr2Odometry::publish_tf_

private

enable or disable tf publishing

Definition at line 325 of file pr2_odometry.h.

◆ sequence_

int controller::Pr2Odometry::sequence_

private

Definition at line 336 of file pr2_odometry.h.

◆ sigma_theta_

double controller::Pr2Odometry::sigma_theta_

private

Definition at line 320 of file pr2_odometry.h.

◆ sigma_x_

double controller::Pr2Odometry::sigma_x_

private

Definition at line 320 of file pr2_odometry.h.

◆ sigma_y_

double controller::Pr2Odometry::sigma_y_

private

Definition at line 320 of file pr2_odometry.h.

◆ state_publish_rate_

double controller::Pr2Odometry::state_publish_rate_

private

Definition at line 346 of file pr2_odometry.h.

◆ state_publisher_

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 313 of file pr2_odometry.h.

◆ tf_prefix_

std::string controller::Pr2Odometry::tf_prefix_

private

Definition at line 356 of file pr2_odometry.h.

◆ transform_publisher_

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 318 of file pr2_odometry.h.

◆ verbose_

bool controller::Pr2Odometry::verbose_

private

Definition at line 340 of file pr2_odometry.h.

◆ w_fit

OdomMatrix16x16 controller::Pr2Odometry::w_fit

private

Definition at line 353 of file pr2_odometry.h.

◆ weight_matrix_

OdomMatrix16x16 controller::Pr2Odometry::weight_matrix_

private

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

◆ Pr2Odometry()

◆ ~Pr2Odometry()

Member Function Documentation

◆ computeBaseVelocity()

◆ findWeightMatrix()

◆ getCorrectedWheelSpeed()

◆ getOdometry() [1/2]

◆ getOdometry() [2/2]

◆ getOdometryMessage()

◆ init()

◆ isInputValid()

◆ iterativeLeastSquares()

◆ populateCovariance()

◆ publish()

◆ publishOdometer()

◆ publishState()

◆ publishTransform()

◆ starting()

◆ update()

◆ updateOdometry()

Member Data Documentation

◆ base_footprint_frame_

◆ base_kin_

◆ base_link_floor_z_offset_

◆ base_link_frame_

◆ caster_calibration_multiplier_

◆ cbv_lhs_

◆ cbv_rhs_

◆ cbv_soln_

◆ cov_x_theta_

◆ cov_x_y_

◆ cov_y_theta_

◆ current_time_

◆ debug_publisher_

◆ expected_odometer_publish_time_

◆ expected_publish_time_

◆ expected_state_publish_time_

◆ fit_lhs_

◆ fit_residual_

◆ fit_rhs_

◆ fit_soln_

◆ ils_max_iterations_

◆ last_odometer_publish_time_

◆ last_publish_time_

◆ last_state_publish_time_

◆ last_time_

◆ last_transform_publish_time_

◆ matrix_publisher_

◆ node_

◆ odom_

◆ odom_frame_

◆ odom_publish_rate_

◆ odom_vel_

◆ odometer_angle_

◆ odometer_distance_

◆ odometer_publish_rate_

◆ odometer_publisher_

◆ odometry_publisher_

◆ odometry_residual_

◆ odometry_residual_max_

◆ publish_odom_

◆ publish_odometer_

◆ publish_state_

◆ publish_tf_

◆ sequence_

◆ sigma_theta_

◆ sigma_x_

◆ sigma_y_

◆ state_publish_rate_

◆ state_publisher_

◆ tf_prefix_

◆ transform_publisher_

◆ verbose_

◆ w_fit