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

Constructor & Destructor Documentation

◆ Pr2Odometry()

controller::Pr2Odometry::Pr2Odometry ( )

Constructor for the odometry.

Definition at line 49 of file pr2_odometry.cpp.

◆ ~Pr2Odometry()

controller::Pr2Odometry::~Pr2Odometry ( )

Destructor for the odometry.

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

◆ isInputValid()

bool controller::Pr2Odometry::isInputValid ( )

private

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

◆ publish()

void controller::Pr2Odometry::publish ( )

Publishes the currently computed odometry information.

Definition at line 499 of file pr2_odometry.cpp.

◆ publishOdometer()

void controller::Pr2Odometry::publishOdometer ( )

Publishes the currently computed odometer information.

Definition at line 464 of file pr2_odometry.cpp.

◆ publishState()

void controller::Pr2Odometry::publishState ( )

Publishes the odometry state information.

Definition at line 477 of file pr2_odometry.cpp.

◆ publishTransform()

void controller::Pr2Odometry::publishTransform ( )

Publishes the currently computed odometry information to tf.

Definition at line 512 of file pr2_odometry.cpp.

◆ starting()

void controller::Pr2Odometry::starting ( )

virtual

Reimplemented from pr2_controller_interface::Controller.

Definition at line 179 of file pr2_odometry.cpp.

◆ update()

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.

◆ updateOdometry()

void controller::Pr2Odometry::updateOdometry ( )

private

Finds and stores the latest odometry information.

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

◆ base_link_floor_z_offset_

double controller::Pr2Odometry::base_link_floor_z_offset_

private

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

◆ caster_calibration_multiplier_

double controller::Pr2Odometry::caster_calibration_multiplier_

private

Definition at line 316 of file pr2_odometry.h.

◆ cbv_lhs_

OdomMatrix16x3 controller::Pr2Odometry::cbv_lhs_

private

Definition at line 318 of file pr2_odometry.h.

◆ cbv_rhs_

OdomMatrix16x1 controller::Pr2Odometry::cbv_rhs_

private

Definition at line 319 of file pr2_odometry.h.

◆ cbv_soln_

OdomMatrix3x1 controller::Pr2Odometry::cbv_soln_

private

Definition at line 322 of file pr2_odometry.h.

◆ cov_x_theta_

double controller::Pr2Odometry::cov_x_theta_

private

Definition at line 288 of file pr2_odometry.h.

◆ cov_x_y_

double controller::Pr2Odometry::cov_x_y_

private

Definition at line 288 of file pr2_odometry.h.

◆ cov_y_theta_

double controller::Pr2Odometry::cov_y_theta_

private

Definition at line 288 of file pr2_odometry.h.

◆ current_time_

ros::Time controller::Pr2Odometry::current_time_

private

Definition at line 196 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 302 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 261 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 256 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 266 of file pr2_odometry.h.

◆ fit_lhs_

OdomMatrix16x3 controller::Pr2Odometry::fit_lhs_

private

Definition at line 318 of file pr2_odometry.h.

◆ fit_residual_

OdomMatrix16x1 controller::Pr2Odometry::fit_residual_

private

Definition at line 319 of file pr2_odometry.h.

◆ fit_rhs_

OdomMatrix16x1 controller::Pr2Odometry::fit_rhs_

private

Definition at line 320 of file pr2_odometry.h.

◆ fit_soln_

OdomMatrix3x1 controller::Pr2Odometry::fit_soln_

private

Definition at line 322 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 211 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 246 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 236 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 251 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 196 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 241 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 301 of file pr2_odometry.h.

◆ node_

ros::NodeHandle controller::Pr2Odometry::node_

private

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

◆ odom_frame_

std::string controller::Pr2Odometry::odom_frame_

private

The topic name of the published odometry.

Definition at line 221 of file pr2_odometry.h.

◆ odom_publish_rate_

double controller::Pr2Odometry::odom_publish_rate_

private

Definition at line 310 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 206 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 191 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 186 of file pr2_odometry.h.

◆ odometer_publish_rate_

double controller::Pr2Odometry::odometer_publish_rate_

private

Definition at line 312 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 276 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 271 of file pr2_odometry.h.

◆ odometry_residual_

OdomMatrix16x1 controller::Pr2Odometry::odometry_residual_

private

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

◆ publish_odom_

bool controller::Pr2Odometry::publish_odom_

private

Definition at line 308 of file pr2_odometry.h.

◆ publish_odometer_

bool controller::Pr2Odometry::publish_odometer_

private

Definition at line 308 of file pr2_odometry.h.

◆ publish_state_

bool controller::Pr2Odometry::publish_state_

private

Definition at line 308 of file pr2_odometry.h.

◆ publish_tf_

bool controller::Pr2Odometry::publish_tf_

private

enable or disable tf publishing

Definition at line 293 of file pr2_odometry.h.

◆ sequence_

int controller::Pr2Odometry::sequence_

private

Definition at line 304 of file pr2_odometry.h.

◆ sigma_theta_

double controller::Pr2Odometry::sigma_theta_

private

Definition at line 288 of file pr2_odometry.h.

◆ sigma_x_

double controller::Pr2Odometry::sigma_x_

private

Definition at line 288 of file pr2_odometry.h.

◆ sigma_y_

double controller::Pr2Odometry::sigma_y_

private

Definition at line 288 of file pr2_odometry.h.

◆ state_publish_rate_

double controller::Pr2Odometry::state_publish_rate_

private

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

◆ tf_prefix_

std::string controller::Pr2Odometry::tf_prefix_

private

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

◆ verbose_

bool controller::Pr2Odometry::verbose_

private

Definition at line 308 of file pr2_odometry.h.

◆ w_fit

OdomMatrix16x16 controller::Pr2Odometry::w_fit

private

Definition at line 321 of file pr2_odometry.h.

◆ weight_matrix_

OdomMatrix16x16 controller::Pr2Odometry::weight_matrix_

private

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