Ask Your Question
0

Can't copy same data than IMU with robot_localization

asked 2018-05-03 11:36:06 -0600

pl.guhur gravatar image

updated 2018-05-03 11:45:50 -0600

I have troubles with robot_localization. So I decided to test only data from the Madgwick filter obtained from my IMU.

However, results provided by robot_localization are far from these data.

Here is the bagfile

Here is the launch file:

 <?xml version="1.0"?>
 <launch>
 <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_madgwick" output="screen" respawn="false" >
 <param name="fixed_frame" value="base_link" />
 <param name="use_mag" value="true" />
 <param name="use_magnetic_field_msg" value="true" />
 <param name="publish_tf" value="true" />
 </node>

<node pkg="nodelet" type="nodelet" name="imu_manager"
 args="manager" output="screen" />

 <node pkg="nodelet" type="nodelet" name="PhidgetsImuNodelet"
   args="load phidgets_imu/PhidgetsImuNodelet imu_manager"
   output="screen">
   <param name="period" value="4"/>
 </node>
 <node pkg="tf2_ros" type="static_transform_publisher" name="baselink_imu"        args="0 0 0.0 0 0 0 base_link imu" />
 <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
  <param name="frequency" value="5"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="imu0" value="/imu/data"/>
  <param name="two_d_mode" value="false"/>
  <rosparam param="imu0_config">[false, false, false, true,  true,  true, false, false, false, true,  true,  true, true,  true,  true]</rosparam>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>
  <param name="debug"           value="true"/>
  <param name="debug_out_file"  value="~/debug_ekf_localization.txt"/>

  <param name="publish_acceleration" value ="true"/>
  <param name="publish_tf" value ="true"/>

  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <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,    100, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                      0,    0,    0,    0,    100, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                      0,    0,    0,    0,    0,    100, 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,    100,  0,     0,     0,    0,    0,
                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     100,  0,     0,    0,    0,
                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     100,  0,    0,    0,
                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     100, 0,    0,
                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    100, 0,
                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    100]</rosparam>
</node>
</launch>

Here is what I get from /imu/data:

---
header: 
  seq: 13861
  stamp: 
    secs: 1525363752
    nsecs: 950200058
  frame_id: "imu"
orientation: 
  x: -0.348316741047
  y: -0.602712667954
  z: 0.590834173906
  w: 0.407833135901
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00855211333477
  y: 0.0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-05-03 19:42:03 -0600

stevejp gravatar image

updated 2018-05-03 19:42:54 -0600

I had a look at your bag file and I think the problem is related to your tf setup. Here's what I think is happening:

  • The madgwick filter publishes a tf from base_link to imu (because you set the publish_tf param to be true)
  • When r_l receives the imu measurement, it will attempt to transform it from imu to base_link so that it fuses the orientation properly
  • Because the madgwick filter is constantly updating the tf between base_link and imu, when the imu measurements are transformed to base_link, it is resulting in a measurement with unit quaternion orientation.

I ran your bag data (only /imu/data and /tf_static) with your r_l launch file configuration and the ekf odometry orientation matches the /imu/data orientation. So I think the only thing you'll need to do is set the publish_tf param to false and you should be okay. Give that a try and see what happens.

edit flag offensive delete link more

Comments

Completely right thanks!

pl.guhur gravatar image pl.guhur  ( 2018-05-04 05:32:04 -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

Stats

Asked: 2018-05-03 11:36:06 -0600

Seen: 98 times

Last updated: May 03 '18