Get starting pose from localization

[mjeronimo]

Currently, the robot's starting pose is stubbed out in the BtNavigator and SimpleNavigator classes. Instead, it should come from the Localization module.

[mjeronimo]

Both navigator classes now get the current robot pose from the Robot class, which receives it from AMCL via the amcl_pose topic.

[SteveMacenski]

@mjeronimo that's not the current pose, you should be checking TF for the current pose. That will only run every once in awhile (every 20cm or so) which isn't up to date or particularly accurate at any given time.

This is why folks use robot_pose_ekf, robot_localization, etc in order to get high rate accurate (according to model) pose estimates

[mikeferguson]

I'm just going to add a comment here for clarity: AMCL itself will only update every so often -- which means that the odom->map transform only gets updated occasionally. In theory, the odom->map shouldn't change quickly (unless you are slipping wheels/etc).

The base_link->odom should be updating at the rate of the base controller odom/tf publication, which should be much higher (alternatively, the base controller might not publish TF, but rather let something like robot_localization fuse the base odometry and IMU to get a more accurate base_link->odom TF publication).

TF is still the right way to get the base_link->map transformation though.

[mkhansenbot]

Let's open a new issue for the change to use TF for the initial starting pose