ekf_localization node not responding

asked 2014-11-13 14:25:52 -0500

Raphael Nagel
15 ●2 ●4 ●7 https://github.com/compiaffe

updated 2014-11-17 05:53:56 -0500

I am trying to use the robot_localization package to fuse absolute attitude from an IMU, velocity in the world_frame from the IMU with a SONAR and pressure sensor (all aboard an AUV).

Following your tutorials I set up the launch file, but when I publish sensor data nothing gets fused at all. In fact I haven't seen a single message on topic /odometry/filtered.

I read all tutorials and everything on answers.ros.org but can't get my head around it. Any ideas? Thanks a lot, Rapha

The sensors frame_id is set to "odom". My launchfile looks like this:

 <launch>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <param name="frequency" value="100"/>  

  <param name="sensor_timeout" value="0.1"/>  

  <param name="two_d_mode" value="false"/>

  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="/odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="/odom"/>


  <param name="twist0" value="xsens/velocity"/>      
  <param name="pose0" value="example/pressure"/>
  <param name="pose1" value="example/sonar"/>      
  <param name="imu0" value="imu/data"/> 

  <rosparam param="twist0_config">[false, false, false, 
                                  false, false, false, 
                                  true,  true, true, 
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="pose0_config">[false,  false,  true, 
                                  false, false, false, 
                                  false, false, false, 
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="pose1_config">[true, true, false, 
                                 false,  false,  false, 
                                 false, false, false, 
                                 false,  false,  false,
                                 false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false, 
                                 true,  true,  true, 
                                 false, false, false, 
                                 false,  false,  false,
                                 true, true, true]</rosparam>                                     


  <param name="twist0_differential" value="false"/> 

  <param name="pose0_differential" value="true"/>

  <param name="pose1_differential" value="true"/>

  <param name="imu0_differential" value="true"/>
  <param name="remove_gravitational_acceleration" value="false"/>

  <param name="debug"           value="true"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0 ...

(more)

answered 2014-11-13 15:10:59 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2014-11-17 10:18:06 -0500

A few things:

No output at all only occurs when it's not receiving sensor data or it can't transform the data into the target frame. Do rostopic info on all your topics to make sure that they are being published and subscribed to.
Velocities should ideally be reported in the frame of the vehicle (base_link), but if they're not, the package will transform them into base_link anyway before fusing them with the state estimate. In this case, I'm guessing that it's attempting to transform your velocity message from odom->base_link, but can't, as the node that produces that transform is ekf_localization_node itself. It's a chicken-or-egg problem.
However, the odom->base_link transform should be getting generated if any one of the sensors is being successfully fused, so check (1) again, and perhaps post examples of your other sensor messages?

EDIT: Also, turn debug mode off unless you want to eat up disk space in a hurry. If you want to run your node for a few seconds with all of the sensor data coming in and then send me that log, that will help, but in general, don't run with that on.

EDIT in response to update. Forgive me if I go over anything you already know. I just want to make sure we're on the same page.

Q1: Let me back up a bit. First, in your system, we actually have three separate coordinate frames (at least with regard to this question, you probably have more):

odom
base_link
imu (which is equivalent to the ENU frame)

Just so we're clear, both your IMU's velocity message frame_id (as reported in xsens/velocity) and its orientation frame_id (as reported in imu/data) are now reported in the imu (i.e., ENU) frame, correct?

Assuming this is true, let's consider the velocity first. If your robot heads directly east, your IMU velocity sensor reads only +X velocity, and if it drives straight north, you get only +Y, correct?

(Incidentally, this would surprise me. If you're using accelerations, the velocities you generate, unless you transform them, are going to be in the body frame of the IMU, but not in the ENU frame. However, the rest of my answer assumes you did mean that the velocities are reported in the ENU frame. Apologies if I misunderstood.)

Assuming I'm still correct, then what you need is a transform from base_link->imu, which you said you have. Question: how did you generate this transform? It can't be static, as it will constantly change depending on the heading of the robot. For example, if you drive northeast, in the imu (ENU) frame, you'd get equal values for X and Y velocity. However, in base_link, you'd only have +X velocity. Now if you turned around and drove southwest, you'd have -X and -Y velocity in the imu frame, but would still have +X velocity in the base_link frame. Essentially, your ... (more)

edit flag offensive delete link

Comments

Hi Tom,

I have implemented your last suggestions and it does seem to work now. However, without integrating the next sensor (i.e. Position from Sonar) I cannot really investigate further - the position drifts a lot. I'll update here once that the Sonar is in place. Thank you for your help.