Ask Your Question
0

Problem with ROS Navigation Package (IMU GPS)

asked 2018-08-07 19:09:49 -0600

jpde.lopes gravatar image

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)

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2018-08-07 21:12:42 -0600

stevejp gravatar image

In your EKF node config, your gps odometry topic name looks incorrect. It should be the output coming from your navsat_transform_node (which by default is odometry/gps). Also, not critical, but in your imu config you are fusing x/y/z velocities which aren't published by your imu.

edit flag offensive delete link more

Comments

So, in my IMU config, should I be fusing the first two rows; the angular velocity and acceleration?

jpde.lopes gravatar image jpde.lopes  ( 2018-08-08 10:40:24 -0600 )edit

According to this, doesnt the output of navsat defaults to odom/filtered (which I am then remapping to /odom/out)?

jpde.lopes gravatar image jpde.lopes  ( 2018-08-08 12:18:43 -0600 )edit
1

For your IMU, the only valid rows to fuse are [2,4,5] which correspond to roll-pitch-yaw, roll dot-pitch dot-yaw dot, and accel x, accel y, accel z. My advice would be to start with just fusing roll-pitch-yaw and then add the others and compare performance.

stevejp gravatar image stevejp  ( 2018-08-08 18:46:04 -0600 )edit
1

The statement Tom made "The odometry input to...output (it defaults to odometry/filtered)." is saying that the default input odom topic for navsat_transform is odometry/filtered, which is the default output of the EKF. But the default output of navsat_transform is odometry/gps

stevejp gravatar image stevejp  ( 2018-08-08 18:48:54 -0600 )edit

Thank you very much for your answer and tips. It seems to be stable so now, but if I move the ArduPilot board, nothing happens, which I assume is because the gps coordinates are not changing. Tomorrow I'll go outside to test it better. Nevertheless, I will accept your answer.

jpde.lopes gravatar image jpde.lopes  ( 2018-08-09 18:21:44 -0600 )edit

One thing that is bothering me now, is that with rqt_graph, the navsat_transform_node is not receiving any IMU topic, as it should.

jpde.lopes gravatar image jpde.lopes  ( 2018-08-09 18:22:44 -0600 )edit

Also, if I set accel x, accel y, accel z on IMU config to true, the base_link starts getting away from odom infinitely.

jpde.lopes gravatar image jpde.lopes  ( 2018-08-09 19:35:31 -0600 )edit
1

navsat_transform_node only needs one imu message to initiate the utm->map frame TF, so it could be dropping the connection once it's publishing that TF. Regarding accelerations: does this happen even when you're receiving good gps?

stevejp gravatar image stevejp  ( 2018-08-09 20:47:57 -0600 )edit
0

answered 2018-08-09 18:30:18 -0600

jpde.lopes gravatar image

updated 2018-08-09 19:51:31 -0600

jayess gravatar image

Based on @stevejp answer, I've made the changes below:

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
    <param name="frequency" value="30 "/>
    <param name="sensor_timeout" value="2"/>

    <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="/odometry/gps"/>
    <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,
                                    true, true, true,     <!-- roll, pitch, yaw -->
                                    false, false, false,             
                                    true, true, true,     <!-- droll, dpitch, dyaw -->
                                    false, false, false]</rosparam>     

    <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="imu0_pose_rejection_threshold" value="0.3"/>
    <param name="imu0_twist_rejection_threshold" value="0.1"/>
    <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/> -->

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

</node>

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

    <param name="frequency" value="10"/>
    <!-- <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"/>

</node>

</launch>

The resulting rqt_graph is shown below:

robot_navigation_package rqt_graph

edit flag offensive delete link more

Comments

1

Hi, Just wondering if you manage to fuse IMU and GPS? if you did is it possible to share your launch file?

acp gravatar image acp  ( 2019-02-06 04:46:34 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-08-07 19:09:49 -0600

Seen: 1,377 times

Last updated: Aug 09 '18