robot_localization: IMU not being used in sensor fusion
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
Hi! In general, in other answers I read that the author of
r_l
asks for sample input messages.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 likeEnqueued new measurement for imu0
or similar in the logs.is your input IMU topic correct? I've normally seen it published as
/imu/data
rather than just/imu
Hey guys, I've edited the question according to your suggestions. I've tested with
debug: true
and it looks like theEnqueued new measurement for imu0
is being written to the logs.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.
Hi! I checked the logs and there is no line containing "Received".
Hi! How do you set your pose.orientation in the /uwb topic? Always set them [0, 0, 0, 1]?