Robot Localization Wrong Settings or Odom message?
BLUF (Bottom line up front): Using ackermann style odometry only, Robot_localization isn't providing an /odometry/filtered output.
Machine: Ubuntu 16 & ROS Kinetic
Details: I have an ackermann style vehicle with simulated sensors running in Unity and broadcasting to ROS.
For odometry, I wrote a bicycle model script to estimate position, orientation, and velocity. I publish this to /odometry/bicycle and its a child of /map. That looks like:
odom_quat = tf.transformations.quaternion_from_euler(0, 0, veh.yaw)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform(
(veh.x, veh.y, 0.),
odom_quat,
current_time,
"bicycle",
"map"
)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "map"
# set the position
odom.pose.pose = Pose(Point(veh.x, veh.y, 0.), Quaternion(*odom_quat))
odom.pose.covariance = [0.01, 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.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
# set the velocity
odom.child_frame_id = "bicycle"
odom.twist.twist = Twist(Vector3(veh.xdot, veh.ydot, 0), Vector3(0, 0, veh.yawdot))
odom.twist.covariance = [0.01, 0.00, 0.00, 0.00, 0.00, 0.00,
0.00, 0.01, 0.00, 0.00, 0.00, 0.00,
0.00, 0.00, 0.01, 0.00, 0.00, 0.00,
0.00, 0.00, 0.00, 0.10, 0.00, 0.00,
0.00, 0.00, 0.00, 0.00, 0.10, 0.00,
0.00, 0.00, 0.00, 0.00, 0.00, 0.10]
# publish the message
odom_pub.publish(odom)
Some common errors I've read about include having a forward slash (/) before the tf. I don't think I have that issue.
I also have a simulated IMU broadcasting to /imu_data, and gps broadcasted to /gps/fix. I will probably have questions on them later, but right now I want to focus on filtering just the odometry message. Ideally I want to recreate the dual EKF setup described on the robot localization page.
When setting up the EKF for just odometry, I'm not getting an odometry filtered message. Diagnostics does give me an error about erroneous settings.
Odometry message:
header:
seq: 66
stamp:
secs: 1554917950
nsecs: 73329925
frame_id: "map"
child_frame_id: "bicycle"
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [0.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0 ...
I think part of what I should be doing, is broadcasting the bicycle publisher on frame id "odom" to child "bicycle". I should create a static relationship between map and odom (until earth/etc is set up).
And in odom I should track vx, vy, and vyaw instead of positions.
However, I still don't have a functioning robot localization.
from here: https://answers.ros.org/question/3141... I've confirmed my wheel odometry child frame should be "base link" and my frame id should be "odom" (I have map above)..
Still No improvement.
I've also gone ahead and simplified my ekf yaml file:
And if anyone else is reading this: Many of the parameters added such as pose and twist thresholds, accel/decel limits, can cause the localization package to ignore the info...
However I still have the same error...