Ask Your Question
2

Problem while integrating GPS data into robot_localization

asked 2017-01-25 10:54:28 -0500

GuidoBartoli gravatar image

updated 2017-01-26 01:50:52 -0500

Hi to Tom and everybody,

I'm currently making some experiments with robot_localization, and I'm having some problems with the integration of GPS data for global localization of a non-holonomic mobile robot constrained to a 2D plane (no Z coordinate and no roll/pitch).

Here is the overview of my setup for odometry which is very similar to the one described in this section of the wiki where the use of a dual EKF node is discussed.

Odometry overview

  • X and Y are bidimensional coordinates
  • Vx and Vy are linear velocities
  • H^, X^^ and Y^^ are angular velocity and linear accelerations, respectively
  • The topic name is inside parentheses
  • The provided TF transformation is inside square brackets

This is my launch file (only the part relevant to replay the bag file):

<launch>

<!-- Reference frames -->
<arg name="world_frame"  default="earth"/>
<arg name="map_frame"    default="map_link"/>
<arg name="odom_frame"   default="odom_link"/>
<arg name="robot_frame"  default="base_link"/>
<arg name="camera_frame" default="camera_link"/>

<!-- Odometry topics -->
<arg name="wheels_odom"  default="/odometry/wheels"/>
<arg name="imu_odom"     default="/odometry/imu"/>
<arg name="gps_odom"     default="/odometry/gps"/>
<arg name="navsat_odom"  default="/odometry/navsat"/>
<arg name="visual_odom"  default="/odometry/visual"/>
<arg name="local_odom"   default="/odometry/local"/>
<arg name="global_odom"  default="/odometry/global"/>

<!-- Sensor fusion 1 -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf1_local" clear_params="true" output="screen">
    <param name="frequency"         value="50"/>
    <param name="sensor_timeout"    value="0.2"/>
    <param name="two_d_mode"        value="true"/>
    <param name="map_frame"         value="$(arg map_frame)"/>
    <param name="odom_frame"        value="$(arg odom_frame)"/>
    <param name="base_link_frame"   value="$(arg robot_frame)"/>
    <param name="world_frame"       value="$(arg odom_frame)"/>

    <param name="use_control"       value="false"/>
    <param name="print_diagnostics" value="true"/>
    <param name="debug"             value="false"/>
    <param name="debug_out_file"    value="~/Log/ekf1.txt"/>

    <remap from="odometry/filtered" to="$(arg local_odom)"/>

    <param name="odom0" value="$(arg wheels_odom)"/>
    <rosparam param="odom0_config">
        [ false, false, false,
          false, false, false,
          true,  true,  false,
          false, false, false,
          false, false, false ]
    </rosparam>
    <param name="odom0_differential" value="false"/>
    <param name="odom0_relative"     value="false"/>
    <param name="odom0_queue_size"   value="10"/>
    <param name="odom0_nodelay"      value="true"/>

    <param name="imu0" value="$(arg imu_odom)"/>
    <rosparam param="imu0_config">
        [ false, false, false,
          false, false, false,
          false, false, false,
          false, false, true,
          true,  true,  false ]
    </rosparam>
    <param name="imu0_differential" value="false"/>
    <param name="imu0_relative"     value="false"/>
    <param name="imu0_queue_size"   value="10"/>
    <param name="imu0_nodelay"      value="false"/>
    <param name="imu0_remove_gravitational_acceleration" value="false"/>
</node>

<!-- GPS coordinates transformation -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
    <param name="frequency"            value="20"/>
    <param name="yaw_offset"           value="1.570796327"/>
    <param name="zero_altitude"        value="true"/>
    <param name="publish_filtered_gps" value="false"/>
    <param name="use_odometry_yaw"     value="false"/>
    <param name="wait_for_datum"       value="true"/>
    <param name="broadcast_utm_transform" value="false"/>
    <param name="delay"                value="3"/>
    <param name="magnetic_declination_radians" value="0.00383972"/>
    <rosparam param="datum">[43.542883, 11.571365, 0]</rosparam>

    <remap from="imu/data"          to="$(arg imu_odom)"/>
    <remap from="odometry/filtered" to="$(arg local_odom)"/>
    <remap from="gps/fix"           to="$(arg gps_odom)"/>
    <remap from="odometry/gps"      to="$(arg navsat_odom)"/>
</node>

<!-- Sensor fusion 2 -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf2_global" clear_params="true" output="screen">
    <param name="frequency"         value="10"/>
    <param name ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2017-01-26 02:35:10 -0500

Tom Moore gravatar image

Thanks for the nicely formatted question! I'm not positive this will do anything, but before I start looking at bag files, I do see that your navsat_transform_node instance is listening to the wrong EKF output. navsat_transform_node will work with a two-level EKF setup, but doesn't require it, so this line in your navsat_transform_node configuration:

<remap from="odometry/filtered" to="$(arg local_odom)"/>

should be

<remap from="odometry/filtered" to="$(arg global_odom)"/>

The circular dependency between navsat_transform_node and the EKF is intentional. Again, I'm not sure that's your issue, but we should fix it before continuing.

edit flag offensive delete link more
1

answered 2017-01-26 08:26:36 -0500

GuidoBartoli gravatar image

updated 2017-01-27 02:19:05 -0500

Ok, I set the input of navsat to the output of the second EKF, the situation is a little better, i.e. setting map_link as fixed in RVIZ, I can see odom-->base TF remaining consistent as before, since it comes from the first EKF, but map_link-->odom_link stays a bit closer to the real trajectory, even if after a bit it still diverges very much, even if it seems the raw GPS data are not so noisy.

So, the overview graph is now like that?

image description

Some more questions about this setup:

  • With your correction, the first warning about odom-->map transform disappeared, but the second one about map-->base is still there. I have GPS data produced at about 1Hz, do I have to change navsat frequency to a lower value?
  • I also tried to set use_odometry_yaw="true" in navsat parameters, but from the node graph it still seems not using the IMU orientation for creating /odometry/navsat. Is this an intended behavior?
  • If I have understood well, looping /odometry/global into navsat and keeping /odometry/local separate from it have the effect of the first one producing map-->odom and the second odom-->base independently, right? This concept is still a bit obscure to me after reading this and your dual_ekf ROS example... :(
  • The topic /odometry/global from the second EKF has frame_id = map and child_frame_id = base_link. Should not the second be equal to odom?
  • A small note, in this wiki page, you mention the datum parameter has 5 arguments, but it seems the latest version accepts only the first 3, frames are not necessary.

Thanks,

Guido

edit flag offensive delete link more

Comments

This is an excellent graph with good questions; can you please edit the original question with this information? They try to avoid turning these questions into threads.

Tom Moore gravatar imageTom Moore ( 2017-02-01 07:09:53 -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

2 followers

Stats

Asked: 2017-01-25 10:54:28 -0500

Seen: 701 times

Last updated: Jan 27 '17