dead_reckoning.h

Go to the documentation of this file.

#include "nav_msgs/Odometry.h"
#include "grizzly_msgs/Drive.h"
#include <grizzly_msgs/eigen.h>
#include "sensor_msgs/JointState.h"

#include <Eigen/Core>
#include <Eigen/Dense>

const Eigen::MatrixXd ODOM_POSE_COVAR_MOTION = (Eigen::MatrixXd(6, 6) << 
  1e-3, 0,    0,   0,   0,   0, 
  0,    1e-1, 0,   0,   0,   0,
  0,    0,    1e6, 0,   0,   0,
  0,    0,    0,   1e6, 0,   0,
  0,    0,    0,   0,   1e6, 0,
  0,    0,    0,   0,   0,   0.174).finished();

const Eigen::MatrixXd ODOM_POSE_COVAR_NOMOVE = (Eigen::MatrixXd(6, 6) << 
  1e-9, 0,    0,   0,   0,   0, 
  0,    1e-9, 0,   0,   0,   0,
  0,    0,    1e6, 0,   0,   0,
  0,    0,    0,   1e6, 0,   0,
  0,    0,    0,   0,   1e6, 0,
  0,    0,    0,   0,   0,   1e-9).finished();

const Eigen::MatrixXd ODOM_TWIST_COVAR_MOTION = (Eigen::MatrixXd(6, 6) << 
  1e-3, 0,    0,   0,   0,   0, 
  0,    1e-1, 0,   0,   0,   0,
  0,    0,    1e6, 0,   0,   0,
  0,    0,    0,   1e6, 0,   0,
  0,    0,    0,   0,   1e6, 0,
  0,    0,    0,   0,   0,   0.174).finished();

const Eigen::MatrixXd ODOM_TWIST_COVAR_NOMOVE = (Eigen::MatrixXd(6, 6) << 
  1e-9, 0,    0,   0,   0,   0, 
  0,    1e-9, 0,   0,   0,   0,
  0,    0,    1e6, 0,   0,   0,
  0,    0,    0,   1e6, 0,   0,
  0,    0,    0,   0,   1e6, 0,
  0,    0,    0,   0,   0,   1e-9).finished();


class DeadReckoning
{
public:
  DeadReckoning(double vehicle_width, double wheel_radius) 
  : max_dt_(0.1), width_(vehicle_width), radius_(wheel_radius), initialize(false)
  {
  }

  bool next(const grizzly_msgs::DriveConstPtr& encoders, nav_msgs::Odometry* odom, sensor_msgs::JointState* joints);
 
protected:
  // Maintaining state between encoder updates.
  ros::Time last_time_;
  Eigen::Vector2f last_vels_;

  geometry_msgs::Point position_; 
  geometry_msgs::Twist twist_; 
  double yaw_;
  bool initialize;
  std::vector<double> last_joint_pos_;

  // Configuration
  double width_, radius_;
  ros::Duration max_dt_;
};