AMCL: Failed to transform initial pose in time
I'm trying to get AMCL localization going, but it's failing to set initial pose when I'm receiving both scan and odom messages:
[amcl-3] [INFO] [1623053407.722524945] [amcl]: Configuring
[amcl-3] [INFO] [1623053407.722612978] [amcl]: initTransforms
[amcl-3] [INFO] [1623053407.726553016] [amcl]: initPubSub
[amcl-3] [INFO] [1623053407.729087097] [amcl]: Subscribed to map topic.
[lifecycle_manager-4] [INFO] [1623053407.732307402] [lifecycle_manager_localization]: Activating map_server
[map_server-2] [INFO] [1623053407.732457864] [map_server]: Activating
[amcl-3] [INFO] [1623053407.732811799] [amcl]: Received a 274 X 129 map @ 0.050 m/pix
[lifecycle_manager-4] [INFO] [1623053407.732881614] [lifecycle_manager_localization]: Activating amcl
[amcl-3] [INFO] [1623053407.733255660] [amcl]: Activating
[amcl-3] [WARN] [1623053407.733272405] [amcl]: Publishing the particle cloud as geometry_msgs/PoseArray msg is deprecated, will be published as nav2_msgs/ParticleCloud in the future
[lifecycle_manager-4] [INFO] [1623053407.733503925] [lifecycle_manager_localization]: Managed nodes are active
[hardware_control-1] [WARN] [1623053408.150909118] [hardware_control]: Publishing odometry
[amcl-3] [INFO] [1623053426.173220415] [amcl]: initialPoseReceived
[amcl-3] [INFO] [1623053426.173416533] [amcl]: Setting pose (1623053426.173416): 0.149 0.224 -0.016
[amcl-3] [INFO] [1623053435.902485150] [amcl]: initialPoseReceived
[amcl-3] [INFO] [1623053435.902558674] [amcl]: Setting pose (1623053435.902559): 1.195 0.444 0.033
[amcl-3] [INFO] [1623053468.538590892] [amcl]: createLaserObject
[amcl-3] [INFO] [1623053477.014112592] [amcl]: initialPoseReceived
[amcl-3] [WARN] [1623053477.014242176] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future. Requested time 1623053477.014179 but the latest data is at time 1623053477.012145, when looking up transform from frame [base_link] to frame [odom])
[amcl-3] [INFO] [1623053477.014263234] [amcl]: Setting pose (1623053477.014263): 0.324 0.218 0.006
Initially, I get no warnings, but as soon as I turn on the lidar and it starts sending messages to scan topic I start getting:
Failed to transform initial pose in time (Lookup would require extrapolation into the future. Requested time 1623053477.014179 but the latest data is at time 1623053477.012145, when looking up transform from frame [base_link] to frame [odom])
So it seems like somehow sending messages to scan topic is affecting the base_link to odom lookup.
Most importantly AMCL is not localizing, as far as I understand as soon as you set your initial pose to be close to where the robot is, the lidar scan should "snap" to the map walls so the robot position would be adjusted, but this is not happening.
This is how my frames look like after running:
ROS_DOMAIN_ID=45 ros2 run tf2_tools view_frames.py
I'm trying to set initial pose is set via RViz.
ACML transform_tolerance is set to 1.0, here is the full config:
amcl:
ros__parameters:
use_sim_time: False
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: "base_link"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: false
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 100.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "odom"
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "differential"
save_pose_rate: 0.5
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
z_hit ...
You have a transform map->odom and this is only a warning. I get this one too from time to time, e.g. if my computer is busy and can not deliver the tf message in time. I would ignore this message.
It happens every time when I'm trying to set the initial pose. Also, I don't think AMCL is working properly, I think it should adjust
map->odomtransform after setting the initial pose so that the laser scan aligns with the walls on the map, but it always stays static. If I set initial pose at an angle laser scan will also stay at an angle. I think what's happening is that AMCL is setting the initial pose and is using it to send a staticmap->odomtransform and is not trying to adjust it based onscandata. It also happens every time I'm trying to set the initial pose, so this error is not transient.I did more testing and noticed that when I start without a lidar running, which runs from a Raspberry Pi on the robot I don't get this warning. As soon as I turn the laser on I start getting this issue.
What do you mean with "stays static"? If you move your robot its position in the map stays? Obviously you do not get the warning without any /scan messages, because amcl just does nothing when it does not receive them.
Btw: regarding time: If you synchronize all clocks with NTP you should be fine.
Edit: Just saw the ROS2-tag. The above is true for ROS1, no idea about ROS2, sorry.
I'll chime in and say that I have seen the same behavior in ROS2 in a simulated environment, when dynamically spawning the robot into the world. When AMCL launches at a different time from the node publishing scan information (at least with a simulated clock), it seemed to break down and not work as intended as you've demonstrated above. In simulation, I could work around the issue by starting Gazebo in a paused state and only unpausing it (and starting the sim clock) once all the nodes had spun up. While that workaround certainly won't help you in hardware, I think it points to AMCL being a bit too picky with timestamps, in my humble opinion. I really feel like there should be a better recovery mode if timestamps are off...
alright, stepping off my soap box now
Turns out this warning wasn't the issue I misunderstood how localization works, I thought it should localize immediately, but it turns out that after driving the robot for a bit allows AMCL to localize properly and "align" laser scan to the walls on the map. I still get the warning every time, but it will localize after a while.