<!-- Launch file for test_ukf_localization_node_bag2 -->

<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/test2.bag --clock -d 5" required="true"/>

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

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

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

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

      <param name="odom0" value="/jackal_velocity_controller/odom"/>
      <param name="imu0" value="/imu/data"/>

      <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, false,
                                      false, false, false,
                                      true,  true, true,
                                      false, false, true,
                                      false, false, false]</rosparam>

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

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

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

      <rosparam param="process_noise_covariance">[0.03, 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.4, 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.03, 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.025, 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.05, 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.002, 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.01, 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.01]</rosparam>

      <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                     0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                     0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                     0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                     0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                     0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                     0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                     0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                     0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                     0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                     0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

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

    </node>

    <test test-name="test_ukf_localization_node_bag2_pose" pkg="robot_localization" type="test_ukf_localization_node_bag2" clear_params="true" time-limit="1000.0">
      <param name="final_x" value="1.0438"/>
      <param name="final_y" value="3.4940"/>
      <param name="final_z" value="0.7260"/>
      <param name="tolerance" value="0.2351"/>
      <param name="output_final_position" value="$(arg output_final_position)"/>
      <param name="output_location" value="$(arg output_location)"/>
    </test>

</launch>
