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, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.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, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
ROS Out Diagnostics
header:
seq: 136
stamp:
secs: 1554918082
nsecs: 586047931
frame_id: ''
status:
-
level: 2
name: "ekf_odom: Filter diagnostic updater"
message: "Erroneous data or settings detected for a robot_localization state estimation node."
hardware_id: "none"
values:
-
key: "PITCH_configuration"
value: "Neither PITCH nor its velocity is being measured. This will result in unbounded error\
\ growth and erratic filter behavior."
-
key: "ROLL_configuration"
value: "Neither ROLL nor its velocity is being measured. This will result in unbounded error\
\ growth and erratic filter behavior."
-
key: "X_configuration"
value: "Neither X nor its velocity is being measured. This will result in unbounded error\
\ growth and erratic filter behavior."
-
key: "YAW_configuration"
value: "Neither YAW nor its velocity is being measured. This will result in unbounded error\
\ growth and erratic filter behavior."
-
key: "Y_configuration"
value: "Neither Y nor its velocity is being measured. This will result in unbounded error\
\ growth and erratic filter behavior."
-
key: "Z_configuration"
value: "Neither Z nor its velocity is being measured. This will result in unbounded error\
\ growth and erratic filter behavior."
-
level: 2
name: "ekf_odom: 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.157473"
-
key: "Actual frequency (Hz)"
value: "0.000000"
-
key: "Minimum acceptable frequency (Hz)"
value: "25.200000"
-
key: "Maximum acceptable frequency (Hz)"
value: "35.200000"
Launch Script -- Note: Vehicle URDF is loaded in a separate launch file.
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true">
<rosparam command="load" file="$(find vehsim)/params/vehsim_ekf.yaml" />
</node>
EKF Settings:
ekf_odom:
frequency: 10
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
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
odom0: odometry/bicycle
odom0_config: [true, true, true,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
odom0_queue_size: 5
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
# imu0: imu_data
# imu0_config: [false, false, false,
# false, false, false,
# 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: true
use_control: false #true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
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]
Originally posted by rukie on ROS Answers with karma: 71 on 2019-04-10
Post score: 0
Original comments
Comment by rukie on 2019-04-11:
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.
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
However, I still don't have a functioning robot localization.
Comment by rukie on 2019-04-11:
from here: https://answers.ros.org/question/314169/setting-frame-ids-for-ekf-localization/
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.
Comment by rukie on 2019-04-11:
I've also gone ahead and simplified my ekf yaml file:
ekf_odom:
frequency: 20
sensor_timeout: 2.0
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 1.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map
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: odometry/bicycle
odom0_config: [false, false, false,
false, false, true,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 5
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
Comment by rukie on 2019-04-11:
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...
