Low update rate on robot_localization
I am using ROS kinetic, Raspbian 9.4 (Stretch) on a Raspberry Pi 3. I am equipping a miniature vehicle (similar to the image below, image from google) for autonomous driving.
So far my I have an IMU and encoders installed, and I am attempting to fuse the sensors with robot_localization ekf. The outputs of robot localization are actually close to the expected position/orientation, but the rate is unusable low and slow to converge as a consequence. Here is the output from rostopic hz.
note@pc:~$ rostopic hz odometry/filtered
subscribed to [/odometry/filtered]
no new messages
no new messages
no new messages
no new messages
no new messages
average rate: 0.290
min: 3.444s max: 3.444s std dev: 0.00000s window: 2
no new messages
no new messages
no new messages
average rate: 0.264
min: 3.444s max: 4.122s std dev: 0.33911s window: 3
[..]
The publish rates for the TF transforms is higher than 50Hz (image below), and robot_localization is set up to publish the odom->base_link transform.
The same goes for the topics which robot_localization are subscribed to (below). Outputs from rqt.
The configuration file for robot_localization is as shown below. I have played with frequency and sensor_timeout parameters, but I did not experiment much with the covariance matrices, as I expect to see performance changes by modifying the covariances, and not the filter`s loop frequency (Is this assumption correct?).
<node name="local" pkg="robot_localization" type="ekf_localization_node">
<param name="frequency" value="100"/>
<param name="sensor_timeout" value="0.02"/>
<param name="two_d_mode" value="true"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0"/>
<param name="imu0" value="imu/data"/>
<param name="odom0" value="enc/pos"/>
<rosparam param="imu0_config">
[false, false, true,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
</rosparam>
<rosparam param="odom0_config">
[true, true, true,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
</rosparam>
<param name="imu0_queue_size" value="10"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_differential" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="initial_state">[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]</rosparam>
<param name="publish_tf" value="true"/>
<param name="publish_acceleration" value="false"/>
<param name="print_diagnostics" value="true"/>
</node>
Anyone would have insight on what I am missing?/ How can I get a higher update rate from robot_localization?
Please include a sample sensor message from every sensor input. My guess is that there's an issue with covariance matrices somewhere, but I can't say without more data.