According to the documentation in : http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html I was able to transform the imu data header fram from "imu_link" into "base_link" through a tf transformation in a node.
However, somehow my imu data is not being fused into the ekf_localization node. I mean I was able to see the yaw changes as I move my robot. But when I subscribe to the odometry/filtered, it still shows my yaw to be at 0 degree.
I have double checked with the raw data from the imu and verified it outputs data as shown below: The data below are my imu message after I move the robot. This which reflects the changes of quaternion in the orientation
---
header:
seq: 9520
stamp:
secs: 1617336774
nsecs: 71053028
frame_id: "base_link"
orientation:
x: -873.0
y: -496.0
z: 0.0
w: 16353.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.0
y: 0.01
z: -0.18
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
Below is my configure file for the ekf kalman filter.
<include file="$(find roboteq_driver)/launch/controller.launch"/>
<include file="bno055.launch"/>
<node pkg="robot_localization" type="ekf_localization_node"
name="robot_localization_ekf_node_continuous"
clear_params="true">
<param name="frequency" value="30" />
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true" />
<param name="publish_tf" value="true" />
<param name="odom_frame" value="odom" /> <!-- publish a new data to odom-->
<param name="base_link_frame" value="base_link" />
<param name="world_frame" value="odom" /> <!-- change here if we want to add map frame later-->
<param name="odom0" value="/drive_odometry" />
<rosparam param="odom0_config">
[false,false, false,
false, false, false,
true, true,false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="odom0_nodelay" value="true"/>
<param name="imu0" value="transformed_imu"/>
<rosparam param="imu0_config">
[false, false, false,
false, false,false,
false, false, false,
false , false, true,
true, false, false] <!-- from IMU message acceleration-->
</rosparam>
<param name="imu0_differential" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_nodelay" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="imu0_queue_size" value="10"/>
<param name="use_control" value="false"/>
<!--final odom message-->
<remap from="/odometry/filtered" to="/odometry/filtered_odom"/>
</node>
Odometry message from : drive_odometry
header:
seq: 1392
stamp:
secs: 1617336632
nsecs: 897039890
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 10.4
y: 4.43
z: 0.0
orientation:
x: -873.0
y: -500.0
z: 0.0
w: 1240.0
covariance: [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 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.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4]
---
So In this case, according to the kalman filter in the library, since I only use the yaw velocity from IMU in the configure file to determine the current yaw position of the robot, I am expecting the yaw value output from odometry/filtered_odom to be about the same as the yaw value in the IMU message.
However, when I subscribed to the odometry/filtered_odom, I find the orientation to be: x: 0.0 y: 0.0 z: 0.0 w: 1.0
Note: I only allow the angular velocity of IMU to be fused with the filter in determining the yaw direction. I did not use the yaw message from the odom, because the Odom message is generated by my calculation based on the feedback from the wheel's encoder.