ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

robot_localization : Local cosmap drifts away from map

asked 2022-05-06 19:23:57 -0600

chm007 gravatar image

updated 2022-10-17 15:37:40 -0600

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

image description

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 ...
(more)
edit retag flag offensive close merge delete

Comments

  1. Having a non-zero initial state for the filter (and thus odom->base_link) is unusual.
  2. You should not set process_noise_variance if the ekf filter is not working. You could be making things worse.
  3. You asked about robot_localization config 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?
Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-05-07 11:07:47 -0600 )edit

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?

chm007 gravatar image chm007  ( 2022-05-07 12:06:54 -0600 )edit

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_link transform. 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.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-05-07 13:39:53 -0600 )edit

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.

chm007 gravatar image chm007  ( 2022-05-07 15:18:13 -0600 )edit

I deleted my prior comment.

From what I have seen, amcl does not publish a pose if the robot is not moving. When amcldoes publish, is the pose it generates for the robot accurate?

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-05-07 16:31:59 -0600 )edit

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.

chm007 gravatar image chm007  ( 2022-05-08 07:03:27 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-05-08 10:27:26 -0600

Mike Scheutzow gravatar image

updated 2022-05-08 10:30:00 -0600

According to the wiki page, amcl does not publish the TF map->base_link to /tf. Instead, it does a calculation which publishes map->odom such that map->odom->base_link produces the correct result. This is what you want, because it has the effect of canceling out drift in the odom->base_link.

The 2nd issue you may run into is that the amcl ros pkg outputs 1 pose, then waits. If you see this behavior, your code will need to re-start the amcl node by calling its global_localization service. For a moving robot, this may have to be done every few seconds.

In your amcl config, you may want to experiment with reducing the value of update_min_d to something as small as 0.001 meters. This value is a tradeoff: higher values are better when the robot is moving, but lower values are better when the robot is stationary. So you need to experiment.

edit flag offensive delete link more

Comments

I will spend today and tomorrow reviewing the odom/odometry/ filtered topics and TF..as well as tweaking AMCL and I will report what I find/outcomes. Thanks.

chm007 gravatar image chm007  ( 2022-05-08 11:42:30 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2022-05-06 19:23:57 -0600

Seen: 144 times

Last updated: Oct 17 '22