dead_reckoning.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "tf/transform_datatypes.h"
#include "grizzly_motion/dead_reckoning.h"

using Eigen::Vector2f;

bool DeadReckoning::next(const grizzly_msgs::DriveConstPtr& encoders, nav_msgs::Odometry* odom)
{
  bool success = false;

  // Angular velocity per-side in rad/s, velocity per-size in m/s
  Vector2f avg_rotations((encoders->front_left + encoders->rear_left) / 2,
                         (encoders->front_right + encoders->rear_right) / 2);
  Vector2f vels = avg_rotations * radius_;

  ros::Duration dt = encoders->header.stamp - last_time_;
  if (dt <= max_dt_) {
    // Integrate position based on previous yaw and speed
     position_.x += cos(yaw_) * twist_.linear.x * dt.toSec();
     position_.y += sin(yaw_) * twist_.linear.x * dt.toSec();

    // Update heading by integrating previous angular velocity.
     yaw_ += twist_.angular.z * dt.toSec();

    // Update linear and angular velocity
    twist_.linear.x = vels.mean();
    twist_.angular.z = (vels[1] - vels[0]) / width_;
    
    // Timestamp from encoder message, set frames correctly.
    odom->header = encoders->header;
    odom->header.frame_id = "odom";
    odom->child_frame_id = "base_footprint"; 
    odom->pose.pose.position = position_;
    odom->pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_);
    odom->twist.twist = twist_;

    Eigen::Map<Eigen::MatrixXd> poseCov(odom->pose.covariance.data(), 6, 6);
    Eigen::Map<Eigen::MatrixXd> twistCov(odom->twist.covariance.data(), 6, 6);

    if(twist_.linear.x <= 1e-3 && twist_.angular.z <= 1e-3) {
      poseCov = ODOM_POSE_COVAR_NOMOVE;
      twistCov = ODOM_TWIST_COVAR_NOMOVE;
    } else {
      poseCov = ODOM_POSE_COVAR_MOTION;
      twistCov = ODOM_TWIST_COVAR_MOTION;
    }

    success = true;
  } else {
    static bool first = true;
    ROS_WARN_COND(first, "Gap between encoders messages is too large, no odom generated.");
    first = false;
  }

  last_time_ = encoders->header.stamp;
  last_vels_ = vels;
  return success;
}