2D LiDAR on quadrotor

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view
|

2D LiDAR on quadrotor

heshamhendy
I have troubling with integrating zed2i camera with 2DLiDAR and GX5: As you see I use really many sensor and the reason behind that I have no tolerance with error since I need odometry data für quadrotor and it must be accurate. I tried 2D LiDAR on plane motion and i worked fine; however with z motion something went south!! I attached my launch file and I hope someone can explain me why that happens and if I can fix it.

P.S: I can't use 3D LiDAR because of the weight


rtabmap_zed.launch
Reply | Threaded
Open this post in threaded view
|

Re: 2D LiDAR on quadrotor

heshamhendy

<launch>
 

  <node pkg="tf" type="static_transform_publisher" name="tf_laser_baselink" args="0.0 -0.05 0.0 0 0.0000000 0.0000000 base_link laser 50" />

  <node pkg="tf" type="static_transform_publisher" name="tf_GX5_imu_baselink" args="0.0 0.0 0.0 0 0.0000000 0.0000000 base_link gx5_imu 500" />
  <node pkg="tf" type="static_transform_publisher" name="tf_GX5_filteredimu_baselink" args="0.0 0.0 0.0 0 0.0000000 0.0000000 base_link imu 500" />

  <arg name="imu_ignore_acc"          default="false" />
  <arg name="imu_remove_gravitational_acceleration" default="true" />
 
   

  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
  <arg name="subscribe_rgb"  default="$(arg depth)"/>
 
 
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
 
 
  <arg name="localization"            default="false"/>
 
 
  <arg name="use_sim_time"            default="false"/>
 
 
 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find connect_senses)/rviz/rtabmap_specialized_zed2i.rviz" />
 
  <arg name="frame_id"                default="base_link"/>     
  <arg name="odom_frame_id"           default="odom"/>               
  <arg name="odom_frame_id_init"      default=""/>               
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     
  <arg name="ground_truth_base_frame_id" default=""/> 
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/pcdFiles/rtabmap/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>                            >
  <arg name="args"                    default="--delete_db_on_start"/>             
  <arg name="rtabmap_args"            default="$(arg args)" />   
  <?arg name="rtabmap_args"            value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/ ?>
  <arg name="gdb"                     default="false"/>         
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="clear_params"            default="true"/>
  <arg name="output"                  default="screen"/>       
  <arg name="publish_tf_map"          default="true"/>

 
  <?arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/?>
  <arg name="approx_sync"          default="true"/>     
  <arg name="approx_sync_max_interval"  default="0"/> 
   
 
  <arg name="rgb_topic"               default="/zed2i/zed_node/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/zed2i/zed_node/depth/depth_registered" />
  <arg name="camera_info_topic"       default="/zed2i/zed_node/depth/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
 
 
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />     
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />

 
 
  <arg name="rgbd_sync"               default="true"/>         
  <arg name="approx_rgbd_sync"        default="false"/>         
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />
 
  <arg name="compressed"              default="false"/>         
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="depth_image_transport"   default="compressedDepth"/> 
 
  <arg name="gen_cloud"               default="false"/> 
  <arg name="gen_cloud_decimation"    default="4"/>
  <arg name="gen_cloud_voxel"         default="0.05"/>
   
  <arg name="subscribe_scan"          default="true"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="$(arg gen_cloud)"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> 
  <arg name="gen_scan"                default="false"/> 
 
  <arg name="gen_depth"                  default="false" /> 
  <arg name="gen_depth_decimation"       default="1" />
  <arg name="gen_depth_fill_holes_size"  default="0" />
  <arg name="gen_depth_fill_iterations"  default="1" />
  <arg name="gen_depth_fill_holes_error" default="0.1" />
   
  <arg name="visual_odometry"          default="true"/>         
  <arg name="icp_odometry"             default="true"/>         
  <arg name="odom_topic"               default="odom"/>         
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> 
  <arg name="publish_tf_odom"          default="true"/>            >
  <arg name="odom_tf_angular_variance" default="0.001"/>         
  <arg name="odom_tf_linear_variance"  default="0.001"/>         
  <arg name="odom_args"                default=""/>             
  <arg name="odom_sensor_sync"         default="true"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/gx5/imu/data"/>           
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>
 
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>     
  <arg name="scan_cloud_assembling_max_clouds"   default="0"/>     
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_range_min"    default="0.0"/>   
  <arg name="scan_cloud_assembling_range_max"    default="0.0"/>   
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>   
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
 
  <arg name="subscribe_user_data"      default="false"/>             
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> 
 
  <arg name="gps_topic"                default="/gps/fix" />         
 
  <arg name="tag_topic"                default="/tag_detections" /> 
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             
  <arg name="fiducial_topic"           default="/fiducial_transforms" /> 
 
 
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>

 
  <group ns="$(arg namespace)">
     
   
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" clear_params="$(arg clear_params)" output="$(arg output)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
         
         
         
         
         
        </node>
      </group>
    </group>
    <group if="$(arg stereo)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" clear_params="$(arg clear_params)" output="$(arg output)">
          <remap from="left/image_rect"   to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
         
         
         
        </node>
      </group>
    </group>
   
    <group unless="$(arg rgbd_sync)">
       <group if="$(arg subscribe_rgbd)">
         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
           <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
           <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
           <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
           
         </node>
      </group>
    </group>
   
    <node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_ros/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
      <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
      <remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="cloud"             to="$(arg scan_cloud_topic)" />

     
     
     
     
    </node>

   
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">
     
       
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>
     
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
        </node>

       
        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"             to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"                   to="$(arg odom_topic)"/>
          <remap from="imu"                    to="$(arg imu_topic)"/>
     
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
         
        </node>
      </group>
    </group>
   
   
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
    </node>
   
    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>

      <remap from="odom"            to="$(arg odom_topic)"/>
     
     
     
     
     
     
     
     
     
    </node>
 
   
   
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
       
     
     
     
     
     
     
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
     
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
       
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
     
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="fiducial_transforms"    to="$(arg fiducial_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
     
     
     
     
     

     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     

    </node>
 
   
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
   
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
     
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
     
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
     
      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
 
  </group>
 
 
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

   
   
   
   
  </node>

 

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

     
     
     

     
     
     
     

     

     

     
       >
       
       
     

     

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

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

     

      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>
      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true] </rosparam> 
     
     
     
      <?param name="odom2_differential" value="true"/>
     

     
     
      <?param name="odom2_relative" value="true"/>
     

     

     

     
     
     
      <?param name="odom2_queue_size" value="25"/>
       

     
      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 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.001, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 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.001, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</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>

  </node>
</launch>