Cartographer_ros: Error after activate `TRAJECTORY_BUILDER_2D.use_imu_data` for IMU integration

[ywiyogo]

I'm trying to integrate my LSM9D61 IMU sensor which is mounted under the Lidar sensor. I publish the topic to /imu and get such a result:

header: 
  seq: 143
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "imu_link"
orientation: 
  x: 0.052366103977
  y: 0.0604441389441
  z: -0.490040838718
  w: 0.86802315712
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0036357101053
  y: 0.0145206972957
  z: -0.0011967997998
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -6.94155586604e-05
  y: -7.89901184756e-05
  z: 0.00998626548529
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

My urdf joint is like this:

   <link name="imu_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <box size="0.01 0.008 0.004" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>


  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu_link" />
    <origin xyz="0 0 0" />
  </joint>

  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser" />
    <origin xyz="0.0 0 0.03" />
  </joint>

Then the lua configuration is changed like these line:

options = {
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  publish_frame_projected_to_2d = true,
  imu_sampling_ratio = 1.,
  ...
}
TRAJECTORY_BUILDER_2D.use_imu_data = true

After I start the roslaunch, I got such a warning that prevent the cartographer_ros to work smoothly:

[ INFO] [1605877770.851944410]: World frame: map 
[ INFO] [1605877770.852029539]: Lidar frame: base_link 
[ INFO] [1605877770.963107558]: I1120 14:09:30.000000 21220 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '637414745709617497'.
[ WARN] [1605877814.332847680]: W1120 14:10:14.000000 21220 ordered_multi_queue.cc:155] Queue waiting for data: (0, imu)
[ WARN] [1605877817.107229400]: W1120 14:10:17.000000 21220 ordered_multi_queue.cc:155] Queue waiting for data: (0, imu)

The warnings appears repeatly each 3 secs. If I set TRAJECTORY_BUILDER_2D.use_imu_data to false, the cartographer_ros works normally.
What's wrong with my configuration?

[johuber]

Did you make sure you are publishing and subscribing to the correct topic from IMU and in cartographer (see connections with rostopic info /imu) ?

Also check the rate of the message with rostopic hz /imu and maybe compare it to the rate of the lidar clouds!

[soorajanilkumar]

Did you make sure you are publishing and subscribing to the correct topic from IMU and in cartographer (see connections with rostopic info /imu) ?

Also check the rate of the message with rostopic hz /imu and maybe compare it to the rate of the lidar clouds!

Hi @ywiyogo ,

Apart from what @johuber has mentioned, I noticed that your linear acceleration field does not contain the gravity component.
This most probably is not the cause of your current issue, but might be one later.

Also if you have a rosbag, try cartographer_rosbag_validate which can weed out most issues in sensor data or frequency.

[soorajanilkumar]

Oh btw, I noticed that the timestamp header in your IMU msg is empty. Cartographer_ros uses the timestamp header. If these are empty or zero , you need to fix your imu driver to publish the ros::Time too.

[ywiyogo]

Did you make sure you are publishing and subscribing to the correct topic from IMU and in cartographer (see connections with rostopic info /imu) ?

Thanks @johuber for the fast reply!
The output of rostopic info /imu show that no component subscribes the topic. That is really a good analysis. However, why the cartographer doesn't subscribe? I thought the lua configuration will assign it.
This is the output:

Type: sensor_msgs/Imu

Publishers: 
 * /rtimulib_ros (http://pi:43681/)

Subscribers: None

Also check the rate of the message with rostopic hz /imu and maybe compare it to the rate of the lidar clouds!

rostopic hz /imu returns

subscribed to [/imu]
average rate: 9.997
    min: 0.099s max: 0.101s std dev: 0.00058s window: 10
average rate: 9.997
    min: 0.099s max: 0.101s std dev: 0.00043s window: 20
average rate: 9.999
    min: 0.099s max: 0.101s std dev: 0.00040s window: 30
average rate: 9.999
    min: 0.097s max: 0.102s std dev: 0.00067s window: 40
average rate: 9.999
    min: 0.097s max: 0.102s std dev: 0.00062s window: 50
average rate: 10.000
    min: 0.097s max: 0.102s std dev: 0.00056s window: 60
average rate: 9.999
    min: 0.097s max: 0.102s std dev: 0.00053s window: 70
average rate: 9.999
    min: 0.097s max: 0.102s std dev: 0.00050s window: 80
average rate: 10.000
    min: 0.097s max: 0.102s std dev: 0.00049s window: 90
average rate: 9.993
    min: 0.097s max: 0.106s std dev: 0.00079s window: 93

[johuber]

Did you make sure you are publishing and subscribing to the correct topic from IMU and in cartographer (see connections with rostopic info /imu) ?

Thanks @johuber for the fast reply!
The output of rostopic info /imu show that no component subscribes the topic. That is really a good analysis. However, why the cartographer doesn't subscribe? I thought the lua configuration will assign it.
This is the output:

Type: sensor_msgs/Imu

Publishers: 
 * /rtimulib_ros (http://pi:43681/)

Subscribers: None

Also check the rate of the message with rostopic hz /imu and maybe compare it to the rate of the lidar clouds!

rostopic hz /imu returns

subscribed to [/imu]
average rate: 9.997
  min: 0.099s max: 0.101s std dev: 0.00058s window: 10
average rate: 9.997
  min: 0.099s max: 0.101s std dev: 0.00043s window: 20
average rate: 9.999
  min: 0.099s max: 0.101s std dev: 0.00040s window: 30
average rate: 9.999
  min: 0.097s max: 0.102s std dev: 0.00067s window: 40
average rate: 9.999
  min: 0.097s max: 0.102s std dev: 0.00062s window: 50
average rate: 10.000
  min: 0.097s max: 0.102s std dev: 0.00056s window: 60
average rate: 9.999
  min: 0.097s max: 0.102s std dev: 0.00053s window: 70
average rate: 9.999
  min: 0.097s max: 0.102s std dev: 0.00050s window: 80
average rate: 10.000
  min: 0.097s max: 0.102s std dev: 0.00049s window: 90
average rate: 9.993
  min: 0.097s max: 0.106s std dev: 0.00079s window: 93

you can check with rosnode info /cartographer/cartographer_node which topics cartographer subscribes to (or your node may be called something else, check with rosnode list)

Also as @soorajanilkumar pointed out you also want add timestamps to your IMU message.

[ywiyogo]

@johuber huge thanks! I found the cause. The remapping <remap from="imu" to="/imu" />of the cartographer node in my launch file was missing!

[ywiyogo]

Also if you have a rosbag, try cartographer_rosbag_validate which can weed out most issues in sensor data or frequency.

@soorajanilkumar cartographer_rosbag_validate crashed on my robot:

cartographer_rosbag_validate -bag_filename carto-bag-validate
terminate called after throwing an instance of 'rosbag::BagIOException'
  what():  Error opening file: carto-bag-validate

[soorajanilkumar]

Also if you have a rosbag, try cartographer_rosbag_validate which can weed out most issues in sensor data or frequency.

@soorajanilkumar cartographer_rosbag_validate crashed on my robot:

cartographer_rosbag_validate -bag_filename carto-bag-validate
terminate called after throwing an instance of 'rosbag::BagIOException'
  what():  Error opening file: carto-bag-validate

Never seen that except when the rosbag is corrupt or the full path to the rosbag is not provided. Make sure you give the full path to the rosbag file if you are not in the specific directory.

Are you sure you are able to play your rosbag ?

[ywiyogo]

Apart from what @johuber has mentioned, I noticed that your linear acceleration field does not contain the gravity component.
This most probably is not the cause of your current issue, but might be one later.

Also if you have a rosbag, try cartographer_rosbag_validate which can weed out most issues in sensor data or frequency.

@soorajanilkumar could you share me the formula to calculate the gravity component? I took the implementation from https://github.com/romainreignier/rtimulib_ros/blob/master/src/rtimulib_ros.cpp, and there is no implementation for the gravity component.
Moreover, could you maybe understand that rtimulib_ros.cpp code, why the x and y are switched and the z axis is inverted? Is that because of the different orientation of the driver? This is what I see if I start the driver demo application for my LSM9DS1.

Are you sure you are able to play your rosbag ?

Yes, I've already recorded and played several bagfiles.

[soorajanilkumar]

@soorajanilkumar could you share me the formula to calculate the gravity component? I took the implementation from https://github.com/romainreignier/rtimulib_ros/blob/master/src/rtimulib_ros.cpp, and there is no implementation for the gravity component.

I have not looked at the driver. From what I understand your IMU should provide you with accelerometer values in one of these:

• linear acceleration + gravity
• linear acceleration (gravity removed)

When I checked the data sheet of LSM9DS1, I think it provides linear acceleration + gravity. In that case, since the output is in _g_ , you just need to multiply by 9.8* .

Moreover, could you maybe understand that rtimulib_ros.cpp code, why the x and y are switched and the z axis is inverted? Is that because of the different orientation of the driver? This is what I see if I start the driver demo application for my LSM9DS1.

I didn't have time to check yet, but you are right. The axis convention in the driver does not comply with ROS convention. You will have to check LSM9DS1 manual to see if it is possible to change axis convention to right handed orthogonal .