0
$\begingroup$

Rosanswers logo

I ma trying to use the robot_localization package to fuse odometry, imu, and gps data according to link.

As such I have two ekf nodes and a navsat transform node. In the documentation it says that the navsat transform node can work by using the first GPS reading as the datum, as far as I am aware this then means that the wait_for_datum parameter should be set to false.

For me, this results in a neverending loop where ekf_localization_map waits on /odometry_gps from navsat_transform, navsat_transform then waits on /odometry/filtered/map from ekf_localization_map. The whole time navsat_transform spams with messages of

[navsat_transform_node-8] [INFO] [1641296576.110339410] [navsat_transform]: Datum (latitude, longitude, altitude) is (37.50, -122.00, 0.03)
[navsat_transform_node-8] [INFO] [1641296576.110379312] [navsat_transform]: Datum UTM coordinate is (10S, 588391.24, 4150810.87)

I'm not really sure how to set this up properly? Note, I think my setup more or less corresponds to the example param and launch files from the robot_localization github repo.

My config and launch files: config file

# For parameter descriptions, please refer to the template parameter files for each node.
ekf_localization_odom:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.0 #0.1
    two_d_mode: true
    transform_time_offset: 0.0 #0.01
    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/wheel
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  false,
                   false, false, true,
                   false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    imu0: imu/data
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  true,  true,  true,
                  true,  true,  true]
    imu0_nodelay: true # false
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true # trying false with 2d_mode: true

    use_control: false

ekf_localization_map:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.0 #0.1
    two_d_mode: true
    transform_time_offset: 0.0 #0.01
    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/wheel
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  false,
                   false, false, true,
                   false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    odom1: odometry/gps
    odom1_config: [true,  true,  true,
                   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,  true,
                  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 # trying false with 2d_mode: true

    use_control: false

navsat_transform:
  ros__parameters:
# Frequency of the main run loop
        frequency: 30.0

# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially
# important if you have use_odometry_yaw set to true. Defaults to 0.
        delay: 3.0

# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame.
# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using
# it. 

# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it,
# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory.
        magnetic_declination_radians: 0.054977871 # 0.076096355

# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it
# doesn't, enter the offset here. Defaults to 0.
        yaw_offset: 1.570796327

# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false.
        zero_altitude: false

# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false.
        broadcast_cartesian_transform: false

# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform. 
# Note that broadcast_utm_transform still has to be enabled. Defaults to false.
        broadcast_utm_transform_as_parent_frame: false

# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as
# /gps/filtered. Defaults to false.
        publish_filtered_gps: false # true

# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the
# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this!
# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw
# if your yaw data is based purely on integrated velocities. Defaults to false.
        use_odometry_yaw: false

# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists,
# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message.
        wait_for_datum: false

# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the
# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees,
# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east.
        # datum: [55.877627, 8.463545, 0.0]

launch file

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node


def generate_launch_description():
    # get directories
    tractor_sim_dir = get_package_share_directory("tractor_sim")

    # Create the launch configuration variables
    use_sim_time = LaunchConfiguration("use_sim_time")

    # declare launch arguments
    declare_use_sim_time = DeclareLaunchArgument("use_sim_time", default_value="True")

    # Specify the actions
    robot_localization_odom = Node(
        package="robot_localization",
        executable="ekf_node",
        name="ekf_localization_odom",
        output="screen",
        parameters=[
            {"use_sim_time": use_sim_time},
            os.path.join(
                tractor_sim_dir,
                "config",
                "dual_ekf_navsat_toolcarrier.yaml",
            ),
        ],
        remappings=[("odometry/filtered", "odometry/filtered/odom")],
    )
    robot_localization_map = Node(
        package="robot_localization",
        executable="ekf_node",
        name="ekf_localization_map",
        output="screen",
        parameters=[
            {"use_sim_time": use_sim_time},
            os.path.join(
                tractor_sim_dir,
                "config",
                "dual_ekf_navsat_toolcarrier.yaml",
            ),
        ],
        remappings=[("odometry/filtered", "odometry/filtered/map")],
    )
    navsat_transform = Node(
        package="robot_localization",
        executable="navsat_transform_node",
        name="navsat_transform",
        output="screen",
        parameters=[
            {"use_sim_time": use_sim_time},
            os.path.join(
                tractor_sim_dir,
                "config",
                "dual_ekf_navsat_toolcarrier.yaml",
            ),
        ],
        remappings=[("odometry/filtered", "odometry/filtered/map")],
    )

    return LaunchDescription(
        [
            declare_use_sim_time,
            robot_localization_odom,
            robot_localization_map,
            navsat_transform,
        ]
    )

Originally posted by morten on ROS Answers with karma: 198 on 2022-01-04

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

For me, this results in a neverending loop where ekf_localization_map waits on /odometry_gps from navsat_transform, navsat_transform then waits on /odometry/filtered/map from ekf_localization_map.

You should still be getting publications on /odometry/filtered/map. That EKF instance will start publishing as soon as it receives a single sensor message from any sensor input. So it's not waiting on anything from navsat_transform, but navsat_transform is waiting for /odometry/filtered/map. So you should just echo /odometry/filtered/map. I suspect it actually is publishing.

But I don't think this is your issue. The message you see spamming will keep printing every time we receive a GPS message, but we have not yet received all the other data required to compute the transform. The criteria to get past that condition are here:

https://github.com/cra-ros-pkg/robot_localization/blob/5d2e88268d988135af8642f4814cac7e2940cc87/src/navsat_transform.cpp#L234

You can see that it needs IMU data, odometry data (which you are suspecting), and GPS data. In this case, your IMU appears to be publishing on imu/data, but navsat_transform_node is subscribing to imu:

https://github.com/cra-ros-pkg/robot_localization/blob/5d2e88268d988135af8642f4814cac7e2940cc87/src/navsat_transform.cpp#L181

I think you need a remap. I'm thinking you can confirm this by looking at the node and topic info for navsat_transform_node and the imu/data topic, respectively.


Originally posted by Tom Moore with karma: 13689 on 2022-01-14

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

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