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)
 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 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.
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_
 Point that stores the current translational position (x,y) and angular position (z)
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 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:
indexThe 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:
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 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:
odomPoint into which the odometry position is placed (z is theta)
odom_velinto 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:
msgThe 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_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 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.

Publishes the currently computed odometry information.

Definition at line 499 of file pr2_odometry.cpp.

Publishes the currently computed odometer information.

Definition at line 464 of file pr2_odometry.cpp.

Publishes the odometry state information.

Definition at line 477 of file pr2_odometry.cpp.

Publishes the currently computed odometry information to tf.

Definition at line 512 of file pr2_odometry.cpp.

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.

Finds and stores the latest odometry information.

Definition at line 231 of file pr2_odometry.cpp.


Member Data Documentation

The topic name of the base footprint frame.

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

The topic name of the base link frame.

Definition at line 227 of file pr2_odometry.h.

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.

Definition at line 289 of file pr2_odometry.h.

Definition at line 289 of file pr2_odometry.h.

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

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

Definition at line 262 of file pr2_odometry.h.

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

Definition at line 257 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.

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.

Number of iterations used during iterative least squares.

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

The last time the odometry information was published.

Definition at line 252 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 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.

Definition at line 127 of file pr2_odometry.h.

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.

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.

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

Definition at line 320 of file pr2_odometry.h.

Maximum residual from the iteritive least squares.

Definition at line 217 of file pr2_odometry.h.

Definition at line 309 of file pr2_odometry.h.

Definition at line 309 of file pr2_odometry.h.

Definition at line 309 of file pr2_odometry.h.

enable or disable tf publishing

Definition at line 294 of file pr2_odometry.h.

Definition at line 305 of file pr2_odometry.h.

Definition at line 289 of file pr2_odometry.h.

Definition at line 289 of file pr2_odometry.h.

Definition at line 289 of file pr2_odometry.h.

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.

Publishes the transform between the odometry frame and the base frame.

Definition at line 287 of file pr2_odometry.h.

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:


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Sat Jun 8 2019 20:49:34