Ask Your Question
1

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

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

lalten gravatar image

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: image description image description

What do I have to change to get this working?

Cheers
Laurenz

edit retag flag offensive close merge delete

Comments

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

sloretz gravatar imagesloretz ( 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 imagelalten ( 2016-01-14 08:54:35 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

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

Comments

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 imagetfoote ( 2016-01-12 13:36:42 -0500 )edit

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

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

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

Seen: 154 times

Last updated: Jan 12 '16