Public Member Functions | Private Member Functions | Private Attributes
controller::Pr2Odometry Class Reference

#include <pr2_odometry.h>

Inheritance diagram for controller::Pr2Odometry:
Inheritance graph
[legend]

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

Constructor for the odometry.

Definition at line 49 of file pr2_odometry.cpp.

Destructor for the odometry.

Definition at line 54 of file pr2_odometry.cpp.


Member Function Documentation

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:
indexThe 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:
xX component of the current odom position
yY component of the current odom position
yawYaw (theta) component of the current odom position
vxX component of the current odom velocity
vyY component of the current odom velocity
vwAngular 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:
odomPoint into which the odometry position is placed (z is theta)
odom_velinto 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:
msgThe 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_stateThe robot's current state
configTiny xml element pointing to this controller
Returns:
Successful init

Implements pr2_controller_interface::Controller.

Definition at line 58 of file pr2_odometry.cpp.

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.

Publishes the currently computed odometry information.

Definition at line 558 of file pr2_odometry.cpp.

Publishes the currently computed odometer information.

Definition at line 527 of file pr2_odometry.cpp.

Publishes the odometry state information.

Definition at line 540 of file pr2_odometry.cpp.

Publishes the currently computed odometry information to tf.

Definition at line 571 of file pr2_odometry.cpp.

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.

Finds and stores the latest odometry information.

Definition at line 241 of file pr2_odometry.cpp.


Member Data Documentation

The topic name of the base footprint frame.

Definition at line 242 of file pr2_odometry.h.

class where the robot's information is computed and stored

Definition at line 132 of file pr2_odometry.h.

Definition at line 301 of file pr2_odometry.h.

The topic name of the base link frame.

Definition at line 237 of file pr2_odometry.h.

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.

Definition at line 299 of file pr2_odometry.h.

Definition at line 299 of file pr2_odometry.h.

Definition at line 299 of file pr2_odometry.h.

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.

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

Definition at line 272 of file pr2_odometry.h.

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

Definition at line 267 of file pr2_odometry.h.

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.

Number of iterations used during iterative least squares.

Definition at line 222 of file pr2_odometry.h.

The type of weighting used in findWeightMatrix.

Definition at line 217 of file pr2_odometry.h.

The last time the odometry information was published.

Definition at line 257 of file pr2_odometry.h.

The last time the odometry information was published.

Definition at line 247 of file pr2_odometry.h.

The last time the odometry information was published.

Definition at line 262 of file pr2_odometry.h.

Stores the last update time and the current update time.

Definition at line 197 of file pr2_odometry.h.

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.

Definition at line 127 of file pr2_odometry.h.

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.

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.

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

Definition at line 192 of file pr2_odometry.h.

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

Definition at line 187 of file pr2_odometry.h.

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.

Definition at line 330 of file pr2_odometry.h.

Maximum residual from the iteritive least squares.

Definition at line 227 of file pr2_odometry.h.

Definition at line 319 of file pr2_odometry.h.

Definition at line 319 of file pr2_odometry.h.

Definition at line 319 of file pr2_odometry.h.

enable or disable tf publishing

Definition at line 304 of file pr2_odometry.h.

Definition at line 315 of file pr2_odometry.h.

Definition at line 299 of file pr2_odometry.h.

Definition at line 299 of file pr2_odometry.h.

Definition at line 299 of file pr2_odometry.h.

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.

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_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Apr 24 2014 15:44:51