asked 2021-06-24 04:24:54 -0600

meng100 gravatar image

updated 2021-06-28 05:39:09 -0600

Hi, Anyone knows how to use ekf_localizer? I'm basically following this PR.

I'm running Quick Start in Runtime Manager which uses ndt_localization with LGSVL simulator, and then enabled the ekf_localizer and vehicle_status_converter in Computing tab. However, when the vehicle is moving in autonomous mode with Mission Planning and Motion Planning modules, in RViz, the ekf_pose is not moving when the vehicle starts to move, but ndt_pose moves together with the vehicle.

Anyone knows why ekf_localizer is not working here? Help needed! Thanks!

Sorry I don't have enough points to upload a picture here.

edit retag flag offensive close merge delete


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.
TakaHoribe gravatar image TakaHoribe  ( 2021-06-27 06:45:07 -0600 )edit

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

    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:

    x: 0.0
    y: 0.0
    z: 0.0   
    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!

meng100 gravatar image meng100  ( 2021-06-27 23:53:15 -0600 )edit

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.

 * /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).

TakaHoribe gravatar image TakaHoribe  ( 2021-06-28 20:42:44 -0600 )edit

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.

TakaHoribe gravatar image TakaHoribe  ( 2021-06-28 20:56:50 -0600 )edit

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!

meng100 gravatar image meng100  ( 2021-06-30 02:57:35 -0600 )edit

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.

TakaHoribe gravatar image TakaHoribe  ( 2021-07-01 04:37:18 -0600 )edit

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?


  seq: 85
    secs: 1625198945
    nsecs:  45853952
  frame_id: "/base_link"
    x: -0.0160068532671
    y: 0.0
    z: 0.0
    x: 0.0
    y: 0.0
    z: -0.000139225506364

rostopic info:

Type: geometry_msgs/TwistStamped

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

 * /vel_relay (http://meng:41149/)
meng100 gravatar image meng100  ( 2021-07-01 23:12:43 -0600 )edit