Ask Your Question
0

robot_localization with 2 GPS

asked 2018-03-08 09:49:47 -0500

tuandl gravatar image

updated 2018-03-08 10:00:14 -0500

Hi everyone, I am trying to fuse 2 gps, 1 IMU, robot's odometry using robot_localization package. My navsat_transform launch file is as follows:

<?xml version="1.0" encoding="ISO-8859-15"?>
<launch>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_novatel" clear_params="true">
    <rosparam command="load" file="$(find rasberry_navigation)/config/navsat_transform_novatel.yaml" />
      <remap from="odometry/gps" to="novatel/odometry"/>
      <remap from="odometry/filtered" to="rl/odometry"/> 
      <remap from="gps/fix" to="/navsat_fix"/>
      <remap from="gps/filtered" to="navsatfix/filtered"/> 
      <param name="magnetic_declination_radians" value="0.01338085759"/>
      <param name="yaw_offset" value="1.570796327"/> 
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_mtig" clear_params="true">
    <rosparam command="load" file="$(find rasberry_navigation)/config/navsat_transform_mtig.yaml" />
      <remap from="odometry/filtered" to="rl/odometry"/> 
      <remap from="gps/fix" to="/fix"/>
      <remap from="odometry/gps" to="mtig/odometry"/>
      <remap from="gps/filtered" to="fix/filtered"/>
      <param name="magnetic_declination_radians" value="0.01338085759"/>
      <param name="yaw_offset" value="1.570796327"/>
  </node>

  <node name="bl_novatel_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 base_link gps 100" output="log"/>
  <node name="novatel_fix_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 gps base_imu 100" output="log"/>
  <node name="front_left_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0.41 0 0 0 0 1 front_frame frame_left_link 100" output="log"/>
  <node name="front_right_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 -0.41 0 0 0 0 1 front_frame frame_right_link 100" output="log"/>
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rasberry_navigation)/workshop_rviz.rviz"/>
</launch>

My dual_navsat_ekf launch file is as follows:

   <?xml version="1.0" encoding="ISO-8859-15"?>
    <launch>
      <rosparam command="load" file="$(find rasberry_navigation)/config/dual_ekf_navsat_2gps.yaml" />
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true">
        <remap from="odometry/filtered" to="rl/odometry"/>  
      </node>

      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
        <remap from="odometry/filtered" to="odometry/filtered_map"/>
      </node> 
      <include file="$(find rasberry_navigation)/navsat_transform_2_gps.launch"/>
    </launch>

My dual_navsat_ekf config file is as follows:

ekf_se_odom:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  odom0: odometry
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false  #turn off for displaying in rviz

  imu0: imu/data
  imu0_config: [false, false, false,
                false, false, true,
                false, false, false,
                false, false, false,  #for rosbag angular velocity for yaw is zero
                true,  false, false]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: false

  use_control: false

  process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0.3,  0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-03-08 14:27:13 -0500

stevejp gravatar image

Okay, here are a few things which look irregular for your setup (there might be others, but these are obvious ones):

  1. In both of your navsat transform nodes the odometry source you are using is rl/odometry, which is coming from the output of your first EKF (ekf_se_odom). You should be using the output of your second EKF (ekf_se_map) as the input into the navsat transform node, which in your case would be odometry/filtered_map.
  2. In the configuration of your second EKF (ekf_se_map), you have both GPS odometry topics being fused under the same "odom1" heading. One of these needs to be changed to "odom2", otherwise r_l won't know you are using two separate GPS sources.
  3. Again in the configuration of your second EKF (ekf_se_map), you are fusing the output of your first EKF (rl/odometry). This can cause problems - typically what people do is just fuse the raw wheel odemetry & IMU measurements instead of the output from the first EKF.

I think in general it would probably be a good idea to start with a basic setup (i.e., with one instance of r_l, only fusing odometry & IMU data), and then once you're comfortable with the output you can add in more measurement sources. Trying to do everything at once will be a pain to de-bug, there are a lot of things which could be configured incorrectly.

edit flag offensive delete link more

Comments

@stevejp, thank you for your reply. It did fix my problem. I misinterpreted the documentation where it says

typically the output of a robot_localization state estimation node

I thought it requires r_l ouput to get odom->basel_link transform.

tuandl gravatar imagetuandl ( 2018-03-09 03:48:00 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-03-08 09:49:47 -0500

Seen: 202 times

Last updated: Mar 08 '18