robot_localization: IMU not being used in sensor fusion

asked 2021-08-10 12:13:06 -0500

lucash gravatar image

updated 2021-08-11 12:37:21 -0500

Hello!

I've been trying to use the robot_localization package in order to position a person in relation to a robot by fusing UWB (ultra wideband - range measurements) and IMU data. This application is a little bit different than standard RL software because I'm trying to position a human in relation to the robot, instead of the robot in relation to the world.

The UWB data is of type PoseWithCovarianceStamped, and IMU is sensor_msgs/Imu. Currently my config file for the RL package is

odom_frame: base_link
base_link_frame: person
world_frame: base_link

two_d_mode: true

frequency: 50
sensor_timeout: 0.1

publish_tf: true

imu0: /imu
imu0_config: [false, false, false,
              true, true, true,
              false, false, false,
              true, true, true,
              true, true, true]
imu0_differential: false
imu0_queue_size: 10
imu0_relative: true
imu0_remove_gravitational_acceleration: true

pose0: /uwb
pose0_config: [true, true, true,
              false, false, false,
              false, false, false,
              false, false, false,
              false, false, false]
pose0_differential: false
pose0_queue_size: 10
pose0_relative: false

Note that odom_frame and world_frame are the base_link of the robot.

The issue is that the sensor fusion seems to be only using the UWB positioning. When I try to run the EKF node with only the IMU data, the odometry/filtered topic is never published. Any idea of what might be happening?

Thanks a lot! Please let me know if I should upload anything else.


Edit

Imu sample

> rostopic echo /imu
header:
  seq: 235
  stamp:
    secs: 1629816216
    nsecs: 723000000
  frame_id: "base_link"
orientation:
  x: -0.851288974285
  y: 0.500671029091
  z: -0.140116006136
  w: 0.0707325041294
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: -0.189139157534
  y: 0.289015620947
  z: 0.574671506882
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
  x: 2.56418991089
  y: -2.65038132668
  z: -9.51725101471
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

UWB sample

> rostopic echo /uwb
header:
  seq: 42
  stamp:
    secs: 1628540566
    nsecs: 118256092
  frame_id: "base_link"
pose:
  pose:
    position:
      x: 2.44
      y: 0.41
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [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.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

Thanks! Lucas

edit retag flag offensive close merge delete

Comments

Hi! In general, in other answers I read that the author of r_l asks for sample input messages.

jcremona gravatar image jcremona  ( 2021-08-10 12:45:42 -0500 )edit

To check that IMU messages are being merged, you could set debug: true and read the logs. You should note that when used in debug mode, performance is affected. You should just run it for a few seconds and look for something like Enqueued new measurement for imu0 or similar in the logs.

jcremona gravatar image jcremona  ( 2021-08-10 12:52:56 -0500 )edit

is your input IMU topic correct? I've normally seen it published as /imu/data rather than just /imu

sgermanserrano gravatar image sgermanserrano  ( 2021-08-10 13:46:11 -0500 )edit

Hey guys, I've edited the question according to your suggestions. I've tested with debug: true and it looks like the Enqueued new measurement for imu0 is being written to the logs.

lucash gravatar image lucash  ( 2021-08-10 14:11:56 -0500 )edit

Do you get any other debug messages like:

Received message that preceded the most recent pose reset. from this line.

which means it would skip the imu message and not initialize the filter until you have received some kind of pose message.

404RobotNotFound gravatar image 404RobotNotFound  ( 2021-08-10 19:05:59 -0500 )edit

Hi! I checked the logs and there is no line containing "Received".

lucash gravatar image lucash  ( 2021-08-11 12:34:55 -0500 )edit

Hi! How do you set your pose.orientation in the /uwb topic? Always set them [0, 0, 0, 1]?

HIT_LR gravatar image HIT_LR  ( 2021-12-08 02:51:13 -0500 )edit