pr2_odometry.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <Eigen/Core>
00036 #include <Eigen/SVD>
00037 #include <nav_msgs/Odometry.h>
00038 #include <pr2_mechanism_controllers/Odometer.h>
00039 #include <realtime_tools/realtime_publisher.h>
00040 #include <tf/tfMessage.h>
00041 #include <tf/tf.h>
00042 #include <pr2_mechanism_controllers/base_kinematics.h>
00043 #include <angles/angles.h>
00044 
00045 #include <boost/scoped_ptr.hpp>
00046 #include <boost/thread/condition.hpp>
00047 
00048 #include <std_msgs/Float64.h>
00049 #include <pr2_mechanism_controllers/OdometryMatrix.h>
00050 #include <pr2_mechanism_controllers/DebugInfo.h>
00051 
00052 #include <std_msgs/Bool.h>
00053 #include <pr2_mechanism_controllers/BaseOdometryState.h>
00054 
00055 namespace controller
00056 {
00057 
00058 typedef Eigen::Matrix<float, 3, 1> OdomMatrix3x1;
00059 typedef Eigen::Matrix<float, 16, 1> OdomMatrix16x1;
00060 typedef Eigen::Matrix<float, 16, 3> OdomMatrix16x3;
00061 typedef Eigen::Matrix<float, 16, 16> OdomMatrix16x16;
00062 
00063 
00067   class Pr2Odometry : public pr2_controller_interface::Controller
00068   {
00069     public:
00070 
00074     Pr2Odometry();
00075 
00079     ~Pr2Odometry();
00080 
00087     bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node);
00088 
00089     /*
00090     * \brief  The starting method is called by the realtime thread just before
00091     * the first call to update.  Overrides Controller.staring().
00092     * @return Successful start
00093     */
00094     void starting();
00095 
00100     void update();
00101 
00105     void publish();
00106 
00110     void publishTransform();
00111 
00115     void publishOdometer();
00116 
00120     void publishState();
00121 
00122 
00123     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00124 
00125     private:
00126 
00127     ros::NodeHandle node_;
00128 
00132     BaseKinematics base_kin_;
00133 
00137     void updateOdometry();
00138 
00143     void getOdometryMessage(nav_msgs::Odometry &msg);
00144 
00154     void getOdometry(double &x, double &y, double &yaw, double &vx, double &vy, double &vw);
00155 
00161     void getOdometry(geometry_msgs::Point &odom, geometry_msgs::Twist &odom_vel);
00162 
00166     void computeBaseVelocity();
00167 
00172     double getCorrectedWheelSpeed(const int &index);
00173 
00177     OdomMatrix3x1 iterativeLeastSquares(const OdomMatrix16x3 &lhs, const OdomMatrix16x1 &rhs, const int &max_iter);
00178 
00182     OdomMatrix16x16 findWeightMatrix(const OdomMatrix16x1 &residual);
00183 
00187     double odometer_distance_;
00188 
00192     double odometer_angle_;
00193 
00197     ros::Time last_time_,current_time_;
00198 
00202     geometry_msgs::Point odom_;
00203 
00207     geometry_msgs::Twist odom_vel_;
00208 
00212     int ils_max_iterations_;
00213 
00217     double odometry_residual_max_;
00218 
00222     std::string odom_frame_;
00223 
00227     std::string base_link_frame_;
00228 
00232     std::string base_footprint_frame_;
00233 
00237     ros::Time last_publish_time_;
00238 
00242     ros::Time last_transform_publish_time_;
00243 
00247     ros::Time last_odometer_publish_time_;
00248 
00252     ros::Time last_state_publish_time_;
00253 
00257     double expected_publish_time_;
00258 
00262     double expected_odometer_publish_time_;
00263 
00267     double expected_state_publish_time_;
00268 
00272     boost::scoped_ptr<realtime_tools::RealtimePublisher <nav_msgs::Odometry> > odometry_publisher_ ;
00273 
00277     boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::Odometer> > odometer_publisher_ ;  
00278 
00282     boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::BaseOdometryState> > state_publisher_ ;  
00283 
00287     boost::scoped_ptr<realtime_tools::RealtimePublisher <tf::tfMessage> > transform_publisher_ ;
00288 
00289     double sigma_x_,sigma_y_,sigma_theta_,cov_x_y_,cov_x_theta_,cov_y_theta_;
00290 
00291     double base_link_floor_z_offset_;
00292 
00294     bool publish_tf_;
00295 
00296 
00300     void populateCovariance(const double &residual, nav_msgs::Odometry &msg);
00301 
00302     boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::OdometryMatrix> > matrix_publisher_;
00303     boost::scoped_ptr<realtime_tools::RealtimePublisher <pr2_mechanism_controllers::DebugInfo> > debug_publisher_;
00304 
00305     int sequence_;
00306 
00307     bool isInputValid();
00308 
00309     bool verbose_, publish_odom_, publish_odometer_, publish_state_;
00310 
00311     double odom_publish_rate_;
00312 
00313     double odometer_publish_rate_;
00314 
00315     double state_publish_rate_;
00316 
00317     double caster_calibration_multiplier_;
00318 
00319     OdomMatrix16x3  cbv_lhs_, fit_lhs_;
00320     OdomMatrix16x1  cbv_rhs_, fit_residual_, odometry_residual_;
00321     OdomMatrix16x1  fit_rhs_;
00322     OdomMatrix16x16 weight_matrix_, w_fit;
00323     OdomMatrix3x1   cbv_soln_, fit_soln_;
00324 
00325     std::string tf_prefix_;
00326 
00327   };
00328 }


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52