robot_localization : Local cosmap drifts away from map
Good Afternoon All,
I am using robot_localization to fuse odometery and IMU data. But every time I send the robot to goal the local cosmap and robot footprint drifts off my map (static map).
Before sending robot to goal, I do a TF tree and see the map(AMCL)--->odom (EKF)--->baselink ---> base_laser and IMU FT tree. But after the drift my map frame disappears.
Based on the EKF documentation if I am using the package to fuse these, EKF would output /odometery/filtered/ and because I also enbled publish TF, EKF would also publish my odom to base_link transform. I also ensured that the node generating my odometery for input to EKF does not publish the odom to base_link transform, (obtained via calcuating chasis configuration, calculated from my chasis pose, from robot kinematics. Below are my EFK and AMCL config file. Any Idea why this is happening ?
AMCL Config
EKF Config
//silent_tf_failure: false
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.5
print_diagnostics: true
debug: false
debug_out_file: /home/ubuntu/robot_localizationfile.txt
publish_tf: true
publish_acceleration: false
initial_state: [ 1.0, -4.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0 0.0]
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified but if map specified USE MAP (Check AMCL)
odom0: /odom
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
imu0: /imu
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, true, false]
imu0_differential: false
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 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, 0.06, 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 ...
odom->base_link) is unusual.process_noise_varianceif theekf filteris not working. You could be making things worse.robot_localizationconfig a few months ago, and Tom helped you get it working. Is this the same robot? If so, what has changed since then that might affect this?Thanks Mike. See below. 1) I set the initial state to what I wanted the initial position on the map to be...should I make these 0? Its not the ideal starting location for my robot though.
2) Noted I will remove that until I get the filter working.
3) it is the same robot , and the drifting issue stops but I was getting very odd behavior from DWA, and after reviewing the TF I realized my odometry node, not EKF , was publishing odom to base transform, but I was using EKF to fuse odometry/imu . From the EKF document I am assuming if EKF is fusing odometry and IMU then navigation should use the TF published by EKF and not my odometry node..is that assumption correct?
Regarding (1): you should not have to specify the initial position of the robot if you are using
amcl. The amcl algorithm should "find it" after you move the robot a short distance.Regarding (3): navigation uses the
map->base_linktransform. navigation knows nothing about how that transform is updated, it just assumes the most recent value is correct. What do you mean by "the drifting issue stops"? After you move the robot around, is the pose of base_link accurate or not?P.S. Please do not bring up details of your former problem in this new question - it is too confusing.
Thank again.I will remove the setting from initial state. By drifting I mean both the local cosmap and the base_link literally floats off the map(static map). So this drifting stops if I use odom--->base_link transform from my odometry node but if I let EKF publish the odom-->base_link transform ...the drifting happens..and oddly somehow AMCL stops publishing the map transform when I inspect the TF tree post cosmap/ footprint drifting.
I deleted my prior comment.
From what I have seen,
amcldoes not publish a pose if the robot is not moving. Whenamcldoes publish, is the pose it generates for the robot accurate?You previous comment may actually be valid and apply here because before I send the robot to goal I see the AMCL publish the MAP ---> base_link transform, but after the robot starts moving, the drifting (local costmap/ robot_footprint) starts and AMCL stops publishing my map transform.