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 imagejpde.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 imagejpde.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 imagestevejp ( 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 imagestevejp ( 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 imagejpde.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 imagejpde.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 imagejpde.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 imagestevejp ( 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

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

acp gravatar imageacp ( 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

1 follower

Stats

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

Seen: 617 times

Last updated: Aug 09 '18