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