Setting initialpose for AMCL using difference between initial TFs

Question

I'm working on bag files containing odometry and laser scan data. I have a launch file that runs simultaneously the bag, AMCL and a map node. The TF from odom to base_link is different in each bag.

When I run AMCL with initial pose not set (default values) it takes much time to estimate proper robot position on map and sometimes it doesn't even find it properly.

I'm trying to init AMCL using first odom -> base_link transform value in the bag file so that laser scan image matches the map with the highest probability. Calling /global_localization service helps but not well and it also takes too much time. Bags are only 30 sec.

Question: What is the best way to pass this transform values as initial positions in AMCL? Should I write a publisher that reads first tf in bag and then publish on /initialpose topic or is there any easier way to do it (params, services, bash files, extract data from bag to csv and then read it somehow, etc.)?

---

Originally posted by syskaseb on ROS Answers with karma: 33 on 2017-06-26

Post score: 3

---

Original comments

Comment by syskaseb on 2017-06-26:
I suppose that relation between odom->base_link may be insufficient to achieve my goal but finding at least rotation between tfs would be helpful (to eliminate initial rotation between laser scan and map)

Answer 1

The information you want is not in the odom->base_link transform, as amcl provides the map->odom transform.

There are at least two ways of doing what you want:

1- Through the /initialpose topic, which can be set using rviz "2D Pose Estimation" tool or directly publishing that topic through a terminal.

2- Load the parameters with your initial pose when launching AMCL (initial_pose_x, initial_pose_y, initial_pose_a). In this case you will need a different launch file for each bag if the robot does not start at the same position.

If you do not know the initial pose, you will have to play the bag a couple of times and do a bit of a guesswork using rviz "2D Pose Estimation" until you find approximately the initial pose. AMCL will output the received info on a terminal, so once you are satisfied, you can use those values in the AMCL launch file.

---

Originally posted by Procópio with karma: 4402 on 2017-06-28

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by syskaseb on 2017-06-28:
Thanks, I did the same thing that you've explained in the last paragraph. As I didn't know the initialpose I had to wait for AMCL until it found quite good approximation, use tf_echo to find map->base_link transform values and then put it as the argument in the launch file.

Comment by syskaseb on 2017-06-28:
I also wanted to automate the process of gaining the best approximation of the initialpose but I don't have so many bag files to decide bothering myself with creating node calculating and publishing it :)

P.S.it was necessary to set update_min_d and update_min_a positive vals to prevent map dri