ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

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

asked 2016-01-12 07:31:59 -0500

lalten gravatar image


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;

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);
    return 0;

My launchfile looks like this:

<?xml version="1.0"?>

  <!-- 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 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


Lastly, here are my TF Tree and Node Graph: image description image description

What do I have to change to get this working?


edit retag flag offensive close merge delete


How did you come up with the formula converting uncertainty to covariance?

sloretz gravatar image sloretz  ( 2016-01-13 18:17:34 -0500 )edit

opt_flow->quality is 0 at min and 255 max. I wanted high uncertainty (~1) for low quality and low uncertainty for high quality (1e-6). The exponential curve puts emphasis on higher quality.

lalten gravatar image lalten  ( 2016-01-14 08:54:35 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2016-01-12 10:43:59 -0500

lalten gravatar image

It works when you change the header construction of the converted message. The timestamp must be reset and the leading / in the frame_id must be removed. I'm not sure changing the timestamps is a good idea, but it works like this.

// Build new message from old header
geometry_msgs::TwistWithCovarianceStamped twist;
twist.header = opt_flow->header;
twist.header.stamp = ros::Time::now(); // Otherwise the timestamp on the message is more than the cache length earlier than the newest data in the transform cache
if(twist.header.frame_id.substr(0,1) == "/")
twist.header.frame_id = twist.header.frame_id.erase(0,1); // Otherwise: error, because tf2 frame_ids cannot start with a '/'
edit flag offensive delete link more


The timestamp in the range of 3000 suggests that it's not time since epoch. Maybe time since startup. For a more accurate timestamp you need to calibrate the offset between the sensors timestamps and ros time, and then apply that correction instead of stamping it with the current time of receipt.

tfoote gravatar image tfoote  ( 2016-01-12 13:36:42 -0500 )edit

Good idea, that's probably the reason! Thanks :)

lalten gravatar image lalten  ( 2016-01-14 09:02:13 -0500 )edit

Question Tools



Asked: 2016-01-12 07:21:50 -0500

Seen: 458 times

Last updated: Jan 12 '16