Fixes interpretation of a delayed initialpose message (see ros-planni…

…ng#424).

The tf lookup as it was before this change was very likely to fail as
ros::Time::now() was used to look up a tf without waiting on the tf's
availability. Additionally, the computation of the "new pose" by
multiplying the delta that the robot moved from the initialpose's
timestamp to ros::Time::now() was wrong. That delta has to by multiplied
from the right to the "old pose".
This commit also changes the reference frame to look up this delta to be
the odom frame as this one is supposed to be smooth and therefore the
best reference to get relative robot motion in the robot (base link) frame.

src/amcl_node.cpp

@@ -1327,9 +1327,14 @@ AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStampe
   tf::StampedTransform tx_odom;
   try
   {
-    tf_->lookupTransform(base_frame_id_, ros::Time::now(),
-                         base_frame_id_, msg.header.stamp,
-                         global_frame_id_, tx_odom);
+    ros::Time now = ros::Time::now();
+    // wait a little for the latest tf to become available
+    tf_->waitForTransform(base_frame_id_, msg.header.stamp,
+                         base_frame_id_, now,
+                         odom_frame_id_, ros::Duration(0.5));
+    tf_->lookupTransform(base_frame_id_, msg.header.stamp,
+                         base_frame_id_, now,
+                         odom_frame_id_, tx_odom);
   }
   catch(tf::TransformException e)
   {
@@ -1344,7 +1349,7 @@ AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStampe
 
   tf::Pose pose_old, pose_new;
   tf::poseMsgToTF(msg.pose.pose, pose_old);
-  pose_new = tx_odom.inverse() * pose_old;
+  pose_new = pose_old * tx_odom;
 
   // Transform into the global frame