Ask Your Question
0

robot_localization: imu ignored

asked 2014-11-25 18:14:04 -0500

Hernan gravatar image

updated 2014-11-26 09:42:04 -0500

Hi All,

I'm beginner with robot_localization and ROS in general so would greatly appreciate your help. I've configured the robot_localization launch file to receive an odometry and a imu. The sensor data is actually simulated, so I'm providing only a global position estimate on the odometry message:

  <param name="odom0" value="/head/fake_gps"/>
  <rosparam param="odom0_config">[true, true, true, 
                                  false, false, false, 
                                  false, false, false, 
                                  false, false, false]</rosparam>
  <param name="odom0_differential" value="false"/>

and only a positive yaw velocity and x acceleration on the imu:

  <param name="imu0" value="/head/can_imu"/>
  <rosparam param="imu0_config">[false, false, false, 
                                 false, false, false, 
                                 false, false, false, 
                                 false, false, true,
                                 true, false, false]</rosparam>
  <param name="imu0_differential" value="false"/>

The odometry message is indeed received by robot_localization and the filter initializes and track the position. My problem is that the imu0 is completely ignored. It is not incorporated into the filter, even when the debug output of robot_localization is indeed reporting the reception of the measurement:

----- /FilterBase::integrateMeasurements ------
------ RosFilter::imuCallback (imu0) ------
IMU message:
header: 
  seq: 174
  stamp: 6474.783371000
  frame_id: imu
orientation: 
....

Furthermore, I have the corresponding transformation:

<node pkg="tf" type="static_transform_publisher" name="vehicle_to_head" args="1.70 0 1.40 0 0 0 vehicle head 20" />
<node pkg="tf" type="static_transform_publisher" name="head_to_imu" args="0 0 0 0 0 0 head imu 20" />
<node pkg="tf" type="static_transform_publisher" name="head_to_fakegps" args="0 0 0 0 0 0 head fakegps 20" />

<node name="ekf_localization_node" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="vehicle"/>
  <param name="world_frame" value="odom"/> 
  ....

I know it is not integrating the imu data because no matter what values I assign to the yaw velocity or x acceleration, the filter output is always the same. The assigned covariances are in the order of 0.01*2 for rotation speed and 0.0012 for translation acceleration.

Does anyone can give me a hint of what I might be doing wrong?

I would highly appreciate any help.


EDIT 1: (11/26/2014)

  • I don't see any error or warning in the debug output.
  • There is no prepareTwist printed in the debug output.
  • This is a imu sample: header: seq: 2540 stamp: secs: 6478 nsecs: 433795000 frame_id: imu orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 orientation_covariance: [100.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 100.0] angular_velocity: x: 0.0 y: 0.0 z: 0.0872664611111 angular_velocity_covariance: [1.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001] linear_acceleration: x: 10.0 y: 0.0 z: 0.0 linear_acceleration_covariance: [0.002, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
  • I have tried to include only the imu and it is the same, ignored.

EDIT 2:

Yay, Tom, it seems that you are right. It is a problem with the tf.

transforms: 
  - 
    header ...
(more)
edit retag flag offensive close merge delete

Comments

1

Interesting! It looks like your IMU driver is stamping its messages with a different time. Can you edit that source to fix it? Also, if you are able (and you may not be, due to low karma), would you mind marking my answer as correct?

Tom Moore gravatar imageTom Moore ( 2014-11-26 12:18:40 -0500 )edit

Tom, I was able to fix the problem and now everything integrates. A suggestion: would it be possible to add the reason of integration failure in the debug output? In any case, thanks a lot.

Hernan gravatar imageHernan ( 2014-11-26 12:33:40 -0500 )edit

The error occurs within the message filter object, but it looks like I can add a failure callback that I can use to warn users. I'll add a new issue on the GitHub page. BTW, make sure you turn debug mode back off. It'll eat up disk space faster than you think. :)

Tom Moore gravatar imageTom Moore ( 2014-11-26 13:21:08 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-11-26 07:59:57 -0500

Tom Moore gravatar image

updated 2014-11-26 08:45:40 -0500

Can you post the debug output from when the sensor actually attempts to integrate the measurement? I see that it's being received, but there may be an error that tells you why it isn't integrating the measurement. Look for the reception of the message, then look for prepareTwist. Also, can you post a sample IMU message? Finally, have you tried only including the IMU and not the odometry?

EDIT: Also, just so I'm sure, all of this data is originating on the same machine, correct? I'm interested in the message's time stamp.

EDIT 2 (in response to update):

Any chance you can post a link to either a bag file (preferred) or the full debug output? I realize the latter grows quickly. If you go the bag file route, would you please post your entire launch file?

Also, a couple more things: do you see any reference to TwistCallback, where the message being printed out is again the IMU message? If not, then there is likely an issue with the tf tree. Internally, I break each IMU message up into a pose message and a twist message, and then send them off to their respective callbacks. I'm using tf message filters, and if tf can't transform the message from the message's frame_id to the target frame_id, then it doesn't even fire the callback. I see you have your tf static transforms publishing correctly, but could there be any re-parenting elsewhere in the tree? Any chance you can post the tf tree?

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-11-25 18:14:04 -0500

Seen: 905 times

Last updated: Nov 26 '14