Robot_pose_ekf bad frecuency

edit

Hi all,

I'm using the robot_pose_ekf whit odometry, imu and gps. It's pretty good by now, the only thing is that it's going too slow. When i make a "rostopic hz" to the odom_combined topic i get this:

salvador@salvador-VIT:~$ rostopic hz rosphere/odom_combined
subscribed to [/rosphere/odom_combined]
average rate: 0.933
        min: 1.072s max: 1.072s std dev: 0.00000s window: 2
average rate: 0.938
        min: 1.062s max: 1.072s std dev: 0.00497s window: 3

My .launch file is the following:

<?xml version="1.0"?>

<launch>
<arg name="use_cam" default="0"/>
<node pkg="rosphere" name="rosphere_AP_Comm" type="Ardupilot_control1c.py" ns="rosphere" output="screen" required="true"/>
<group ns="rosphere" if="$(arg use_cam)" >
    <include file="$(find raspicam)/launch/raspicam.launch"/>
</group>
<group ns="rosphere">
    <param name="tf_prefix" value="rosphere" />
    <node pkg="pose_ekf_rosphere" type="pose_ekf_rosphere" name="ekf_pose" output="screen">
        <param name="output_frame" value="odom_combined"/>
        <param name="freq" value="20.0"/>
        <param name="sensor_timeout" value="2.5"/>
        <param name="odom_used" value="true"/>
        <param name="imu_used" value="true"/>
        <param name="gps_used" value="true"/>
        <param name="odom_topic" value="odom_vel"/>
        <param name="imu_topic" value="imu"/>
        <param name="gps_topic" value="gps"/>
    </node>
</group>
<node pkg="tf" type="static_transform_publisher" name="base_to_imu_tf" args="0 0 0 0 0 0 /rosphere/base_footprint /rosphere/Imu 10"/>
<node pkg="rosphere" name="pose_to_odom" type="pose_to_odom.py" ns="rosphere" output="screen"/>
</launch>

the "rostopic hz" for odometry, imu and gps is:

salvador@salvador-VIT:~$ rostopic hz rosphere/odom_vel
subscribed to [/rosphere/odom_vel]
average rate: 19.753
        min: 0.020s max: 0.078s std dev: 0.01508s window: 14
average rate: 19.885
        min: 0.020s max: 0.078s std dev: 0.01462s window: 34
average rate: 19.744
        min: 0.020s max: 0.079s std dev: 0.01495s window: 39
salvador@salvador-VIT:~$ rostopic hz rosphere/imu
subscribed to [/rosphere/imu]
average rate: 21.394
        min: 0.020s max: 0.087s std dev: 0.01867s window: 14
average rate: 19.404
        min: 0.020s max: 0.095s std dev: 0.01794s window: 30
salvador@salvador-VIT:~$ rostopic hz rosphere/gps
subscribed to [/rosphere/gps]
average rate: 1.040
        min: 0.962s max: 0.962s std dev: 0.00000s window: 2

In the code odomestimation_test.cpp i setup for the filter loop to only check for odometry and imu constantly and check for gps only when new gps data has arrived by commenting a simple line :

//if (gps_active_)   filter_stamp_ = min(filter_stamp_, gps_stamp_);

This give me an error print but it should make my filter faster than the gps (the gps sensors are to slow compared to odometry or imu sensors).

If i don't comment this line, it prints a lot of time an info telling that the filter will not update with no change of time (it's obvious because the filter is trying to update at 20hz but the data is being processed at gps rate 1hz).

With all of this changes my filter should be processing the information at odometry or imu rate but it still update at ... (more)