controller::Pr2Odometry Class Reference

#include <pr2_odometry.h>

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 (geometry_msgs::Point &odom, geometry_msgs::Twist &odom_vel)
 Takes the current odometry information and stores it into the Point and Twist parameters.
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 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 65 of file pr2_odometry.h.


Constructor & Destructor Documentation

controller::Pr2Odometry::Pr2Odometry (  ) 

Constructor for the odometry.

Definition at line 45 of file pr2_odometry.cpp.

controller::Pr2Odometry::~Pr2Odometry (  ) 

Destructor for the odometry.

Definition at line 50 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 360 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 476 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 396 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 260 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 350 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 267 of file pr2_odometry.cpp.

bool controller::Pr2Odometry::init ( pr2_mechanism_model::RobotState *  robot_state,
ros::NodeHandle &  node 
)

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

Definition at line 54 of file pr2_odometry.cpp.

bool controller::Pr2Odometry::isInputValid (  )  [private]

Definition at line 168 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 407 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 289 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publish (  ) 

Publishes the currently computed odometry information.

Definition at line 554 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishOdometer (  ) 

Publishes the currently computed odometer information.

Definition at line 523 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishState (  ) 

Publishes the odometry state information.

Definition at line 536 of file pr2_odometry.cpp.

void controller::Pr2Odometry::publishTransform (  ) 

Publishes the currently computed odometry information to tf.

Definition at line 567 of file pr2_odometry.cpp.

void controller::Pr2Odometry::starting (  ) 

Definition at line 183 of file pr2_odometry.cpp.

void controller::Pr2Odometry::update (  ) 

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

Definition at line 193 of file pr2_odometry.cpp.

void controller::Pr2Odometry::updateOdometry (  )  [private]

Finds and stores the latest odometry information.

Definition at line 237 of file pr2_odometry.cpp.


Member Data Documentation

The topic name of the base footprint frame.

Definition at line 240 of file pr2_odometry.h.

class where the robot's information is computed and stored

Definition at line 130 of file pr2_odometry.h.

Definition at line 299 of file pr2_odometry.h.

The topic name of the base link frame.

Definition at line 235 of file pr2_odometry.h.

Definition at line 325 of file pr2_odometry.h.

Definition at line 327 of file pr2_odometry.h.

Definition at line 328 of file pr2_odometry.h.

Definition at line 331 of file pr2_odometry.h.

Definition at line 297 of file pr2_odometry.h.

Definition at line 297 of file pr2_odometry.h.

Definition at line 297 of file pr2_odometry.h.

Definition at line 195 of file pr2_odometry.h.

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

Definition at line 311 of file pr2_odometry.h.

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

Definition at line 270 of file pr2_odometry.h.

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

Definition at line 265 of file pr2_odometry.h.

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

Definition at line 275 of file pr2_odometry.h.

Definition at line 327 of file pr2_odometry.h.

Definition at line 328 of file pr2_odometry.h.

Definition at line 329 of file pr2_odometry.h.

Definition at line 331 of file pr2_odometry.h.

Number of iterations used during iterative least squares.

Definition at line 220 of file pr2_odometry.h.

The type of weighting used in findWeightMatrix.

Definition at line 215 of file pr2_odometry.h.

The last time the odometry information was published.

Definition at line 255 of file pr2_odometry.h.

The last time the odometry information was published.

Definition at line 245 of file pr2_odometry.h.

The last time the odometry information was published.

Definition at line 260 of file pr2_odometry.h.

Stores the last update time and the current update time.

Definition at line 195 of file pr2_odometry.h.

The last time the odometry information was published.

Definition at line 250 of file pr2_odometry.h.

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

Definition at line 310 of file pr2_odometry.h.

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

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

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

The topic name of the published odometry.

Definition at line 230 of file pr2_odometry.h.

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

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

Definition at line 190 of file pr2_odometry.h.

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

Definition at line 185 of file pr2_odometry.h.

Definition at line 321 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 285 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 280 of file pr2_odometry.h.

Definition at line 328 of file pr2_odometry.h.

Maximum residual from the iteritive least squares.

Definition at line 225 of file pr2_odometry.h.

Definition at line 317 of file pr2_odometry.h.

Definition at line 317 of file pr2_odometry.h.

Definition at line 317 of file pr2_odometry.h.

enable or disable tf publishing

Definition at line 302 of file pr2_odometry.h.

Definition at line 313 of file pr2_odometry.h.

Definition at line 297 of file pr2_odometry.h.

Definition at line 297 of file pr2_odometry.h.

Definition at line 297 of file pr2_odometry.h.

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

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

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

Definition at line 317 of file pr2_odometry.h.

Definition at line 330 of file pr2_odometry.h.

Definition at line 330 of file pr2_odometry.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Friends Defines


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Fri Jan 11 09:39:08 2013