0
$\begingroup$

Rosanswers logo

I'm using ros Indigo, and trying to fuse IMU and GPS, I don't have any odom source. Reading forum and the roscon presentation about the robot_localization package, I tried the following configuration:

   <launch>
  <!-- gps and imu fusion -->
    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
    <param name="magnetic_declination_radians" value="0.36878808"/>
    <param name="zero_altitude" value="false"/>
    <param name="publish_filtered_gps" value="true"/>
    <param name="broadcast_utm_transform" value="true"/>
    <param name="wait_for_datum" value="true"/>

    <remap from="/gps/fix" to="/gps"/>
    <remap from="/imu/data" to="/imu"/>
    <remap from="/odometry/filtered" to="/odometry/map"/>
  </node>

 <!-- map frame -->
  <node pkg="robot_localization" type="ekf_localization_node"  name="map_transform" clear_params="true">

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

    <param name="frequency" value="30"/>
    <param name="sensor_timeout" value="2"/>
    <param name="two_d_mode" value="true"/>

    <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="map"/>

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

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

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

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

    <param name="print_diagnostics" value="true"/>
    <param name="debug"           value="false"/>
    <param name="debug_out_file"  value="$(env HOME)/adroit_files/debug_ekf_localization.txt"/>

    <remap from="odometry/filtered" to="/odometry/map"/>

  </node>



<!-- odom frame -->
  <node pkg="robot_localization" type="ekf_localization_node"  name="odom_transform" clear_params="true">

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

    <param name="frequency" value="30"/>
    <param name="sensor_timeout" value="2"/>
    <param name="two_d_mode" value="true"/>

    <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="imu0_config">[false, false, false,
                                   false, false, true,
                                   false, false, false,
                                   false, false, true,
                                   true, false, false]</rosparam>

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

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


    <param name="print_diagnostics" value="true"/>
    <param name="debug"           value="false"/>
    <param name="debug_out_file"  value="$(env HOME)/debug_ekf_localization.txt"/>

</node>

</launch>

as result, I'm getting a drift that increases in time, and the vector is always perpendicular to robot.

This warning appear several times too

[ WARN] [1465502165.359329185, 20.510000000]: Could not obtain base_link-> transform. Will not remove offset of navsat device from robot's origin.
[ WARN] [1465502167.550982133, 21.310000000]: Could not obtain transform from  to base_link. Error was Invalid argument passed to lookupTransform argument source_frame in tf2 frame_ids cannot be empty

I'm not using a real robot yet, for now I'm on gazebo, using hector_gazebo plugins for IMU and GPS.

IMU sample message:

  header: 
  seq: 10464
  stamp: 
    secs: 10
    nsecs: 918000000
  frame_id: base_link
orientation: 
  x: 0.019510418191
  y: -0.00138183083974
  z: -0.00971101353096
  w: 0.999761536739
orientation_covariance: [2.6030820491461885e-07, 0.0, 0.0, 0.0, 2.6030820491461885e-07, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00310991562662
  y: -0.00912178750215
  z: 0.0116196962976
angular_velocity_covariance: [2.5e-05, 0.0, 0.0, 0.0, 2.5e-05, 0.0, 0.0, 0.0, 2.5e-05]
linear_acceleration: 
  x: -2.21806651905
  y: 2.26472137133
  z: 6.24316872463
linear_acceleration_covariance: [2.5e-05, 0.0, 0.0, 0.0, 2.5e-05, 0.0, 0.0, 0.0, 2.5e-05]
---

GPS sample message:

 header: 
  seq: 244
  stamp: 
    secs: 61
    nsecs: 500000000
  frame_id: base_link
status: 
  status: 0
  service: 1
latitude: -23.7134276213
longitude: -46.5536684712
altitude: -3.02017915306
position_covariance: [25.0, 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 25.0]
position_covariance_type: 2
---

The ekf output after some time, with the robot stoped in gazebo

header: 
      seq: 1747
      stamp: 
        secs: 222
        nsecs:  58000000
      frame_id: odom
    child_frame_id: base_link
    pose: 
      pose: 
        position: 
          x: 44.8376708689
          y: -2.12248261091
          z: 0.0
        orientation: 
          x: 0.0
          y: 0.0
          z: -0.0360110406313
          w: 0.99935139213
      covariance: [15.30443876297224, 85.29210019200963, 0.0, 0.0, 0.0, 7.403568159067386e-13, 85.2921001920097, 1661.3211796485384, 0.0, 0.0, 0.0, 7.592048297986206e-12, 0.0, 0.0, 3.331484049746691e-07, -8.211483406385409e-17, -3.2792082619740134e-15, 0.0, 0.0, 0.0, -8.211483406385409e-17, 3.3296398857637426e-07, -1.4560869721212304e-21, 0.0, 0.0, 0.0, -3.2792082619740122e-15, -1.4560869721108193e-21, 3.3296398857631613e-07, 0.0, 7.403568159067387e-13, 7.592048297986206e-12, 0.0, 0.0, 0.0, 9.99999523817672e-10]
    twist: 
      twist: 
        linear: 
          x: 1.06759861895
          y: -0.0267929278598
          z: 0.0
        angular: 
          x: 0.0
          y: 0.0
          z: -0.00130429742184
      covariance: [0.23737282750479055, -0.15158265506748317, 0.0, 0.0, 0.0, 1.405394133138543e-15, -0.1515826550674825, 23.524227691364764, 0.0, 0.0, 0.0, 2.5224508749013602e-14, 0.0, 0.0, 3.3305613291777575e-07, 9.51454551412301e-26, -2.0191178601715107e-24, 0.0, 0.0, 0.0, 9.514545514123008e-26, 3.3223139406571205e-07, 6.682293294008273e-30, 0.0, 0.0, 0.0, -2.0191178601715103e-24, 6.682280875348762e-30, 3.3223139406571205e-07, 0.0, 1.4053941331385318e-15, 2.52245087490136e-14, 0.0, 0.0, 0.0, 2.4165639655974852e-05]
    ---

Doesn't anyone have a clue about what i'm doing wrong? I've tried some other configurations, but none of then worked

Update 1:

I added this to my urdf file:

  <gazebo>
    <plugin name="gps" filename="libhector_gazebo_ros_gps.so">
      <alwayson>1</alwayson>
      <updaterate>50.0</updaterate>
      <bodyname>base_link</bodyname>
      <topicName>/gps</topicName>
      <service>1</service>
      <frameId>gps_link</frameId>
      <velocityTopicName>/gps/fixvelocity</velocityTopicName>
      <drift>5.0 5.0 5.0</drift>
      <referenceLatitude>-23.713367</referenceLatitude>
      <referenceLongitude>-46.553640</referenceLongitude>
      <gaussiannoise>0.1 0.1 0.1</gaussiannoise>
      <velocitydrift>0 0 0</velocitydrift>
      <velocitygaussiannoise>0.1 0.1 0.1</velocitygaussiannoise>
    </plugin>

and I have the tf from gps_link to base_link


Originally posted by torugobeck on ROS Answers with karma: 13 on 2016-06-09

Post score: 1


Original comments

Comment by rajnunes on 2016-06-11:
How exactly did you get the gps plugin to work..?

Comment by torugobeck on 2016-06-13:
added the plugin configuration at the topic, to big to post here

Comment by Tom Moore on 2016-06-14:
Hmmm...this may be an issue. Can you post a bag?

Comment by torugobeck on 2016-06-14:
I generated without any node open, just the gazebo with the plugins https://www.dropbox.com/s/11tcvbdze5azhj8/2016-06-14-12-55-58.bag?dl=0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

So there are a lot of issues here:

  1. You appear to be feeding the output of your map_transform instance of ekf_localization_node into your odom_transform instance. That is probably not going to work well at all.
  2. You have wait_for_datum set to true for navsat_transform_node, but your other settings indicate that you don't actually want this set to true. I just realized that I need to document that parameter on the wiki, so that's my fault. In any case, it's not relevant for your use case.
  3. If all you have is IMU and GPS data, then you don't need to run two EKFs, as it won't really help you.

I did find a bug with the node crashinig when publish_filtered_gps was set to true. I have fixed it and it is in the upstream repository. Please pull down that version until I can get a new release out.

Anyway, here are the settings that worked for me. Please note that I added a static transform publisher for the base_link->gps transform:

<launch>

  <node name="bl_gps" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 1 base_link gps_link" />

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
  <param name="magnetic_declination_radians" value="0.36878808"/>
  <param name="zero_altitude" value="false"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="broadcast_utm_transform" value="true"/>
  <param name="wait_for_datum" value="false"/>

  <remap from="/gps/fix" to="/gps"/>
  <remap from="/imu/data" to="/imu"/>
</node>

<node pkg="robot_localization" type="ekf_localization_node"  name="ekf_odom" clear_params="true">

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

  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="2"/>
  <param name="two_d_mode" value="true"/>

  <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="imu0_config">[false, false, false,
                                 false, false, true,
                                 false, false, false,
                                 false, false, true,
                                 true, false, false]</rosparam>

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

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

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

  <param name="print_diagnostics" value="true"/>
  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="$(env HOME)/adroit_files/debug_ekf_localization.txt"/>

</node>

Originally posted by Tom Moore with karma: 13689 on 2016-06-15

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by torugobeck on 2016-06-15:
Hello Tom, thanks for your help. Actually I'm using Datum, I have a python script that set it. I'm using since I have a map that is streamed through map_server, and I want the origin of map to be the gps coordinate set in datum. I'll try this configuration with a static tf from map to odom.

Comment by torugobeck on 2016-06-15:
I just tried this configuration that you posted, with wait_for_datum and publish_filtered_gps set to false, but i'm still getting the drift in the position, without any other odom source, this continuous drift is expected?

Comment by Tom Moore on 2016-06-15:
Is your GPS drifting?

Comment by torugobeck on 2016-06-15:
I ploted some data in matlab, and it look pretty stable, as you can see here: https://www.dropbox.com/s/1rauxv3ajbkmvys/Captura%20de%20tela%202016-06-15%2015.25.08.png?dl=0

Comment by Tom Moore on 2016-06-15:
Try removing the linear acceleration from your state estimate (odom0 input).

Comment by torugobeck on 2016-06-15:
I'm using the same launch file that you posted, it already have this configuration. https://www.dropbox.com/s/9fbjdn9l1rumlzc/Captura%20de%20tela%202016-06-15%2017.32.png?dl=0 But Im' getting this output as result. Can you see any problem that may cause this?

Comment by Tom Moore on 2016-06-16:
No, the file I posted has X linear acceleration enabled (the last "true" value in imu0_config). I am suggesting that you set it to false instead.

Comment by torugobeck on 2016-06-16:
Sorry I thought that you were talking about the odom0 parameter, it worked well. I have one more doubt, the odom output is getting perpendicular to the robot, do you have any idea what may cause this? Thanks for the Help!!

Comment by Tom Moore on 2016-06-17:
Please read the "yaw_offset" parameter on the wiki here.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.