fix some mp tracking problems.
Merge branch 'catkin' of https://github.com/AutonomyLab/autonomy_hri into catkin