robot_localization (fake) IMU + gps setup Error

asked 2021-02-18 16:37:15 -0500

tdoe321
3 ●3 ●4 ●7

Hello, I am working on getting the robot_localization package up and running to estimate pose using an IMU and a gps, but I'm having a bit of trouble. I've read through the whole docs and I'm still getting an error:

[ERROR] [1613686575.266536469]: Could not obtain transform from odom->base_link
[ WARN] [1613686575.592023869]: Could not obtain map->base_link transform. Will not remove offset of navsat device from robots origin.
[ WARN] [1613686576.692104170]: Could not obtain transform from base_link to map. Error was "map" passed to lookupTransform argument target_frame does not exist.

Currently I'm generating fake gps and imu data over /gps/fix and /imu/data with with the following code (simplified):

def main(args=None):
    rospy.init_node('gps_imu_node', anonymous=True)

    gps_pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10)
    imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # GPS
        msg = NavSatFix()
        msg.header = Header()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "base_link"

        # Position in degrees.
        msg.latitude = 57.047218
        msg.longitude = 9.920100

        gps_pub.publish(msg)

        # IMU
        msg = Imu()
        msg.header = Header()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "base_link"

        imu_pub.publish(msg)
        rate.sleep()

I'm filling it with data, but for example's sake I've taken that out, as that's not related to my error (I don't think).

Additionally, I'm running the navsat_transform_node with the following paramaters:

<launch>
 <!-- -->
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">

    <param name="magnetic_declination_radians" value="0"/>
    <param name="yaw_offset" value="0"/>
    <param name="zero_altitude" value="true"/>


    <param name="broadcast_cartesian_transform" value="false"/>
    <param name="publish_filtered_gps" value="true"/>


    <param name="use_odometry_yaw" value="false"/>
    <param name="wait_for_datum" value="false"/>


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

  </node>

</launch>

And I'm running ekf_localization_node with the following yaml config file:

frequency: 50

two_d_mode: true

publish_tf: true

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

imu0: /imu/data
imu0_config: [false, false, false,
              false, false, true,
              false, false, false,
              false, false, true,
              true, false, false]
imu0_differential: false

odom0: /odometry/gps
odom0_config: [false, false, false,
               false, false, false,
               true, true, false,
               false, false, true,
               false, false, false]
odom0_differential: false

process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0 ...

(more)

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2021-02-23 16:56:25 -0500

xaru8145
105 ●8 ●13 ●17

updated 2021-02-23 17:07:00 -0500

Moving all the comments to a complete answer:

1.Change world_frame: odom so the EKF publishes the odom→base_link transform. If world frame is set to map it will publish map→odom transform and you will need something else generating the odom->base_link transform

2.Publish a static tf between map->odom such as a null rotation RPY(0,0,0):

<node pkg="tf2_ros" type="static_transform_publisher" name="map_odom" args="0 0 0 0 0 0 map odom" />

3.Make sure you fill up the covariance diagonal values of the sensor messages. The covariances indicates the confidence of a measurement and the filter weights the covariances to integrate those measurements. If there are no covariances the filter will not perform as expected.

4.Fill up your IMU message at least with the orientation. If it its for testing purposes set the orientation to (0,0,0), which in quaternions is:

imu_msg.orientation.x = 0
imu_msg.orientation.y = 0
imu_msg.orientation.z = 0
imu_msg.orientation.w = 1

5.Change odom0_config to (in case you do not measure height and or two_d_mode=true):

odom0_config: [true, true, false,
                     false, false, false,
                     false false, false,
                     false, false, false,
                     false, false, false]

6.If you are not populating the IMU msg with angular rate and linear accelerations, I would suggest you set imu0_config lines 4 and 5 to false.

edit flag offensive delete link

add a comment

answered 2021-02-19 01:11:39 -0500

cocodmdr
26 ●4 ●6 ●7

Hello,

image description

See this link of the documentation.

I believed the error comes from these parameters that are not properly set.

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

You should maybe have an other instance of EKF like the one bellow that publishes odom->baselink.

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

Best wishes,

Cocodmdr

edit flag offensive delete link

Comments

Ok, so you're saying I need 2 instances of ekf_localization_node One that is publishing the transform from map->odom and one that is publishing the transform from odom->base_link?

tdoe321 ( 2021-02-19 08:43:24 -0500 )edit

If you are not going to use the odometry output from the EKF for navigation purposes, you do not need 2 instances of robot_localization. The 2 instances are used because you need continuous data and the GPS data is subject to discrete discontinuities (“jumps”). In order to run the 2nd instance you would need odometry sources like visual or wheel odometry.

As @cocodmdr suggests, change the world_frame: odom. You also need to also need to publish a static tf between map->odom. In your case they will be the same so:

<node pkg="tf2_ros" type="static_transform_publisher" name="map_odom" args="0 0 0 0 0 0 map odom" />

Let me know if this solves your problem.

Best,

Xavi

xaru8145 ( 2021-02-23 08:53:43 -0500 )edit

My understanding was that I am using the IMU to generate input for an odom frame. If I do what you say, will that be removing the IMU's input from the EKF?

tdoe321 ( 2021-02-23 09:10:38 -0500 )edit

The inputs to the EKF are used to estimated the position of a robot inside a frame. In order for the filter to work properly you need to make sure you measure (across all different sensors) at least a pose variable. The GPS will give you the position and the IMU will give you the orientation. With that you have the pose (position+orientation) of a robot in the odom frame. The odom and world frames are fix and they can be the same. The EKF filter will start producing its estimates in the origin (0,0,0,) of the odom frame. So one case where it might be useful to put the world frame in a different position from the odom frame iis if you want to reference your position from another one (such as a base station). For simplification you can make them both have the same origin ...(more)

xaru8145 ( 2021-02-23 09:28:49 -0500 )edit

So, when I run it as you say with the world_frame set to odom and with a tf from map to odom, the error goes away and it seems to be running.

However, the output on both /gps/filtered and odometry/gps is filled with nans and the output on /odometry/filtered is always zero (even when I'm varying the gps in time).

I seem to be able to vary the imu in time and get a response from that on odometry/filtered

tdoe321 ( 2021-02-23 11:54:09 -0500 )edit

Glad it works now! This could be because you are creating sensor messages without covariance. I would recommend doing some research on how the Kalman Filter works. The covariances indicate the confidence of a measurement and the filter weights the covariances to integrate those measurements. Try setting the covariances for your GPS and IMU messages. Also fill up your IMU message since it is empty right now. If it its for testing purposes set the orientation to (0,0,0), which in quaternions is:

imu_msg.orientation.x = 0
imu_msg.orientation.y = 0
imu_msg.orientation.z = 0
imu_msg.orientation.w = 1

xaru8145 ( 2021-02-23 12:51:01 -0500 )edit

Also, your odom0_config is wrong. You are integrating linear velocity x,y components and angular velocity in the yaw component for the GPS. The GPS measures lat/long so you have to integrate x,y position components:

odom0_config: [true, true, false,
               false, false, false,
               false false, false,
               false, false, false,
               false, false, false]

If you are not populating the IMU msg with accelerations, I would suggest you set imu0_config lines 4 and 5 to false. I would suggest you read the robot localization documentation more carefully. I know it is a lot of information and it is hard to digest but we all have been there. Good luck!

xaru8145 ( 2021-02-23 12:59:00 -0500 )edit

Ok, Adding in covariance to the diagonals of the covariance matrices seems to have given me non-nan output, which is great. I've set the values of the diagonals to some small values EX: 0.001 because I trust them. Thanks for that.

However, it's still not giving me output that makes sense. I'm varying the gps location with time, and the /odometry/gps topic is changing it's x and y location as I would expect. The /odometry/filtered topic, on the other hand, is not giving me output that changes at all. The values are all zero for pose. I would expect these to change as they should be my estimate in the odom frame (if I understand this correctly). Because these values are not changing, the output on /gps/filtered is also not changing (which isn't what I want, but it is expected because ...(more)

tdoe321 ( 2021-02-23 13:23:00 -0500 )edit

see more comments

robot_localization (fake) IMU + gps setup Error

2 Answers

Comments

Question Tools

Stats

Related questions

robot_localization (fake) IMU + gps setup Error edit

2 Answers

Comments

Question Tools

Stats

Related questions

robot_localization (fake) IMU + gps setup Error