0
$\begingroup$

Rosanswers logo

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.001*2 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: 
      seq: 0
      stamp: 
        secs: 1411672643
        nsecs: 305683335
      frame_id: head
    child_frame_id: imu
    ...

while the imu messages have stamps in the order of 6000. I highly appreciate your help.

Hernan


Originally posted by Hernan on ROS Answers with karma: 13 on 2014-11-25

Post score: 1


Original comments

Comment by Tom Moore on 2014-11-26:
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?

Comment by Hernan on 2014-11-26:
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.

Comment by Tom Moore on 2014-11-26:
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. :)

Comment by alme96 on 2020-11-06:
Hi Tom and Hernan! It seems that I'm having the very same problem as Hernan had: My imu topic gets ignored by the efk node. I read the answer from Tom but couldn't understand how Hernan solved it. How do you see that the IMU drive is stamping its message with a different time? How can you edit the source to fix it? I would love to upload a .bag file and a tf tree but I just registered so I have no rights to do so... Tell me if I should copy paste some messages. I would really appreciate (and need) your help. Kind regards, Alessandro.

Comment by Tom Moore on 2020-11-09:
Please post a new question. Thanks!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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?


Originally posted by Tom Moore with karma: 13689 on 2014-11-26

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.