EKF rejects timestamp more than the cache length earlier than newest transform cache

edit

Hi,

Having a px4flow optical flow sensor, I want to convert its opt_flow_rad message into a TwistWithCovarianceStamped, which I can use in my EKF localization node.

However, the ekf node doesn't accept my twists. It warns (I configured my ros log to show nodes instead of time):

[ WARN] [/ekf_localization_node]: WARNING: failed to transform from /px4flow->base_link for twist0 message received at 3370.342710000. The timestamp on the message is more than the cache length earlier than the newest data in the transform cache.

The message conversion (optflow_odometry) works like this:

#include <ros/ros.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <px_comm/OpticalFlowRad.h>

ros::Publisher twist_publisher;    

void flow_callback (const px_comm::OpticalFlowRad::ConstPtr& opt_flow) {

    // Don't publish garbage data
    if(opt_flow->quality == 0){ return; }

    geometry_msgs::TwistWithCovarianceStamped twist;
    twist.header = opt_flow->header;

    // translation from optical flow, in m/s
    twist.twist.twist.linear.x = (opt_flow->integrated_x/opt_flow->integration_time_us)/opt_flow->distance;
    twist.twist.twist.linear.y = (opt_flow->integrated_y/opt_flow->integration_time_us)/opt_flow->distance;
    twist.twist.twist.linear.z = 0;

    // rotation from integrated gyro, in rad/s
    twist.twist.twist.angular.x = opt_flow->integrated_xgyro/opt_flow->integration_time_us;
    twist.twist.twist.angular.y = opt_flow->integrated_ygyro/opt_flow->integration_time_us;
    twist.twist.twist.angular.z = opt_flow->integrated_zgyro/opt_flow->integration_time_us;

    // Populate covariance matrix with uncertainty values
    twist.twist.covariance.assign(0.0); // We say that generally, our data is uncorrelated to each other
    // However, we have uncertainties for x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
    double uncertainty = pow(10, -1.0 * opt_flow->quality / (255.0/6.0));
    for (int i=0; i<36; i+=7)
        twist.twist.covariance[i] = uncertainty;
    twist_publisher.publish(twist);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "optflow_odometry");
    ros::NodeHandle n;
    ros::Subscriber flow_subscriber = n.subscribe("/px4flow/opt_flow_rad", 100, flow_callback);
    twist_publisher = n.advertise<geometry_msgs::TwistWithCovarianceStamped>("visual_odom", 50);
    ros::spin();
    return 0;
}

My launchfile looks like this:

<?xml version="1.0"?>
<launch>

  <!-- Optical Flow Sensor -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_px4flow" args="0.14 0 0 0 0 0 /base_link /px4flow 50" />
  <node name="px4flow" pkg="px4flow" type="px4flow_node" respawn="true" output="screen">
    <param name="serial_port" value="/dev/serial/by-id/usb-3D_Robotics_PX4Flow_v1.3_000000000000-if00"/>
  </node>
  <node pkg="tas_odometry" type="optflow_odometry" name="optflow_odometry" output="screen"/>

  <!-- EKF for odom->base_link -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" output="screen">
    <param name="map_frame" value="map"/>
    <param name="world_frame" value="odom"/>
    <param name="odom_frame" value="odom"/>
    <param name="base_link_frame" value="base_link"/>

    <param name="print_diagnostics" value="true"/>
    <param name="frequency" value="100"/>
    <param name="two_d_mode" value="true"/>

    <param name="twist0" value="/visual_odom"/>
    <rosparam param="twist0_config">
      [false, false, false, # x, y, z,
      false, false, false,  # roll, pitch, yaw,
      true,  true, false,   # vx, vy, vz, ---> vx, vy from optical flow
      false, false, true,   # vroll, vpitch, vyaw, ---> use px4flow gyro
      false, false, false]  # ax, ay, az
    </rosparam>
  </node> 

</launch>

Lastly, here are my TF Tree and Node Graph:

What do I have to change to get this working?

Cheers
Laurenz