Ask Your Question
0

robot_localization not producing data while fusing Odometry

asked 2019-06-13 13:56:03 -0500

zlacelle gravatar image

updated 2019-06-13 14:28:31 -0500

I have a robot_localization node, and for testing purposes I want to fuse in a very accurate GPS receiver as if it was a normal smooth odometry sensor (e.g. wheel encoders). I have a nav_msgs/Odometry topic producing an ENU-formatted UTM position, as well as a sensor_msgs/Imu topic filled with FLU-formatted rates and accelerations.

However, I get the following error, and robot_localization doesn't seem to produce data:

Could not obtain transform from gps_enu to odom. Error was "odom" passed to lookupTransform argument target_frame does not exist.

This doesn't make sense, because I'm running this node specifically to produce this transform!

EDIT: I've found the lines in robot_localization that are the culprit, but I'm not sure how to proceed. In ros_filter.cpp:

    if (poseCallbackData.updateSum_ > 0)
    {
      // Grab the pose portion of the message and pass it to the poseCallback
      geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();
      posPtr->header = msg->header;
      posPtr->pose = msg->pose;  // Entire pose object, also copies covariance

      geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
      poseCallback(pptr, poseCallbackData, worldFrameId_, false);
    }

For Odometry messages, it's taking the Pose part and attempting to transform it all the way to the "world" frame (which is odom).

END EDIT

EDIT #2: Switching on the "_differential" flag works because it integrates the pose as a twist instead. However, then my covariances grow with time, which I don't want (they should remain bounded, as the covariances of the poses are bounded).

END EDIT #2

Here's my configuration:

frequency: 50                                                                                                                                                                                               
sensor_timeout: 0.1                                                                                                                                                                                         
transform_time_offset: 0.1                                                                                                                                                                                  
transform_timeout: 0                                                                                                                                                                                        

odom_frame: odom                                                                                                                                                                                            
base_link_frame: base_link                                                                                                                                                                                  
world_frame: odom                                                                                                                                                                                           

two_d_mode: false                                                                                                                                                                                           

imu0: /sensor/imu/data_enu                                                                                                                                                              
imu0_config:                                                                                                                                                                                                
  [false, false, false,                                                                                                                                                                                     
  false, false, false,                                                                                                                                                                                      
  false, false, false,                                                                                                                                                                                      
  true, true, true,                                                                                                                                                                                         
  false, false, false ]                                                                                                                                                                                                                                
imu0_differential: false                                                                                                                                                                                                                       
imu0_relative: false                                                                                                                                                                                        
imu0_queue_size: 10                                                                                                                                                                                         
imu0_remove_gravitational_acceleration: false                                                                                                                                                               

odom0: /sensor/gps/utm_enu                                                                                                                                                              
odom0_config:                                                                                                                                                                                               
  [true, true, true,                                                                                                                                                                                        
  true, true, true,                                                                                                                                                                                         
  false, false, false,                                                                                                                                                                                      
  false, false, false,                                                                                                                                                                                      
  false, false, false]                                                                                                                                                                                      
odom0_differential: false                                                                                                                                                                                   
odom0_relative: true                                                                                                                                                                                        
odom0_queue_size: 10

publish_tf: true                                                                                                                                                                                            
publish_acceleration: true                                                                                                                                                                                  
print_diagnostics: true                                                                                                                                                                                     

debug: false                                                                                                                                                                                                
debug_out_file: /mnt/ramdisk/relative_localization_filter_debug.txt                                                                                                                                                                                                                                                                                                                            

use_control: false                                                                                                                                                                                          

alpha: 0.001                                                                                                                                                                                                
kappa: 0                                                                                                                                                                                                    
beta: 2

Here's an example output of each message:

---
header: 
  seq: 77976
  stamp: 
    secs: 1560451949
    nsecs: 823664380
  frame_id: "imu_link_enu"
orientation: 
  x: -0.00890241184106
  y: -0.0122976751383
  z: 0.941373781502
  w: -0.337023616012
orientation_covariance: [2.4716113409053997e-07, 0.0, 0.0, 0.0, 2.466269938791087e-07, 0.0, 0.0, 0.0, 7.467046920414323e-08]
angular_velocity: 
  x: -0.00106572255027
  y: 0.00128244573716
  z: -7.25421487004e-05
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0310777239501
  y: 0.000748675898649
  z: -0.0431123189628
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
---
header: 
  seq: 79161
  stamp: 
    secs: 1560451990
    nsecs: 483678060
  frame_id: "gps_enu"
child_frame_id: "imu_link_flu"
pose: 
  pose: 
    position: 
      x: 308876.393439
      y: 4310470.5311
      z: -77.3288650513
    orientation: 
      x: -0.00886148846847
      y: -0.0123107011776
      z: 0.941312561823
      w: -0.337195168431
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2019-07-03 04:33:20 -0500

Tom Moore gravatar image

So the EKF, as you have it configured, is going to generate the odom->base_link transform. That's the only transform it will produce. This transform represents your robot's pose in the odom frame.

You are then handing it a pose in the gps_enu frame, and asking it to fuse that it its state estimate. But the filter's first step will be to note that the gps_enu frame is not the odom frame, so in order to use that data, it need to transform it into the odom frame. It can then fuse it in its state estimate.

The problem is that you are not providing a transform from the gps_enu frame to the odom frame, so the filter has no idea how those world frames are related to each other.

If your expectation is for the filter to just use the pose from your GPS data as ground truth, you can do two things:

  1. Change the odom_frame parameter to gps_enu
  2. Provide a static transform from odom -> gps_enu, and make that static transform the identity transform
edit flag offensive delete link more

Comments

Ah, understood. The gps_enu frame should really be the origin of the UTM zone in ENU convention, so then the gps_enu -> odom transform (assuming we initialize odom to the origin on startup) would be the 6DOF pose of the vehicle in UTM upon initialization, correct?

I was making the mistake of setting gps_enu to the GPS antenna location on the vehicle, which now that I think about it clearly doesn't make sense since the GPS measurements are really w.r.t. the UTM zone origin.

zlacelle gravatar imagezlacelle ( 2019-07-03 08:30:19 -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: 2019-06-13 13:56:03 -0500

Seen: 166 times

Last updated: Jul 03