0
$\begingroup$

Rosanswers logo

We are using robot_localization to filter some of our sensor outputs. We are running two instances of RL but the one that takes care of the transform from map to the odom frame is not publishing the transform. For the instance of RL that is responsible for the transform between the map frame to the odom frame the diagnostics say /odom/filtered: no events recorded.

here are our launch files:

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_odom" clear_params="true">


  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="0.1"/>

  <param name="two_d_mode" value="true"/>


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

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

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

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


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

  <param name="odom0_differential" value="false"/>
  <param name="imu0_differential" value="false"/>

  <param name="odom0_relative" value="true"/>
  <param name="imu0_relative" value="true"/>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>

  <param name="print_diagnostics" value="true"/>


  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>

  <param name="odom0_pose_rejection_threshold" value="5"/>
  <param name="odom0_twist_rejection_threshold" value="1"/>
  <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_odom.txt"/>

  <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>


       <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>




</node>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_map" clear_params="true">

  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="two_d_mode" value="true"/>

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

  <param name="transform_time_offset" value="0.0"/>
 
  <param name="odom0" value="/rtabmap/odom"/>

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


 
  <param name="odom0_differential" value="false"/>

  <param name="odom0_relative" value="true"/>

  <param name="print_diagnostics" value="true"/>


  <param name="odom0_queue_size" value="10"/>

  <param name="odom0_pose_rejection_threshold" value="5"/>

  <param name="odom0_twist_rejection_threshold" value="1"/>

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

  <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>


       <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>




</node>

https://github.com/mantraes/ROS-Launch-Files.git

The original launch file is robo_loco.launch however we found something on the internet claiming that the order in which the instances of RL happen may effect how RL publishes its transforms so we made 2 separate launch files to try to see if that worked and those are called ekf_localization_map and ekf_localization_odom

Thanks for the help and let me know if you need anything else!


Here are inputs from the visual odometrey from RTabMap and the UM7 imu we are using:

Sensor Inputs from rtabmap Sensor Inputs from um7 imu


Originally posted by Wagner2x on ROS Answers with karma: 3 on 2016-02-08

Post score: 0


Original comments

Comment by Tom Moore on 2016-02-09:
The order of instances coming up won't make a difference. Can you post a sample message from each input to each filter instance?

Comment by Tom Moore on 2016-02-09:
Also, for readability, can you get rid of the comments in the launch files?

Comment by Wagner2x on 2016-02-09:
I updated the post. Is that what you were wanting? I posted images of the output of rtabmap visual odometrey and the imu sensor information being inputted into RL.

Comment by Orhan on 2016-02-11:
You can use yaml files for setting parameters.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Ok, lots of things:

  1. I don't see a remap statement in either instance. That means both instances are publishing to /odometry/filtered. You have two different state estimates going out over the same topic.
  2. The frame_id on the rtabmap data is odom, yet you are fusing it in both instances. That means that for the EKF instance where world_frame_id is map, it's going to transform the data from odom to map, but that is the very transform that instance is trying to produce. I would avoid that.
  3. You are fusing linear acceleration from your IMU, but you lack a velocity measurement.
  4. You have two_d_mode on, yet you are fusing Z, roll, and pitch in a few places. It doesn't hurt anything, but it's better to let them be false.

All that aside, though, I think you might not be using this correctly. Why do you need two instances? Your odom frame is meant to be continuous, yet rtabmap will almost certainly give you positions that jump around a small amount. Perhaps you should give me more information about what you're trying to do. As it is, your map EKF instance is probably interfering with the operation of rtabmap.

EDIT 1 in response to comments

It's not that the second instance is needed per se, but if you want to control your robot without it constantly changing direction when the pose jumps (and it will), you'll want to have two instances. For instance 1, fuse just the IMU and visual odometry data (just use the velocity). What is the LIDAR providing? I don't see that in your launch file.

My advice would be to start with the odom frame EKF, and add one sensor at a time. Make sure you are completely satisfied that it's behaving, then introduce another sensor. When you're done fusing all the continuous sensors into the odom instance, add the second map instance, and again, start with one sensor. Debugging these kinds of things gets very difficult when you have this many sensor sources, so it's critical to make sure each component is working and configured correctly.


Originally posted by Tom Moore with karma: 13689 on 2016-02-11

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Wagner2x on 2016-02-15:
Sorry for the delay. We are fusing a 2d lidar, visual odometry, IMU, and gps information. It was my understanding that since we were using the gps that we needed the second instance of RL.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.