Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00091
00092
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 }