#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, 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_ |
Definition at line 67 of file pr2_odometry.h.
| controller::Pr2Odometry::Pr2Odometry | ( | ) |
Constructor for the odometry.
Definition at line 49 of file pr2_odometry.cpp.
| controller::Pr2Odometry::~Pr2Odometry | ( | ) |
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 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.
| index | The index of the wheel |
Definition at line 400 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 264 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 354 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 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.
| 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 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.
| void controller::Pr2Odometry::publish | ( | ) |
Publishes the currently computed odometry information.
Definition at line 558 of file pr2_odometry.cpp.
| void controller::Pr2Odometry::publishOdometer | ( | ) |
Publishes the currently computed odometer information.
Definition at line 527 of file pr2_odometry.cpp.
| void controller::Pr2Odometry::publishState | ( | ) |
Publishes the odometry state information.
Definition at line 540 of file pr2_odometry.cpp.
| void controller::Pr2Odometry::publishTransform | ( | ) |
Publishes the currently computed odometry information to tf.
Definition at line 571 of file pr2_odometry.cpp.
| void controller::Pr2Odometry::starting | ( | ) | [virtual] |
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.
| void controller::Pr2Odometry::updateOdometry | ( | ) | [private] |
Finds and stores the latest odometry information.
Definition at line 241 of file pr2_odometry.cpp.
std::string controller::Pr2Odometry::base_footprint_frame_ [private] |
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.
double controller::Pr2Odometry::base_link_floor_z_offset_ [private] |
Definition at line 301 of file pr2_odometry.h.
std::string controller::Pr2Odometry::base_link_frame_ [private] |
The topic name of the base link frame.
Definition at line 237 of file pr2_odometry.h.
double controller::Pr2Odometry::caster_calibration_multiplier_ [private] |
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.
double controller::Pr2Odometry::cov_x_theta_ [private] |
Definition at line 299 of file pr2_odometry.h.
double controller::Pr2Odometry::cov_x_y_ [private] |
Definition at line 299 of file pr2_odometry.h.
double controller::Pr2Odometry::cov_y_theta_ [private] |
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.
double controller::Pr2Odometry::expected_odometer_publish_time_ [private] |
The time that the odometry is expected to be published next.
Definition at line 272 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 267 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 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.
int controller::Pr2Odometry::ils_max_iterations_ [private] |
Number of iterations used during iterative least squares.
Definition at line 222 of file pr2_odometry.h.
std::string controller::Pr2Odometry::ils_weight_type_ [private] |
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.
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 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.
double controller::Pr2Odometry::odom_publish_rate_ [private] |
Definition at line 321 of file pr2_odometry.h.
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.
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 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.
OdomMatrix16x1 controller::Pr2Odometry::odometry_residual_ [private] |
Definition at line 330 of file pr2_odometry.h.
double controller::Pr2Odometry::odometry_residual_max_ [private] |
Maximum residual from the iteritive least squares.
Definition at line 227 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_odom_ [private] |
Definition at line 319 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_odometer_ [private] |
Definition at line 319 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_state_ [private] |
Definition at line 319 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_tf_ [private] |
enable or disable tf publishing
Definition at line 304 of file pr2_odometry.h.
int controller::Pr2Odometry::sequence_ [private] |
Definition at line 315 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_theta_ [private] |
Definition at line 299 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_x_ [private] |
Definition at line 299 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_y_ [private] |
Definition at line 299 of file pr2_odometry.h.
double controller::Pr2Odometry::state_publish_rate_ [private] |
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.
bool controller::Pr2Odometry::verbose_ [private] |
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.
