0
$\begingroup$

Rosanswers logo

I had asked this question a while back, but received no response. Hopefully the revised subject line and info helps me come to a solution.

I'm running the latest robot_localization package on Kinetic/Ubuntu 16.

For testing certain functions of the vehicle, we would like to place the vehicle at a certain point in the map(which is already generated and published). To initialize the EKF to a location, I use the /set_pose rosservice call, which works IF odom0_differential=true. /set_pose does not work if odom0_differential=false. There is a tiny blip on the EKF output to the set location, but then the EKF starts at 0 again.

There are two sensors currently providing Pose data. The odometry(/odom) from wheel encoders and imu yaw(imu/data).

The EKF is setup as follows. Everything else is set to default.

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: /odom
imu0:  /imu/data

odom0_config: [true,  true,  false,          
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]

imu0_config:  [false, false,  false,
               false, false,  true,
               false, false,  false,
               false, false,  true,
               false, false,  false]

odom0_differential: false
imu0_differential:  false

odom0_relative: true
imu0_relative:  true

Rossservice call, initializing EKF to (1,1):

rosservice call --wait /set_pose "pose:
  header:
    seq: 0
    stamp:
      secs: 0
      nsecs: 0
    frame_id: odom
  pose:
    pose:
      position: {x: 1.0, y: 1.0, z: 0.0}
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
    covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]"

Edit(April 4th, 2017, 8:15AM): I've stripped my vehicle bag file to a new bag file called ekfTest.bag, with just odom+imu+tf data - Download. This bag contains /tf, /odom and /imu/data topics. Please note that I've turned off "publish_tf" in the EKF parameters as the bag file already has this /tf data.

I've also attached my ekf parameter file(ekf_template.yaml) and launch file.

  1. Start the robot_localization node with the parameter file. 2.Open a window to view EKF data: rostopic echo /odometry/filtered/pose/pose/position 3.Run the bag file: rosbag play ekfTest.bag
  2. Set pose: rosservice call /set_pose "pose: header: seq: 0 stamp: now frame_id: odom pose: pose: position: {x: 1.0, y: 1.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]"

Note: with @TomMoore's recommendation, I've made w=1. I usually press tab after /set_pose and was not paying attention to orientation.


Originally posted by bluehash on ROS Answers with karma: 120 on 2017-04-03

Post score: 1


Original comments

Comment by Tom Moore on 2017-04-04:
Please post sample input messages from all sensors. Also, you are passing in an invalid quaternion (set w to 1.0).

Comment by bluehash on 2017-04-04:
I've added data to the bottom of my pose. I've made w=1. I usually press tab after /set_pose and was not paying attention to orientation. The default is 0. This is a controlled test with sensor data(odom+imu) streaming into the EKF. I see the EKF latching to the /set_pose value and not change.

Comment by Tom Moore on 2017-05-03:
Would you mind accepting the answer (circular checkbox)? Thanks!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Ah, sorry, didn't look at your config closely enough. You are fusing X and Y in your odom0 config, which is pose data, so the first measurement you get after you call set_pose will snap it right back. In other words, this is happening:

  • Time t0: filter gets measurement from odom0. X and Y position in the filter output will be very near the odom0 measurement.
  • Time t1: you call set_pose with some specific pose. EKF pose jumps to that position.
  • Time t2: filter gets measurement from odom0. X and Y position in the filter output will be very near the odom0 measurement.

This is why differential mode works: it converts the odom0 pose to a velocity, which won't interfere with your set_pose call. All set_pose is doing is sending your robot to the location you send it. If you then give it measurements that tell it that it is somewhere else, it's going to jump back.


Originally posted by Tom Moore with karma: 13689 on 2017-04-11

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by bluehash on 2017-05-02:
Thanks Tom!

$\endgroup$

Your Answer

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