Why 'extrapolation into future' error using freenect on moving base ?
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 ...