Help initializing EKF to a set position
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.
- 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
- 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 ...
Please post sample input messages from all sensors. Also, you are passing in an invalid quaternion (set
w
to 1.0).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.
Would you mind accepting the answer (circular checkbox)? Thanks!