Problem with ROS Navigation Package (IMU GPS)

edit

Hi,

I have the objective of collecting laser data using a tripod and moving the tripod and I am using a Mavros ArduPilot board on the top of that tripod so that I can fuse the GPS and IMU. Initially, I have created a simple .urdf file, where I have the laser (laser_base) approximately 1m above the base_link.

To fuse the IMU and GPS, my launch file is as follows:

<arg name="fcu_url" default="/dev/ttyUSB1:57600" />
(...)
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
  <param name="frequency" value="10 "/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="two_d_mode" value="false"/>

  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom_combined"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom_combined"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="/odom/out"/>
  <param name="imu0" value="/mavros/imu/data"/>

  <rosparam param="odom0_config">[true, true, true,
                                  false, false, false,
                                  false,  false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 false, false, false,       <!-- roll, pitch, yaw -->
                                 true, true, true,           <!-- dx, dy, dz2 -->
                                 true, true, true,           <!-- droll, dpitch, dyaw -->
                                 false, false, false]</rosparam> <!-- dx2, dy2, dz2 -->

  <param name="odom0_differential" value="false"/> <!-- this was false -->
  <param name="imu0_differential" value="false"/>
  <param name="odom0_relative" value="false"/>
  <param name="imu0_relative" value="false"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>
  <param name="print_diagnostics" value="true"/>

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <remap from="/odometry/filtered" to="/odom/out"/>
</node>

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">

    <param name="frequency" value="20"/>
    <param name="delay" value="3"/>
    <param name="magnetic_declination_radians" value="0"/>
    <param name="yaw_offset" value="0"/>
    <param name="zero_altitude" value="false"/>
    <param name="broadcast_utm_transform" value="false"/>
    <param name="publish_filtered_gps" value="false"/>
    <param name="use_odometry_yaw" value="false"/>
    <param name="wait_for_datum" value="false"/>

    <remap from="/gps/fix" to="/mavros/global_position/raw/fix"/>
    <remap from="/imu/data" to="/mavros/imu/data"/>
    <remap from="/odometry/filtered" to="/odom/out" />

</node>

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
</node>
<param name="robot_description" textfile="$(find ODpkg)/description/tripod.urdf" />
<node name="modelvisualization" pkg="rviz" type="rviz" output="screen"/>

So, navsat_transform_node receives the IMU from the ekf_localization_node and produces the odometry that will be used by ekf_localization_node, which results in a closed loop, as stated here.

Also, I have defined the world frame the same as the odom frame, as stated in the Wiki.

As the odom is being produced by the gps, in odom_config, I have only defined x, y and z as true. Also, if on imu_config, I define either roll, Pitch, yaw, dx2, dy2 or dz2 as true, the base_link frame starts moving away from odom_frame infinitely and I don't know why.

So, to sum up, I wan't to move the tripod and keep recording laser data, so I want to notice any inclination or change in the direction of the tripod, as well as x, y, z changes. As it is right now, the distance between odom and base_link remains the ... (more)