autoware.ai ekf_localizer

edit

Could you check the following?

1. Check if the topics are connected correctly with the ekf_localizer.
2. Check if the input twist and pose to the ekf_localizer has a reasonable value (e.g. don’t send zero velocity when the input pose (maybe ndt_pose?) is moving)
3. Check if any errors are not on the terminal. The ekf ignores the old timestamp data or outliers that is too far from the current estimation. You can check if the latest information is ignored from the error message.

I've checked for No.2, my ekf_pose has a value

pose: 
  position: 
    x: -10.272908075
    y: 41.1519393921
    z: -2.73687601089

which is where I draw my initial position for NDT_Localizer using 2D Pose Estimate, but it doesn't change when the vehicle is moving (ndt_pose changes along the way).

Whereas for ekf_twist, the value is always 0:

twist:    
   linear: 
    x: 0.0
    y: 0.0
    z: 0.0   
   angular: 
    x: 0.0
    y: 0.0
    z: 0.0

I suppose that's not right behaviour for both of the above-mentioned input value, could you please shed some light on what could the possible causes be?

By the way, could you please elaborate on point 1 and 3? Much appreciated!

It seems the OUTPUTs from the ekf_localizer have consistency (pose doesn't move and velocity is zero).

Could you check the INPUTs to ekf_localizer? First, check the input topics with

$ rosnode info /ekf_localizer

In my environment, the output is like below.

Subscriptions: 
 * /can_twist [unknown type]
 * /initialpose [unknown type]
 * /input_pose_with_cov_UNUSED [unknown type]
 * /input_twist_with_covariance_UNUSED [unknown type]
 * /ndt_pose [unknown type]

Then check if the inputs have reasonable values (in my case, /can_twist and /ndt_pose).

For point 1, I meant you need to check if the related topics are really published and subscribed by the ekf. You can just use rosnode info and rostopic echo to check them.

For point 3, please check the terminal that launches the runtime_manager. If some problem occurs, ekf prints some error messages. Alternatively, all messages are published on /rosout, so you can run something like this:

$ rostopic echo /rosout | grep ekf_localizer -A 5 -B 5

to get the message from the ekf.

Note: you can set set_debug_output flag to true to get all information from ekf in App in runtime_manager.

Thanks for your replies! I checked rosnode info /ekf_localizer, and checked both /can_twist and /ndt_pose, /ndt_pose has reasonable value and it changes when the vehicle moves whereas /can_twist doesn't have any value. I searched a bit online, according to this, /can_twist is converted from /vehicle_status, so I went to check /vehicle_status it also has no input value. As I am using LGSVL simulator as the input source to autoware, I suppose LGSVL doesn't have a sensor for /vehicle_status input.

Not sure if you're familiar with LGSVL, if you do please share, thanks!

Not much familiar with lgsvl simulator, but maybe the simulator interface publishes the ego-velocity information. If it is a TwistStamped type, you can connect it with EKF as a measurement source (replace with the default: /can_twist). If not, you need to somehow convert the velocity topic into a twist type.

And I think the pose information (/ndt_pose) is not considered in the EKF due to a large time delay as well, because if the pose info is considered, the ekf_pose should move at least slightly even if the measurement velocity is zero. You should check the error message.

Thanks for your reply! May I check with you what data should be found in /can_twist? I found /estimate_twist, published by /ndt_matching, not sure if it's useful?

data:

header: 
  seq: 85
  stamp: 
    secs: 1625198945
    nsecs:  45853952
  frame_id: "/base_link"
twist: 
  linear: 
    x: -0.0160068532671
    y: 0.0
    z: 0.0
  angular: 
    x: 0.0
    y: 0.0
    z: -0.000139225506364

rostopic info:

Type: geometry_msgs/TwistStamped

Publishers: 
 * /ndt_matching (http://meng:44485/)

Subscribers: 
 * /vel_relay (http://meng:41149/)