Merge pull request #427 from stwirth/fix-424-jade

Fixes #424 for jade: New test and fix for proper interpretation of a delayed initial pose.

amcl/CMakeLists.txt

@@ -121,6 +121,7 @@ if(CATKIN_ENABLE_TESTING)
 
   # Tests
   add_rostest(test/set_initial_pose.xml)
+  add_rostest(test/set_initial_pose_delayed.xml)
   add_rostest(test/basic_localization_stage.xml)
   add_rostest(test/small_loop_prf.xml)
   add_rostest(test/small_loop_crazy_driving_prg.xml)

amcl/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
 

amcl/test/set_initial_pose_delayed.xml

@@ -0,0 +1,54 @@
+<!-- setting pose: 47.943 21.421 -0.503
+setting pose: 30.329 34.644 3.142 
+117.5s -->
+<launch>
+    <param name="/use_sim_time" value="true"/>
+    <node name="rosbag" pkg="rosbag" type="play" 
+        args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
+    <node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
+    <node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
+    <!-- The bagfile starts at 34.6s. We try to send the same initial pose as above with stamp 34.8 at 5 seconds
+         into the bagfile (at 39.6s). AMCL should add the robot motion from 34.8 to 39.6 to that initial pose
+         before using it, so there should be no mis-localization caused by this. -->
+    <node name="delayed_pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003 34.8 39.6"/>
+    <node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
+      <remap from="scan" to="base_scan" />
+      <param name="transform_tolerance" value="0.2" />
+      <param name="gui_publish_rate" value="10.0"/>
+      <param name="save_pose_rate" value="0.5"/>
+      <param name="laser_max_beams" value="30"/>
+      <param name="min_particles" value="500"/>
+      <param name="max_particles" value="5000"/>
+      <param name="kld_err" value="0.05"/>
+      <param name="kld_z" value="0.99"/>
+      <param name="odom_model_type" value="omni"/>
+      <param name="odom_alpha1" value="0.2"/>
+      <param name="odom_alpha2" value="0.2"/>
+      <!-- translation std dev, m -->
+      <param name="odom_alpha3" value="0.8"/>
+      <param name="odom_alpha4" value="0.2"/>
+      <param name="odom_alpha5" value="0.1"/>
+      <param name="laser_z_hit" value="0.5"/>
+      <param name="laser_z_short" value="0.05"/>
+      <param name="laser_z_max" value="0.05"/>
+      <param name="laser_z_rand" value="0.5"/>
+      <param name="laser_sigma_hit" value="0.2"/>
+      <param name="laser_lambda_short" value="0.1"/>
+      <param name="laser_lambda_short" value="0.1"/>
+      <param name="laser_model_type" value="likelihood_field"/>
+      <!-- <param name="laser_model_type" value="beam"/> -->
+      <param name="laser_likelihood_max_dist" value="2.0"/>
+      <param name="update_min_d" value="0.2"/>
+      <param name="update_min_a" value="0.5"/>
+      <param name="odom_frame_id" value="odom"/>
+      <param name="resample_interval" value="1"/>
+      <param name="transform_tolerance" value="0.1"/>
+      <param name="recovery_alpha_slow" value="0.0"/>
+      <param name="recovery_alpha_fast" value="0.0"/>
+      <!--param name="initial_pose_x" value="47.443"/>
+      <param name="initial_pose_y" value="21.421"/>
+      <param name="initial_pose_a" value="-1.003"/-->
+    </node>
+  <test time-limit="180" test-name="set_initial_pose" pkg="amcl" 
+        type="basic_localization.py" args="0 47.060 21.603 -1.074 0.75 0.75 90.0"/>
+</launch>

amcl/test/set_pose.py

@@ -8,12 +8,15 @@
 
 
 class PoseSetter(rospy.SubscribeListener):
-    def __init__(self, pose):
+    def __init__(self, pose, stamp, publish_time):
         self.pose = pose
+        self.stamp = stamp
+        self.publish_time = publish_time
 
     def peer_subscribe(self, topic_name, topic_publish, peer_publish):
         p = PoseWithCovarianceStamped()
         p.header.frame_id = "map"
+        p.header.stamp = self.stamp
         p.pose.pose.position.x = self.pose[0]
         p.pose.pose.position.y = self.pose[1]
         (p.pose.pose.orientation.x,
@@ -23,11 +26,21 @@ def peer_subscribe(self, topic_name, topic_publish, peer_publish):
         p.pose.covariance[6*0+0] = 0.5 * 0.5
         p.pose.covariance[6*1+1] = 0.5 * 0.5
         p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
+        # wait for the desired publish time
+        while rospy.get_rostime() < self.publish_time:
+            rospy.sleep(0.01)
         peer_publish(p)
 
 
 if __name__ == '__main__':
-    pose = map(float, rospy.myargv()[1:])
+    pose = map(float, rospy.myargv()[1:4])
+    t_stamp = rospy.Time()
+    t_publish = rospy.Time()
+    if len(rospy.myargv()) > 4:
+        t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4]))
+    if len(rospy.myargv()) > 5:
+        t_publish = rospy.Time.from_sec(float(rospy.myargv()[5]))
     rospy.init_node('pose_setter', anonymous=True)
-    pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose))
+    rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec()))
+    pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1)
     rospy.spin()