<!-- Launch file for test_ukf_localization_node_bag3 -->

<launch>
    <arg name="output_final_position" default="false"/>
    <arg name="output_location" default="test.txt" />

    <param name="/use_sim_time" value="true" />

    <node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test3.bag --clock -d 5" required="true"/>

    <node name="test_ukf_localization_node_bag3_ukf" pkg="robot_localization" type="ukf_localization_node" clear_params="true">

      <param name="frequency" value="30"/>  

      <param name="sensor_timeout" value="0.1"/>

      <param name="two_d_mode" value="false"/>

      <param name="odom0" value="/ardrone/odometry/raw"/>
      <param name="imu0" value="/ardrone/imu"/> 

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom"/>

      <rosparam param="odom0_config">[false, false, true,
                                     false, false, false,
                                     true,  true, false,
                                     false, false, true,
                                     false, false, false]</rosparam>

      <rosparam param="imu0_config">[false, false, false,
                                     true, true, true,
                                     false, false, false,
                                     true, true, true,
                                     false, false, false]</rosparam>

      <!-- Differential setting is off for this test -->
      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <param name="odom0_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="imu0_remove_gravitational_acceleration" value="true"/>

      <param name="alpha" value="0.001"/> 
      <param name="kappa" value="0"/> 
      <param name="beta" value="2"/>

      <rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam> 

    </node>

    <test test-name="test_ukf_localization_node_bag3_pose" pkg="robot_localization" type="test_ukf_localization_node_bag3" clear_params="true" time-limit="1000.0">
      <param name="final_x" value="-0.41148"/>
      <param name="final_y" value="-0.2513"/>
      <param name="final_z" value="2.990"/>
      <param name="tolerance" value="0.1681"/>
      <param name="output_final_position" value="$(arg output_final_position)"/>
      <param name="output_location" value="$(arg output_location)"/>
    </test>

</launch>
