robot_localization package ros
I am trying to use the robot localization package of ros to fuse the data coming from imu (on imu topic), wheel encoder (computed odometry on /odom topic) and a RTK GPS (differential gps on topic /odom_gps). The data coming from this differential gps is not lot/long coordinates. They are the relative position from the base station which is at a certain distance from the robot. Since this differential gps is giving data in the NED frame, I am publishing this data using odometry message. And all the positions and orientations are with respect to this base station frame.
Now since I have 2 odoms and 1 imu to fuse, I need 2 tf's, that is one from baselink to odom_gps and the other from baselink to odom. Only then it will be fused by the filter correctly. I am not able to do this since it gives warning/error of having 2 parents for 1 child (baselink).
Please can someone suggest the correct way to do this.
This is my config file:
#Configuation for robot odometry EKF
frequency: 30
two_d_mode: true
publish_tf: true
odom_frame: odom
base_link_frame: base_link
world_frame: odom
map_frame: map
#the odom0 configuration
odom0: /odom_gps
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom0_differential: false
# odom1 configuration
odom1: /odom
odom1_config: [true, true, false,
false, false, true,
true, true, false,
false, false, true,
false, false, false]
odom1_differential: true
# imu0 configuration
imu0: /imu
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true, #angular velocity about z
true, false, false] #linear accleration along x
imu0_differential: false
process_noise_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.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
I want the filter to rely more on gps position data and orientation can be from odom or imu.
This is the output when using odom0- odometry computed from wheel velocities, odom1- differential gps, imu Link to the output trajectory plot picture
When I try to just publish gps odom containing the veloctiy from wheel odometry , and have only one transform, from baselink to odom, then this is the output link to the output trajectory plot image
Odom
header:
seq: 1
stamp:
secs: 1686064458
nsecs: 662529468
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 0.8127841038800032
y: 1.9069460257000466
z: 0.0
orientation:
x: -0.0
y: 0.0
z: 0.08912760218623027
w: -0.9960202159236192
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 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, 1e-10]
twist:
twist:
linear:
x: 0.8031378416879796
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.10566960046983992
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]
RTK GPS data converted in odometry format
header:
seq: 1
stamp:
secs: 1686064575
nsecs: 672857046
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: -2.3454999923706055
y: 2.3854000568389893
z: 0.6060000061988831
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 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, 1e-10]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.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]
IMU
header:
seq: 862
stamp:
secs: 1685745231
nsecs: 436386762
frame_id: "imu"
orientation:
x: -0.01190185546875
y: 0.04925537109375
z: 0.90948486328125
w: 0.41259765625
orientation_covariance: [0.0010000000474974513, 0.0, 0.0, 0.0, 0.0010000000474974513, 0.0, 0.0, 0.0, 0.0010000000474974513]
angular_velocity:
x: -0.037109375
y: -0.31640625
z: 0.169921875
angular_velocity_covariance: [0.0010000000474974513, 0.0, 0.0, 0.0, 0.0010000000474974513, 0.0, 0.0, 0.0, 0.0010000000474974513]
linear_acceleration:
x: -0.0625
y: 0.66015625
z: -1.1484375
linear_acceleration_covariance: [0.0010000000474974513, 0.0, 0.0, 0.0, 0.0010000000474974513, 0.0, 0.0, 0.0, 0.0010000000474974513]
Asked by Siddharth Bhurat on 2023-05-25 14:39:59 UTC
Answers
I am more familiar with ROS2 but can perhaps be to some help. First about "warning/error of having 2 parents for 1 child (baselink)". It sounds like you have done it the wrong way around. Shouldn't the baslink be the parent and the other two the child. From /odom_gps you should probably only use x and y and not yaw. Do not know what type of robot you are using but if it is a differential odom1 should probably be velocity x and angular velocity. The colors in the plots makes no sense from a sensor perspective. The gps should be noisy but little drift and the encoder should be the opposite so something seems wrong.
Asked by ketill on 2023-05-25 18:43:43 UTC
Comments
I have updated the correct config file in the post above. But the problem I am still facing are the transforms. Because I have odometry coming from differential gps which is in the base station frame and the computed wheel odometry which is also in the base station frame. How will the tranform look like. Because there will be 2 transforms from odom and odom_gps to baselink. How to have the correct transforms ?
The colors are just denoting which sensor these x,y values are coming from. The gps is not that noisy because it is differential gps and so its not giving lat long coordinates but north, east, down (x, y, z respectively) values in the base station frame.
Asked by Siddharth Bhurat on 2023-05-26 10:24:03 UTC
I think it would be better to only use velocity estimation from odom1. Should not be necessary but I have experience better performance that way. Especially if they have different start up times. For example if the wheels are spinning for some time before the startup of the filter they could give a state estimation far of from the starting point. If instead velocity is used the previous states would not matter. About the frames: do you need two frames? The only frame you care about is from base_station - base_link? Then you just fuse the data in the filter.
Asked by ketill on 2023-05-26 16:35:56 UTC
The 2 parents for 1 child might be because you have 2 nodes broadcasting Odom->base_link and it won’t show in the tf tree. Do any of your odometry sources broadcast that tf? If so, turn it off it allows.
Asked by chased11 on 2023-05-26 17:27:56 UTC
@ketill, I tried using just velocity estimates, but then there are more jumps between odom and gps_odom. When I use positions from both, then the jumps are less. The plots for both:
with position and velocity from odom1
And for the transforms, I will have odom to baselink, odom_gps to baselink. This might cause that warning of redundant transforms and having multiple parents
Asked by Siddharth Bhurat on 2023-05-27 11:11:10 UTC
@chased11 yes I have 2 broadcasters, one from odom_gps to baselink, and other is from odom to baselink. I think if I don't have one of the transform, then the filter might not accept/use it right ?
Asked by Siddharth Bhurat on 2023-05-27 11:14:07 UTC
Are you shore the wheel odometry is correct. Are you using simulation or a real robot? It is important that the radius of the wheels and the distance between them is correct. On a real robot the number of tics from the encoder also must be correct.
Asked by ketill on 2023-05-28 05:52:18 UTC
I think it is correct, I have checked those dimension twice. It is a real robot. But whatever calculations I am doing are using wheel velocities and being transformed to basestation frame (both position and orientation). I have another question:
Since I am publishing one odometry using wheel velocities and other odometry from gps, is it fine if I set both the messages to have the frame id as odom and child as baselink. And the I dont publish any transform between these links, rather set the publish transform as true in the robot localization config file. Will that be fine ?
Asked by Siddharth Bhurat on 2023-05-28 22:50:30 UTC
Yes that should be fine. You only want the final pose estimation to come from robot_localization
Asked by ketill on 2023-05-30 02:12:12 UTC
Did that work? what does your tf tree look like? I think I usually had a gps_link with a static transform to base_link.
Asked by chased11 on 2023-05-30 17:39:21 UTC
Comments
Please edit your message and include a sample message from each sensor input.
Asked by Tom Moore on 2023-06-06 03:43:28 UTC
@tom moore , I have updated the details
Asked by Siddharth Bhurat on 2023-06-06 10:19:08 UTC
Sorry for the run-around, but I have one more request. Given that this site is migrating to robotics.stackexchange.com, would you mind asking this question again there and sharing the link?
Asked by Tom Moore on 2023-06-19 03:50:15 UTC