Why 'extrapolation into future' error using freenect on moving base ?

asked 2016-09-29 17:46:16 -0600

elpidiovaldez gravatar image

Environment: ROS Kinetic on Lubuntu 16.04.1

I am setting up ROS navigation stack on my robot by following the tutorial.

I have built a node, called zen_base_node, which:

  • Publishes odometry as a Twist message and a transform.
  • Publishes the static transform from base_link to camera_link.
  • Subscribes to cmd_vel and sends appropriate commands to the robots differential drive.

I built a launch file which launches freenect and zen_base_node.

After launching, I can see the odometry information updating in rviz. I set /odom as the fixed frame and view pointcloud2 sent by freenect. The pointcloud visualisation gives an error saying:

For frame [camera_depth_optical_frame]: No transform to fixed frame [odom]. TF error: [Lookup would require extrapolation into the future. Requested time 1475183101.321405719 but the latest data is at time 1475183100.575132753, when looking up transform from frame [camera_depth_optical_frame] to frame [odom]]

After some googling I see I am not the first to see this problem, but I have not found a solution or explanation. I would appreciate some advice for resolving this problem. Since I have only being using ROS for a few days, I suspect that I am doing something wrong in the code for zen_base_node which will be pretty obvious to an experienced user.

Here is the transform tree from rviz:

Tree
  odom
    base_link
      camera_link
        camera_rgb_frame
          camera_rgb_optical_frame
        camera_depth_frame
          camera_depth_optical_frame

Here is the full code for zen_base_node:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>

double Vx  = 0.1;
double Vy  = -0.1;
double Vth = 0.1;

//*****************************************************************************************

void VelCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
  ROS_INFO("I heard Vel: linear[%lf %lf %lf]  angular[%lf %lf %lf]",
           msg->linear.x, msg->linear.y, msg->linear.z,
           msg->angular.x, msg->angular.y, msg->angular.z);

  Vx  = msg->linear.x;
  Vy  = msg->linear.y;
  Vth = msg->angular.z;
}

//*****************************************************************************************

int main(int argc, char** argv)
{
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster broadcaster;

  ros::Subscriber vel_sub = n.subscribe("cmd_vel", 50, VelCallback);

  double x  = 0.0;
  double y  = 0.0;
  double th = 0.0;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();

  ros::Rate r(1.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages
    current_time = ros::Time::now();

    //compute odometry in a typical way given the velocities of the robot
    double dt = (current_time - last_time).toSec();
    double delta_x  = (Vx * cos(th) - Vy * sin(th)) * dt;
    double delta_y  = (Vx * sin(th) + Vy * cos(th)) * dt;
    double delta_th = Vth * dt;

    x  += delta_x;
    y  += delta_y;
    th += delta_th;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation      = odom_quat;

    //send the odometry transform
    broadcaster.sendTransform(odom_trans);

    //now send the transform from base_link ...
(more)
edit retag flag offensive close merge delete