#include <pr2_odometry.h>
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_ |
Definition at line 67 of file pr2_odometry.h.
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.
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.
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.
index | The 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.
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.
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.
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.
void controller::Pr2Odometry::getOdometryMessage | ( | nav_msgs::Odometry & | msg | ) | [private] |
Builds the odometry message and prepares it for sending.
msg | The 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.
robot_state | The robot's current state |
config | Tiny xml element pointing to this controller |
Implements pr2_controller_interface::Controller.
Definition at line 58 of file pr2_odometry.cpp.
bool controller::Pr2Odometry::isInputValid | ( | ) | [private] |
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.
void controller::Pr2Odometry::publish | ( | ) |
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.
void controller::Pr2Odometry::starting | ( | ) | [virtual] |
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.
void controller::Pr2Odometry::updateOdometry | ( | ) | [private] |
Finds and stores the latest odometry information.
Definition at line 231 of file pr2_odometry.cpp.
std::string controller::Pr2Odometry::base_footprint_frame_ [private] |
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.
double controller::Pr2Odometry::base_link_floor_z_offset_ [private] |
Definition at line 291 of file pr2_odometry.h.
std::string controller::Pr2Odometry::base_link_frame_ [private] |
The topic name of the base link frame.
Definition at line 227 of file pr2_odometry.h.
double controller::Pr2Odometry::caster_calibration_multiplier_ [private] |
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.
double controller::Pr2Odometry::cov_x_theta_ [private] |
Definition at line 289 of file pr2_odometry.h.
double controller::Pr2Odometry::cov_x_y_ [private] |
Definition at line 289 of file pr2_odometry.h.
double controller::Pr2Odometry::cov_y_theta_ [private] |
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.
double controller::Pr2Odometry::expected_odometer_publish_time_ [private] |
The time that the odometry is expected to be published next.
Definition at line 262 of file pr2_odometry.h.
double controller::Pr2Odometry::expected_publish_time_ [private] |
The time that the odometry is expected to be published next.
Definition at line 257 of file pr2_odometry.h.
double controller::Pr2Odometry::expected_state_publish_time_ [private] |
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.
int controller::Pr2Odometry::ils_max_iterations_ [private] |
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.
ros::Time controller::Pr2Odometry::last_time_ [private] |
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.
double controller::Pr2Odometry::odom_publish_rate_ [private] |
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.
double controller::Pr2Odometry::odometer_angle_ [private] |
Total angular distance traveled by the base as computed by the odometer.
Definition at line 192 of file pr2_odometry.h.
double controller::Pr2Odometry::odometer_distance_ [private] |
Total distance traveled by the base as computed by the odometer.
Definition at line 187 of file pr2_odometry.h.
double controller::Pr2Odometry::odometer_publish_rate_ [private] |
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.
OdomMatrix16x1 controller::Pr2Odometry::odometry_residual_ [private] |
Definition at line 320 of file pr2_odometry.h.
double controller::Pr2Odometry::odometry_residual_max_ [private] |
Maximum residual from the iteritive least squares.
Definition at line 217 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_odom_ [private] |
Definition at line 309 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_odometer_ [private] |
Definition at line 309 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_state_ [private] |
Definition at line 309 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_tf_ [private] |
enable or disable tf publishing
Definition at line 294 of file pr2_odometry.h.
int controller::Pr2Odometry::sequence_ [private] |
Definition at line 305 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_theta_ [private] |
Definition at line 289 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_x_ [private] |
Definition at line 289 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_y_ [private] |
Definition at line 289 of file pr2_odometry.h.
double controller::Pr2Odometry::state_publish_rate_ [private] |
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.
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 287 of file pr2_odometry.h.
bool controller::Pr2Odometry::verbose_ [private] |
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.