Ask Your Question
0

How to use nav_transform_node

asked 2017-05-22 01:14:02 -0600

DaDaLee gravatar image

updated 2018-06-16 12:25:03 -0600

jayess gravatar image

Hi Tom/all: I want to fuse GPS data into EKF , which means that I need to run ekf_localization_node together with navsat_transform_node. My launch file goes like follows:

<launch>

    <node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 1 0 0 0 base_link imu_link" />
        <node pkg="tf2_ros" type="static_transform_publisher" name="link2_broadcaster" args="0 0 0 0 0 0 1 base_link gps" />
        <rosparam command="load" file="$(find ivlocalization)/params/dual_ekf_navsat_example.yaml" />

        <node pkg="ivlocalization" type="ekf_localization_node" name="ekf_se_map" clear_params="true" output="screen">
  </node> >

        <node pkg="ivlocalization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
        </node>

        <node name="playbag" pkg="rosbag" type="play" args="--clock $(arg bag_filename)" output="screen" />

 </launch>

And here's my .yaml file:

ekf_se_map:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: false
  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: map

  # odom0: odometry/filtered
  # odom0_config: [false, false, false,
  #                false, false, false,
  #                true,  true,  true,
  #                false, false, true,
  #                false, false, false]
  # odom0_queue_size: 10
  # odom0_nodelay: true
  # odom0_differential: false
  # odom0_relative: false

  odom1: husky_velocity_controller/odom
  odom1_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false

  imu0: imu/data
  imu0_config: [false, false, false,
                true,  true,  false,
                false, false, false,
                true,  true,  true,
                true,  true,  true]
  imu0_nodelay: true
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [1.0,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    1.0,  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,    0,    0,    0.01, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0.5,   0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0.1,  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,    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,    0,    0,    0.3,  0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]

  initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    1.0,  0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0 ...
(more)
edit retag flag offensive close merge delete

Comments

Did you check this? But what I currently see is, that you give your nodes no parameters. Where are your parameters located?

chwimmer gravatar imagechwimmer ( 2017-05-22 07:50:22 -0600 )edit

I've gone through the whole documents of robot_localization. And I load parameters from dual_ekf_navsat_example.yaml as mentioned in the fifth line of the launch file.

DaDaLee gravatar imageDaDaLee ( 2017-05-23 01:27:36 -0600 )edit

Ah ok but you need to specify the parameters in the local scope of nodes! so you just load the parameters in the global scope. Or did you do it in the local scope in the yaml file? Can you post your config?

chwimmer gravatar imagechwimmer ( 2017-05-23 08:10:20 -0600 )edit

I change the topic name in the subscriber function in the local scope,anything left over? I re-edit my question with my config of .yaml posted as you can see.Thank you so much!

DaDaLee gravatar imageDaDaLee ( 2017-05-24 20:07:37 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2017-05-29 00:03:55 -0600

updated 2017-06-01 01:32:54 -0600

So this you really checkout this? Because there are some notes you dont follow. They suggest that you actually run two robot_localization nodes. One in the odom frame where you fuse only continuous data such as odometry and/or an IMU. Another instance of the robot_localization fuses this data too but additionally the GPS data.

What I think that you really wanted to do so but what I just see in your launch file is that you start ekf_se_odom but not the instance of ekf_se_map.

So another thing is that you don't set the parameters of the navsat_transform_node like f.e. here.

Also you dont remap to the imu and the GPS data. Please check if the default one are correct.

So ok the navsat_transform_node needs now the input of the ekf_se_odom what means that your remapping is currently wrong. Your ekf_se_odom dont remap so its default output is /odometry/filtered. But you change the input from the navsat_transform_node to odometry/filtered_map (wherever this comes from). So just dont remap in this case.

Than you need to feed your ekf_se_map with the complete data of ekf_se_odom and additionally with the output of your navsat_transform_node which is publishing its data by default to /odometry/gps.


Edit to comment:

No you need the ekf_se_odom as well! So you need two instances of the EKF of the robot_localization. The ekf_se_odom is also an input for the navsat_transform_node. The instance ekf_se_map get than also the same input of the ekf_se_odom plus the output of the navsat_transform_node.

So you need two lines like:

<node pkg="ivlocalization" type="ekf_localization_node" name="ekf_se_map" clear_params="true" output="screen">

<node pkg="ivlocalization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true" output="screen">
edit flag offensive delete link more

Comments

So if I want to fuse GPS\odom\imu data, I just need to start ekf_se_map in my launch file and delete ekf_se_odom together with its params in .yaml file? Remap is not needed here? I re-edit my question as you can see here, is it right? thank you so much.

DaDaLee gravatar imageDaDaLee ( 2017-05-30 22:10:59 -0600 )edit

Well, I add two nodes ekf_se_map plus ekf_se_odom as you suggested, but the result turns to be wrong. It said "Transform from base_link to odom was unavailable for the time requested. Using latest instead." and the output of odometry/filtered is completely wrong, which seems to be random results.

DaDaLee gravatar imageDaDaLee ( 2017-06-01 06:51:54 -0600 )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

2 followers

Stats

Asked: 2017-05-22 01:14:02 -0600

Seen: 526 times

Last updated: Jun 16 '18