dead_reckoning.h
Go to the documentation of this file.
00001 
00026 #include "nav_msgs/Odometry.h"
00027 #include "grizzly_msgs/Drive.h"
00028 #include <grizzly_msgs/eigen.h>
00029 #include "sensor_msgs/JointState.h"
00030 
00031 #include <Eigen/Core>
00032 #include <Eigen/Dense>
00033 
00034 const Eigen::MatrixXd ODOM_POSE_COVAR_MOTION = (Eigen::MatrixXd(6, 6) << 
00035   1e-3, 0,    0,   0,   0,   0, 
00036   0,    1e-1, 0,   0,   0,   0,
00037   0,    0,    1e6, 0,   0,   0,
00038   0,    0,    0,   1e6, 0,   0,
00039   0,    0,    0,   0,   1e6, 0,
00040   0,    0,    0,   0,   0,   0.174).finished();
00041 
00042 const Eigen::MatrixXd ODOM_POSE_COVAR_NOMOVE = (Eigen::MatrixXd(6, 6) << 
00043   1e-9, 0,    0,   0,   0,   0, 
00044   0,    1e-9, 0,   0,   0,   0,
00045   0,    0,    1e6, 0,   0,   0,
00046   0,    0,    0,   1e6, 0,   0,
00047   0,    0,    0,   0,   1e6, 0,
00048   0,    0,    0,   0,   0,   1e-9).finished();
00049 
00050 const Eigen::MatrixXd ODOM_TWIST_COVAR_MOTION = (Eigen::MatrixXd(6, 6) << 
00051   1e-3, 0,    0,   0,   0,   0, 
00052   0,    1e-1, 0,   0,   0,   0,
00053   0,    0,    1e6, 0,   0,   0,
00054   0,    0,    0,   1e6, 0,   0,
00055   0,    0,    0,   0,   1e6, 0,
00056   0,    0,    0,   0,   0,   0.174).finished();
00057 
00058 const Eigen::MatrixXd ODOM_TWIST_COVAR_NOMOVE = (Eigen::MatrixXd(6, 6) << 
00059   1e-9, 0,    0,   0,   0,   0, 
00060   0,    1e-9, 0,   0,   0,   0,
00061   0,    0,    1e6, 0,   0,   0,
00062   0,    0,    0,   1e6, 0,   0,
00063   0,    0,    0,   0,   1e6, 0,
00064   0,    0,    0,   0,   0,   1e-9).finished();
00065 
00066 
00067 class DeadReckoning
00068 {
00069 public:
00070   DeadReckoning(double vehicle_width, double wheel_radius) 
00071   : max_dt_(0.1), width_(vehicle_width), radius_(wheel_radius), initialize(false)
00072   {
00073   }
00074 
00075   bool next(const grizzly_msgs::DriveConstPtr& encoders, nav_msgs::Odometry* odom, sensor_msgs::JointState* joints);
00076  
00077 protected:
00078   // Maintaining state between encoder updates.
00079   ros::Time last_time_;
00080   Eigen::Vector2f last_vels_;
00081 
00082   geometry_msgs::Point position_; 
00083   geometry_msgs::Twist twist_; 
00084   double yaw_;
00085   bool initialize;
00086   std::vector<double> last_joint_pos_;
00087 
00088   // Configuration
00089   double width_, radius_;
00090   ros::Duration max_dt_;
00091 };
00092 


grizzly_motion
Author(s):
autogenerated on Thu Jun 6 2019 21:44:02