0
$\begingroup$

Rosanswers logo

I have a node publishing imu and odometry data from a simulated robot and I want to set up an ekf_localization_node to estimate the robot position. But I don't really understand how to access the estimate of the ekf properly. Here is my rqt_graph: http://imgur.com/a/fkFtr

I suspect I am not setting up the transforms properly, but I can't figure out what to do. Nothing is published on the /odometry/filtered topic and tf/view_frames just says "no tf data received". This is my parameter file:

frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt 
publish_tf: true

publish_acceleration: false
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

imu0: ekf/imu
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #
imu0_remove_gravitational_acceleration: false

odom0: ekf/odom
odom0_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, true,
               false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false

odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1

deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

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,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

edit: this is the message published on /diagnostics:

header: 
  seq: 174
  stamp: 
    secs: 1498137106
    nsecs: 857927442
  frame_id: ''
status: 
  - 
    level: 1
    name: ekf_localization_node: Filter diagnostic updater
    message: Potentially erroneous data or settings detected for a robot_localization state estimation node.
    hardware_id: none
    values: 
      - 
        key: imu0_timestamp
        value: The imu0 message has a timestamp equal to or before the last filter reset, this message will be ignored. This may indicate an empty or bad timestamp. (message time: 0)
      - 
        key: odom0_timestamp
        value: The odom0 message has a timestamp equal to or before the last filter reset, this message will be ignored. This may indicate an empty or bad timestamp. (message time: 0)
  - 
    level: 2
    name: ekf_localization_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: 10.166615
      - 
        key: Actual frequency (Hz)
        value: 0.000000
      - 
        key: Minimum acceptable frequency (Hz)
        value: 25.200000
      - 
        key: Maximum acceptable frequency (Hz)
        value: 35.200000
---

Originally posted by linusnie on ROS Answers with karma: 98 on 2017-06-22

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

After some investigation I figured out I wasn't handling the transformations properly. I had to add the following to my launch file:

<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0.5 0 0 0 0 0 base_link imu_link" />
<node pkg="tf" type="static_transform_publisher" name="world_frame" args="0 0 0 0 0 0 map odom 100" />

The argument in the bl_imu node is the offset of my IMU and "imu_link" is the header.header_id of my imu topic. The second line is just for rviz to work.

I also had to add a time stamp to my simulated data:

now = rospy.Time.now()
imu.header.stamp.secs = now.secs
imu.header.stamp.nsecs = now.nsecs

where imu is the Imu object I'm publishing (and similarly for odometry). I'll try to make a more detailed post on this later, since I think this part of the toolbox is tricky to get started with.


Originally posted by linusnie with karma: 98 on 2017-06-27

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.