Ask Your Question
0

robot_localization: a very simple usecase not working?

asked 2016-07-26 14:46:08 -0500

jorge gravatar image

updated 2016-11-07 08:21:23 -0500

Hi, I'm trying to use robot_localization to fuse two localization sources: amcl + feature-based localization. But just starting with a very simple case, namely filtering amcl input without any fusion, works very bad for the rotation. It's clear to me that either I'm misunderstanding how robot_localization works, or I did a very stupid mistake on the configuration. Here's my launch file:

<launch>
    <node ns="absolute" pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" respawn="true">

      <!-- ======== STANDARD PARAMETERS ======== -->
      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.5"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_footprint"/>
      <param name="world_frame" value="map"/>

      <param name="transform_time_offset" value="0.0"/>

      <param name="pose0" value="/amcl_pose"/> 

      <!-- AMCL estimates X, Y and yaw against a known map, so use those absolute measures -->
      <rosparam param="pose0_config">[true,  true,  false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <param name="pose0_differential" value="false"/>

      <param name="pose0_relative" value="false"/>

      <param name="print_diagnostics" value="true"/>

      <!-- ======== ADVANCED PARAMETERS ======== -->

      <param name="pose0_queue_size" value="1"/>

      <rosparam param="process_noise_covariance">[0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 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, 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, 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, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 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.00, 0.0, 0.0 ...
(more)
edit retag flag offensive close merge delete

Comments

have you turned off the amcl tf_broadcast (Set this to false to prevent amcl from publishing the transform between the global frame and the odometry frame)?

Procópio gravatar image Procópio  ( 2016-08-03 11:09:48 -0500 )edit

I am having the same issues, if I figure something out, I'll let you know.

Procópio gravatar image Procópio  ( 2016-08-03 11:21:17 -0500 )edit

Hi, yes, duplicated tf is not the problem. In fact, I'm getting better results tweaking the process_noise_covariance and increasing the input odom messages covariance

jorge gravatar image jorge  ( 2016-08-08 03:32:32 -0500 )edit

thanks for the feeback. I am tweaking the amcl code now, I noticed that it was publishing at a very low rate and I am forcing it to publish the pose every loop now, even if amcl the filter has not updated. at amcl_node.cpp line 1095, I have this now: bool force_publication = true;

Procópio gravatar image Procópio  ( 2016-08-08 04:53:34 -0500 )edit

Does that help EKF localization? Think you will keep publishing an outdated pose in between filter updates...

jorge gravatar image jorge  ( 2016-08-09 01:44:39 -0500 )edit

you are right, that is not helping much. I rolled back from that change.

Procópio gravatar image Procópio  ( 2016-08-11 08:55:34 -0500 )edit

Hi I also tested the node with a single absolute pose source first. However, if I set pose0_differential and pose0_relative to false, I couldn't get any output. I can get output while pose0_differential is true, and it starts from 0, which is not I wanted.

bageltz gravatar image bageltz  ( 2016-09-08 08:04:23 -0500 )edit

I also tried your launch file, and had the same problem. Do you have any suggestion to get the node outputting filtered absolute values? Thanks.

bageltz gravatar image bageltz  ( 2016-09-08 08:06:28 -0500 )edit

1 Answer

Sort by » oldest newest most voted
2

answered 2016-08-28 01:57:06 -0500

Tom Moore gravatar image

updated 2016-11-22 04:25:08 -0500

So one thing to change right off the bat is that if you are using pose data from amcl and you need the filter to more or less immediately converge to that same pose, then you need to increase the process_noise_covariance for X, Y, and yaw. Otherwise, the filter will trust its own estimate more than the measurement, and since there's no velocity being measured, that's going to be sluggish. Also, note that amcl does not constantly update its pose. It only sends out pose updates when it reaches the thresholds that are defined in its configuration. You should have some velocity references being fused as well.

EDIT 1:

I still haven't had a chance to look at your bag (sorry, getting set up and reviewing someone's bagged data takes a considerable amount of time that I can't afford right now), but just looking at the EKF config that you linked, I can see some problems right off the bat:

  1. I'm referring to this block:

    odom0: /odom
    odom0_config: [true,true,false,
                   false, false, false,
                   false, false, false,
                   false, false, true,
                   false, false, false]
    odom0_differential: false
    odom0_relative: false
    odom0_queue_size: 2
    odom0_pose_rejection_threshold: 5
    odom0_twist_rejection_threshold: 1
    odom0_nodelay: false
    
    pose0: /amcl_pose
    pose0_config: [true,  true,  false,
                   false, false, false,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    pose0_differential: true
    pose0_relative: false
    pose0_queue_size: 5
    pose0_rejection_threshold: 2
    pose0_nodelay: false
    

    First, you are fusing absolute pose variables from two different sources. Don't do that. If you drive forward 10 meters and your amcl pose puts you at (10, 0) and your wheel odometry puts you at (9.1, 0), then the filter is going to rapidly jump back and forth between those two readings. Change your wheel odometry config to only fuse velocities.

    Second, get rid of the rejection thresholds. Those are parameters that should not be included until you've had a change to solidly characterize the noise in your measurements.

    Third, enable nodelay for the odometry data. The odometry message size is above a threshold that causes trouble with Nagle's algorithm, causing messages to arrive at strange intervals.

  2. The EKF is, obviously, a filter. All filters will induce a non-zero delay in output. If your amcl data is fused in an EKF, it's going to have at least a small amount of lag. You can minimize this, as I said, by increasing the process_noise_covariance for a given variable. I realize you've done this.

  3. Can you repeat your rotation test by eliminating the second EKF (the one fusing the amcl data) and let amcl publish the map->odom transform?

edit flag offensive delete link more

Comments

Thanks @tom. Actually, X and Y are fine. My problems come from rotation (check my capture in UPDATE 2). I managed to improve things by lowering the yaw process noise to 0.01 and raising A LOT the odom covariance (1.0!!!). But still robot rotations pollute costmaps due to mismatching laser scan.

jorge gravatar image jorge  ( 2016-11-07 07:48:33 -0500 )edit

I wonder if the noise in the yaw comes from the fact that the filter uses the same map -> odom tf it creates to transform amcl pose from base_link to odom (substracting odom -> base_link tf). This can trigger positive feedback loops that could explain the noise. Makes this sense?

jorge gravatar image jorge  ( 2016-11-07 09:35:17 -0500 )edit

Just checking: you did turn off the broadcast of map->odom transform by amcl, correct? Also, if the amcl pose is reported in the map frame and you are feeding it into an EKF with its world_frame set to map, then no transformation occurs.

Tom Moore gravatar image Tom Moore  ( 2016-11-07 09:57:05 -0500 )edit

yes, tf_broadcast is disabled. But... EKF tracks map -> base_link, but it broadcasts map -> odom. So I suppose at some point there must be a odom ->base_link subtraction. But well, it's just a guess,,, I never inspected the code carefully!

jorge gravatar image jorge  ( 2016-11-07 10:39:55 -0500 )edit

Yes, it absolutely uses the odom->base_link transform, you are correct. But you are fusing the pose data from amcl, which, if I recall, is NOT given in the base_footprint frame, but in the map frame. Can you add a sample amcl message?

Tom Moore gravatar image Tom Moore  ( 2016-11-07 11:11:20 -0500 )edit

Yes, sure, amcl poses are on map frame. But in EKF it gets combined with the odometry. I tried to make it work without odometry and didn't work at all; robot tf spins like crazy while the robot is not moving at all.

jorge gravatar image jorge  ( 2016-11-07 11:57:57 -0500 )edit

To your knowledge, would it make sense to combine ONLY global poses (amcl and other landmark-based global poses) without any input for the prediction phase? (odom, IMU, cmd_vel...)

jorge gravatar image jorge  ( 2016-11-07 11:59:57 -0500 )edit

Can you post a bag file? Go ahead and record everything, including the tf tree.

Tom Moore gravatar image Tom Moore  ( 2016-11-08 11:23:38 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2016-07-26 14:46:08 -0500

Seen: 1,594 times

Last updated: Nov 22 '16