<?xml version="1.0" ?>
<!-- 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"/>
      <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_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 30.33 34.64 3.14 0.75 0.75 90.0"/>
</launch>