Robotics StackExchange | Archived questions

ekf_localization does not publish odometry

I got this output at terminal. Echoing odometry/filtered does not produce anything.

I am using fovis as odometry source

[ WARN] [1438250290.975116879]: Could not obtain transform from /odom to odom. Error was Invalid argument "/odom" passed to lookupTransform argument source_frame in tf2 frame_ids cannot start with a '/' like: 

EDIT: kinect_odometer/info gives this:

---
header: 
  seq: 428
  stamp: 
    secs: 1438485119
    nsecs: 925958292
  frame_id: ''
change_reference_frame: True
fast_threshold: 20
num_total_detected_keypoints: 1354
num_detected_keypoints: [983, 267, 104]
num_total_keypoints: 219
num_keypoints: [156, 44, 19]
motion_estimate_status_code: 1
motion_estimate_status: SUCCESS
motion_estimate_valid: True
num_matches: 153
num_inliers: 111
num_reprojection_failures: 6
runtime: 0.09568407
---
header: 
  seq: 429
  stamp: 
    secs: 1438485120
    nsecs: 26003795
  frame_id: ''
change_reference_frame: True
fast_threshold: 20
num_total_detected_keypoints: 1340
num_detected_keypoints: [963, 270, 107]
num_total_keypoints: 229
num_keypoints: [165, 43, 21]
motion_estimate_status_code: 1
motion_estimate_status: SUCCESS
motion_estimate_valid: True
num_matches: 165
num_inliers: 124
num_reprojection_failures: 6
runtime: 0.108261896
---

echoing /diagnostics gives this:

status: 
  - 
    level: 0
    name: ekf_localization: Filter diagnostic updater
    message: The robot_localization state estimation node appears to be functioning properly.
    hardware_id: none
    values: []
  - 
    level: 2
    name: ekf_localization: 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.199988
      - 
        key: Actual frequency (Hz)
        value: 0.000000
      - 
        key: Minimum acceptable frequency (Hz)
        value: 25.200000
      - 
        key: Maximum acceptable frequency (Hz)
        value: 35.200000
---

What does it mean?

My launchfile:

<launch>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="0.2"/>
  <param name="two_d_mode" value="true"/>
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="kinect_odometer/odometry" />
  <!-- param name="odom1" value="example/another_odom"/> -->
  <!-- param name="twist0" value="kinect_odometer/odometry/twist"/ -->
  <!-- param name="pose0" value="kinect_odometer/odometry/pose"/ -->
  <!-- param name="twist0" value="example/twist" /-->
  <!-- param name="imu0" value="imu_gyro"/ -->

  <rosparam param="odom0_config">[true, true, true,
                                  true, true, true,
                                  true,  true, true,
                                  true, true, true,
                                  false, false, false]</rosparam>

  <rosparam param="odom1_config">[false, false, false,
                                  false, false, false,
                                  true,  false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="pose0_config">[true,  true,  true,
                                  true, true, true,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="twist0_config">[false, false, false,
                                   false, false, false,
                                   true,  true,  true,
                                   true,  true,  true,
                                   false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 false,  false,  false,
                                 false, false, false,
                                 false,  false,  true,
                                 false,  false,  false]</rosparam>

  <param name="odom0_differential" value="false"/>
  <param name="odom1_differential" value="false"/>
  <param name="pose0_differential" value="false"/>
  <param name="imu0_differential" value="true"/>

  <param name="odom0_relative" value="true"/>
  <param name="odom1_relative" value="false"/>
  <param name="pose0_relative" value="true"/>
  <param name="imu0_relative" value="false"/>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>

  <param name="print_diagnostics" value="true"/>

  <!-- ======== ADVANCED PARAMETERS ======== -->

  <param name="odom0_queue_size" value="2"/>
  <param name="odom1_queue_size" value="1"/>
  <param name="pose0_queue_size" value="10"/>
  <param name="twist0_queue_size" value="3"/>
  <param name="imu0_queue_size" value="10"/>

  <param name="odom0_pose_rejection_threshold" value="5"/>
  <param name="odom0_twist_rejection_threshold" value="1"/>
  <param name="pose0_rejection_threshold" value="2"/>
  <param name="twist0_rejection_threshold" value="1.2"/>
  <param name="imu0_pose_rejection_threshold" value="0.3"/>
  <param name="imu0_twist_rejection_threshold" value="0.1"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>

  <param name="debug"           value="true"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <rosparam param="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,    dfg0,    0.015]</rosparam>

  <!-- This represents the initial value for the state estimate error covariance matrix. Setting a diagonal value (a
       variance) to a large value will result in early measurements for that variable being accepted quickly. Users should
       take care not to use large values for variables that will not be measured directly. The values are ordered as x, y,
       z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->
       <rosparam param="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]</rosparam>

</node>

</launch>

Asked by radzaeem on 2015-07-30 05:01:24 UTC

Comments

Can you please post a sample kinect_odometer/odometry message? My first guess is that the fovis node is using a frame_id of /odom, and you need to remove the leading slash.

Asked by Tom Moore on 2015-07-30 07:52:56 UTC

Sorry for late reply. Remove the slash, but no change. Same error

Asked by radzaeem on 2015-08-01 21:16:53 UTC

If you are getting the exact same error, then can you post an updated fovis message? The only way it will complain about the leading slash in a frame_id is if there is one. Feel free to post a bag as well.

Asked by Tom Moore on 2015-08-01 21:40:36 UTC

yes the error is exact. edited question with fovis info. is frame_id supposed to be = "" ?

Asked by radzaeem on 2015-08-01 22:15:30 UTC

Sorry, I wasn't clear. Please run this and post a sample message:

rostopic echo kinect_odometer/odometry

Asked by Tom Moore on 2015-08-02 19:24:47 UTC

Answers

I had the very same problem that no odometry/filtered was published.

It was, indeed, caused by leading slashes in the input Odometry message. This is how I generate it:

msg = Odometry()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "odom"
msg.child_frame_id = "base_link"
msg.pose.pose.position = Point(*pos)
msg.pose.pose.orientation = Quaternion(*ori)
msg.twist.twist.linear.x = vx
msg.twist.twist.angular.z = va
pub.publish(msg)

Note the two strings without leading slash.

Moreover, you seem to be missing the frame_id completely. There you should define the parent frame.

Asked by Falko on 2015-08-19 10:18:13 UTC

Comments