#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 65 of file pr2_odometry.h.
| 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.
| 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.
| 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.
| 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.
| 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.
| 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.
| robot_state | The robot's current state | |
| config | Tiny xml element pointing to this controller |
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.
std::string controller::Pr2Odometry::base_footprint_frame_ [private] |
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.
double controller::Pr2Odometry::base_link_floor_z_offset_ [private] |
Definition at line 299 of file pr2_odometry.h.
std::string controller::Pr2Odometry::base_link_frame_ [private] |
The topic name of the base link frame.
Definition at line 235 of file pr2_odometry.h.
double controller::Pr2Odometry::caster_calibration_multiplier_ [private] |
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.
double controller::Pr2Odometry::cov_x_theta_ [private] |
Definition at line 297 of file pr2_odometry.h.
double controller::Pr2Odometry::cov_x_y_ [private] |
Definition at line 297 of file pr2_odometry.h.
double controller::Pr2Odometry::cov_y_theta_ [private] |
Definition at line 297 of file pr2_odometry.h.
ros::Time controller::Pr2Odometry::current_time_ [private] |
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.
double controller::Pr2Odometry::expected_odometer_publish_time_ [private] |
The time that the odometry is expected to be published next.
Definition at line 270 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 265 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 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.
int controller::Pr2Odometry::ils_max_iterations_ [private] |
Number of iterations used during iterative least squares.
Definition at line 220 of file pr2_odometry.h.
std::string controller::Pr2Odometry::ils_weight_type_ [private] |
The type of weighting used in findWeightMatrix.
Definition at line 215 of file pr2_odometry.h.
ros::Time controller::Pr2Odometry::last_odometer_publish_time_ [private] |
The last time the odometry information was published.
Definition at line 255 of file pr2_odometry.h.
ros::Time controller::Pr2Odometry::last_publish_time_ [private] |
The last time the odometry information was published.
Definition at line 245 of file pr2_odometry.h.
ros::Time controller::Pr2Odometry::last_state_publish_time_ [private] |
The last time the odometry information was published.
Definition at line 260 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 195 of file pr2_odometry.h.
ros::Time controller::Pr2Odometry::last_transform_publish_time_ [private] |
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.
double controller::Pr2Odometry::odom_publish_rate_ [private] |
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.
double controller::Pr2Odometry::odometer_angle_ [private] |
Total angular distance traveled by the base as computed by the odometer.
Definition at line 190 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 185 of file pr2_odometry.h.
double controller::Pr2Odometry::odometer_publish_rate_ [private] |
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.
double controller::Pr2Odometry::odometry_residual_max_ [private] |
Maximum residual from the iteritive least squares.
Definition at line 225 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_odom_ [private] |
Definition at line 317 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_odometer_ [private] |
Definition at line 317 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_state_ [private] |
Definition at line 317 of file pr2_odometry.h.
bool controller::Pr2Odometry::publish_tf_ [private] |
enable or disable tf publishing
Definition at line 302 of file pr2_odometry.h.
int controller::Pr2Odometry::sequence_ [private] |
Definition at line 313 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_theta_ [private] |
Definition at line 297 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_x_ [private] |
Definition at line 297 of file pr2_odometry.h.
double controller::Pr2Odometry::sigma_y_ [private] |
Definition at line 297 of file pr2_odometry.h.
double controller::Pr2Odometry::state_publish_rate_ [private] |
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.
bool controller::Pr2Odometry::verbose_ [private] |
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.
