Setup frame and nodes

edit

Hello dear community, I am trying to fuse localization data from rtabmap icp odometry (odom->map), rtabmap(map->odom), gps odometry from navsat_tranform node and imu data out of madgwick filter.

<node pkg="nodelet" type="nodelet" name="imu_manager" args="manager" output="screen" />
<node pkg="nodelet" type="nodelet" name="ImuFilterNodelet" args="load imu_filter_madgwick/ImuFilterNodelet imu_manager" output="screen">
    <!-- in -->
    <remap from="/imu/data_raw" to="/zed_multi/zed2_rear/zed_nodelet_rear/imu/data"/>
    <remap from="/imu/mag" to="/zed_multi/zed2_rear/zed_nodelet_rear/imu/mag"/>
    <!-- out -->
    <remap from="/imu/data" to="$(arg imu_topic)"/>

    <param name="publish_debug_topics" value="true"/>
    <param name="publish_tf" value="false"/>
    <param name="world_frame" value="enu"/>
    <param name="use_magnetic_field_msg" value="false"/>
    <param name="use_mag" value="true"/>
    <param name="fixed_frame" value="odom"/>
</node>

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node">
    <param name="magnetic_declination_radians" value="0.083776"/> 
    <param name="yaw_offset" value="0"/><!--IMU heading face east-->
    <param name="zero_altitude" value="true"/>
    <param name="frequency" value="30"/>
    <param name="use_odometry_yaw" value="false"/>
    <param name="broadcast_cartesian_transform" value="true"/>
    <param name="publish_filtered_gps" value="true"/>

    <remap from="/odometry/filtered" to="/rtabmap/odom"/>
    <remap from="/gps/fix" to="$(arg gps_topic)" />
    <remap from="/imu/data" to="/imu/data/madgwicked" />
    <remap from="/odometry/gps" to="/odometry/navsat_gps" />
</node>

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">      
        <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param name="map_frame_id"   type="string" value="map"/>
(...)
</node>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">         
        <remap from="imu"        to="/imu/data/madgwicked"/>
        <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param name="map_frame_id"   type="string" value="map"/>
(...)
with wait_for_imu = true

However, I am unable to put the gps and the lidar odometry in the same frame. No matter what I do I alwas have something like this