Hi ROS friends,
I'm building a custom ROS robot using the SenseHat (https://projects.raspberrypi.org/en/projects/getting-started-with-the-sense-hat) for the IMU.
I have followed REP-105 to define a set of frames published by TF2. All frames move consistently as seen in RViz (see image)
I'm now trying to use the EKF filter in robot_localization to fuse odometry and the IMU. After hours of hacking the EKF node doesnt output anything!!
I think my main issue is the IMU configuration and wondering if any expert out there can help me with this!
What the robot is providing at the moment:
odometry : pose (x,y, theta) and velocity (vx,vy, w)
TF: odom, baselink, trinibot_imu (imu frame) and depth_frame (for a ToF) (As shown in the image below)
Following transforms are being published:
odom->baselink
base_link -> trinibot_imu
base_link -> depth_frame
The IMU frame, trinibot_imu provides a static transform of the IMU relative to base_link like so
imu_tr.header.frame_id = "base_link"
imu_tr.child_frame_id = "trinibot_imu"
imu_tr.transform.translation.x = 0.008
imu_tr.transform.translation.z = 0.084788
imu_tr.transform.translation.y = 0.0
imu_tr.transform.rotation.x = 0
imu_tr.transform.rotation.y = 0
imu_tr.transform.rotation.z = 0
imu_tr.transform.rotation.w = 1
The IMU (sensehat) seemed to be providing a NED transform. The sensehat API only provides Euler angles so I transformed these into a quaternion and then applied a ENU transformation like so:
quat = tf.transformations.quaternion_from_euler(sense.get_orientation_degrees()['roll']*D2R, \
sense.get_orientation_degrees()['pitch']*D2R, \
sense.get_orientation_degrees()['yaw']*D2R)
quat= quat_NED2ENU(quat)
def quat_NED2ENU(iquat):
a = np.array([0.707, 0.707, 0, 0 ])
b = np.array([[[iquat[3], -iquat[2], iquat[1], -iquat[0]], \
[iquat[2], iquat[3], -iquat[0], -iquat[1]], \
[-iquat[1], iquat[0], iquat[3], -iquat[2]], \
[iquat[0], iquat[1], iquat[2], iquat[3]]]])
c = np.array([[[0, 0, -0.707, 0.707], \
[0 , 0, 0.707, 0.707], \
[0.707, -0.707, 0, 0], \
[-0.707, -0.707, 0 ,0]]])
return (a.dot(b)).dot(c)
The above seems to transform the NED frame in an ENU frame. When aligning the rear of the robot against magnetic north (using my phone app), the quaternion looks like this and oriented as seen by the trinibot_frame in the image
orientation:
x: -0.00801062127965
y: 0.00182113633242
z: -0.973651144752
w: 0.226565780595
Pitching the robot (clock-wise base_link Y) results in CW rotation around the imu_frame RED axis
Rolling the robot positive (clock-wise base_link X) results in a Counter-clockwise rotation around the imu_frame GREEN axis
Yaw around base_link Y is aligned with the imu yaw rotation
and accelerations of the IMU correspond ;
+9.81 in the Z axis
+9.81 in the Y axis (base_link roll 90 deg)
-9.81 in the X axis (base_link pitch 90 deg)
as stated here http://docs.ros.org/lunar/api/robot_localization/html/preparing_sensor_data.html
QUESTION 1: The robot_localization docs states that the IMU can be arbitrarily oriented. However it suggests a body fixed frame for the acceleration vector!!!
Acceleration: Be careful with acceleration data. The state estimation nodes in robot_localization assume that an IMU that is placed in its neutral right-side-up position on a flat surface will: Measure +9.819.81 meters per second squared for the ZZ axis. If the sensor is rolled +9090 degrees (left side up), the acceleration should be +9.819.81 meters per second squared for the YY axis. If the sensor is pitched +9090 degrees (front side down), it should read -9.819.81 meters per second
The config file for the EKF is below:
frequency: 10
sensor_timeout: 2
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: true
debug_out_file: ~/sandbox/external-rospacks/src/trinibot_core/scripts/ekf_debug.txt
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: /trinibot/odometry
odom0_config: [true, true, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 100
odom0_nodelay: false
odom0_differential: false
odom0_relative: true
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
imu0: /trinibot_sensors/imu
imu0_config: [false, false, false,
true, true, true,
false, false , false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 100
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: 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]
Launch file:
<launch>
<node pkg="trinibot_core" type="tbmotioncontroller.py" args="/dev/ttyACM0 200 1 1" name="robotcontroller" machine="$(arg machine_bot)" respawn="false" output="screen">
<remap from="/velocity_cmd" to="/trinibot_gui/velocity_cmd" />
<remap from="/string_cmd" to= "/trinibot_gui/string_cmd" />
</node>
<node pkg="trinibot_core" type="tbsensehat.py" name="sensehat" machine="$(arg machine_bot)" respawn="false" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" machine="$(arg machine_c2i)" clear_params="true"/>
<rosparam command="load" file="$(find trinibot_core)/params/tb_ekf.yaml" />
<node pkg="trinibot_core" type="tbteleopgui.py" name="teleoperation_gui" machine="$(arg machine_c2i)" respawn="false" output="screen" />
</launch>
Echoing the /diagnostics topic provides;
header:
seq: 1856
stamp:
secs: 1508590386
nsecs: 474118459
frame_id: ''
status:
-
level: 2
name: ekf_se: 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_se: 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.166661
-
key: Actual frequency (Hz)
value: 0.000000
-
key: Minimum acceptable frequency (Hz)
value: 25.200000
-
key: Maximum acceptable frequency (Hz)
value: 35.200000
Is there something missing here? e.g. do I need to use /set_pose or some other params to get this working. I've read the docs over and over again, but cant progress!
Originally posted by fastestindian on ROS Answers with karma: 131 on 2017-10-21
Post score: 1
