Mavros inconsistency in config file px4_config.yaml

[acp]

Dear People.

I have the following hardware:  
pixhawk mini
Jetsontx2
zed camera

and the following software
ROS kinetic
Ubuntu 16.04.1
Jetsonpack 3.2.1
ZED=SDK version 2.7
Mavros
mavlink


I am using mavros to interface the pixhawk to my ROS environment.


I am trying to read the following topics:

/mavros/global_position/global 
/mavros/global_position/local ////////
/mavros/imu/data
/mavros/local_position/odom  //////
/mavros/local_position/pose
/stereo/camera/rtabmap/odom

I also have drone.xacro model.


I have recorded some bagfiles and when I visualized them in RVIZ I can see the topics  

/mavros/global_position/local 

and

/mavros/local_position/odom  

Totally shifted and rotated. This can be seen in the following figure:



* The red arrow set is the /stereo/camera/rtabmap/odom topic, in other words the odom calculated by rtabmap based on the stereo images.

* The green arrow set is the /mavros/local_position/odom topic that is shifted and do not follow the drones position in RVIZ

* The blue arrow set is the /mavros/global_position/loca topic  that is shifted and do not follow the drones position in RVIZ



The tf tree is depicted in the next figure:



The way I am calling mavros is as follows:

<arg name="mavros_transformation" default="0" />
    <arg name="ns" default="/"/>
    <arg name="fcu_url" default="/dev/ttyTHS1:921600"/>
    <arg name="host_ip" default="10.42.0.253" />
    <arg name="gcs_url" default="" />   <!-- GCS link is provided by SITL -->
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />
    <arg name="serial_no_front_camera" default="819112072871"/>
    <arg name="depth_fps"  default="60"/>
    <arg name="infra1_fps" default="60"/>
    <arg name="infra2_fps" default="60"/>

<!-- Launch MavROS -->
    <group ns="$(arg ns)">
        <include file="$(find mavros)/launch/node.launch">
            <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
            <!-- Need to change the config file to get the tf topic and get local position in terms of local origin -->
            <!--arg name="config_yaml" value="$(find mavros)/launch/px4_config_gp.yaml" /-->
            <arg name="config_yaml" value="$(find mavros)/launch/px4_config_tbsd_forum.yaml" />
            <arg name="fcu_url" value="$(arg fcu_url)" />
            <!--arg name="gcs_url" value="$(arg gcs_url)" /-->
            <arg name="gcs_url"       default="udp://@$(arg host_ip)" />
            <arg name="tgt_system" value="$(arg tgt_system)" />
            <arg name="tgt_component" value="$(arg tgt_component)" />
        </include>
    </group>

and the px4_config_tbsd_forum.yaml I am using according to a forum https://github.com/mavlink/mavros/pull/791/files/a3847cd4016b9da7b7b6ae4f633146a7e8ecf77f#diff-57d2f68a3e0e5cfa2db14ac20d8845dd


is:

px4_config_tbsd_forum.yaml

And also /mavros/imu/data topic is delay with respect to the base_link of the drone.

Can some one help me or suggest what is wrong???


Thank you so much.