1
$\begingroup$

I am using ROS2 Foxy and Gazebo 11 in Ubuntu 20.04. I have a URDF description of a mobile robot that uses 4 wheels for mecanum drive. Using the robot_localization package, I am creating an EKF node that subscribes to the /wheel/odometry topic, to which the mecanum drive node publishes the odometry data. Also, the EKF node is subscribed to data published by IMU. However, the EKF node is not publishing any data on /odometry/filtered topic. The node does publish some data on /diagnostics topic, but that does not seem to help me. Here's my EKF config file-

ekf_filter_node:
    ros__parameters:

        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: false
        publish_acceleration: false
        publish_tf: true
        reset_on_time_jump: true
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

        odom0: wheel/odometry
        odom0_config: [true,  true,  false,
                       false, false, true,
                       true, true, false,
                       false, false, true,
                       true, true, false]
        odom0_queue_size: 2
        odom0_nodelay: false
        odom0_differential: false
        odom0_relative: false

        imu0: imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      true,  true,  true,
                      true, true, true]
        imu0_queue_size: 5
        imu0_nodelay: false
        imu0_differential: false
        imu0_relative: true
        imu0_remove_gravitational_acceleration: true

Here are the sample messages from the sensors:-

----IMU----

header:
  stamp:
    sec: 99
    nanosec: 1000000
  frame_id: base_link
orientation:
  x: -3.2484036403009136e-07
  y: -1.233682290372918e-07
  z: -5.640383185432385e-06
  w: 0.9999999999840328
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: -0.00017136447475541198
  y: 0.00013966822040950878
  z: 4.4918826200876715e-06
angular_velocity_covariance:
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
linear_acceleration:
  x: -0.020305982380950664
  y: -0.005017642374176924
  z: 9.765243743038877
linear_acceleration_covariance:
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003

----Gazebo Plugin that provides 4 wheel mecanum drive----

header:
  stamp:
    sec: 263
    nanosec: 153000000
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.0005238049487633187
      y: -0.00013799493152156435
      z: 0.17620003865237005
    orientation:
      x: -1.5027791139302502e-07
      y: 8.421334553204312e-09
      z: -4.986068853738537e-05
      w: 0.9999999987569447
  covariance:
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
twist:
  twist:
    linear:
      x: 9.15223527049435e-06
      y: 0.00028986655671066065
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 3.0622019018612144e-05
  covariance:
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001

Here's what the /diagnostics topic gets populated with:-

header:
  stamp:
    sec: 1642601976
    nanosec: 319066940
  frame_id: ''
status:
- level: "\x02"
  name: 'ekf_filter_node: odometry/filtered topic status'
  message: No events recorded.
  hardware_id: none
  values:
  - key: Events in window
    value: '0'
  - key: Events since startup
    value: '0'
  - key: Duration of window (s)
    value: '9.999977'
  - key: Actual frequency (Hz)
    value: '0.000000'
  - key: Target frequency (Hz)
    value: '0.000000'
  - key: Maximum acceptable frequency (Hz)
    value: '0.000000'

Can you guys suggest what I might be missing here?

$\endgroup$
4
  • $\begingroup$ Would you mind removing the comments from the config file? Also, please include a sample message from every sensor input. Thanks! $\endgroup$
    – automatom
    Jan 17 at 17:43
  • $\begingroup$ @automatom I have included the sample messages from sensors and, as pointed out by you, removed use_sim_time from the config file. The issue still persists. $\endgroup$ Jan 19 at 14:30
  • $\begingroup$ Oh, also, just checking: what does ros2 topic info say about the wheel odom and IMU topics? Is the EKF subscribed? $\endgroup$
    – automatom
    Jan 20 at 12:33
  • $\begingroup$ @automatom Yes, the EKF node is subscribed to the topics /wheel/odometry and /imu $\endgroup$ Jan 20 at 13:40

1 Answer 1

1
$\begingroup$

You need to include sample messages from your sensor inputs, but this sometimes indicates that the topic name, time stamp, or frame_id of the input message is incorrect.

EDIT: removing question about use_sim_time in ROS 2 in response to @Tully's point in the comments.

I was able to run your config from the command line in foxy:

ros2 run robot_localization ekf_node --ros-args --params-file ekf_params.yaml

I manually published messages to imu using the sample you provided, and the EKF was outputting messages on /odometry/filtered.

If I enable use_sim_time, it doesn't publish (it hangs here), presumably because I don't have a time server running. If I manually publish to /clock, I get output again. Can you confirm that your Gazebo instance is publishing to /clock? If it is, then can you possibly provide a bag so I can see if anything else is wrong?

$\endgroup$
4
  • $\begingroup$ Re: use_sime_time In ROS 2 there's no global parameters so if you're working in simulation you need to set it on all nodes. If you're running in sim it should be set on all nodes. $\endgroup$
    – Tully
    Jan 19 at 16:51
  • $\begingroup$ @Tully neither inclusion nor exclusion of use_sim_time seems to work. $\endgroup$ Jan 20 at 7:18
  • $\begingroup$ I was mostly addressing that to @automatom's question in the answer. It should be used in simluation as you had it. And I don't think is related to your problem. $\endgroup$
    – Tully
    Jan 20 at 10:19
  • $\begingroup$ @HarshitKaushik see this similar issue: github.com/ros-planning/navigation2_tutorials/issues/… Can you make sure you are getting data on /clock? If not, you'll need to make sure you are launching Gazebo correctly. $\endgroup$
    – automatom
    Jan 26 at 9:55

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.