Hello,
I'm trying to integrate an IMU sensor to my mobile robot no holonomic.
I follow the robot_localization tutorial to do that, but I'm a little confused with some questions.
First, how should be my resulting tf tree? I think the frame "odom_ekf" provided from ekf_localization node would be at the top of the tree. The base_link frame would be down. So when I called the set_pose service provided from ekf_localization node, this service could changed the values of transformed between "odom_ekf" frame and "base_link" frame. Is that correct?
Second, only starting to fuse the IMU sensor with the odometry, how the launch file of robot_localization should be?
<launch> <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen"> <param name="frequency" value="30"/> <param name="sensor_timeout" value="0.1"/> <param name="odom_frame" value="odom_ekf"/> <param name="base_link_frame" value="base_link"/> <param name="world_frame" value="odom_ekf"/> <param name="odom0" value="/myOdomTopic"/> <param name="imu0" value="/myImuTopic"/> <rosparam param="odom0_config">[false, false, true, true, true, true, true, true,true, true, true, true, true, true, true]</rosparam> <rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, true, true, true]</rosparam> <param name="imu0_remove_gravitational_acceleration" value="true"/> <param name="odom0_remove_gravitational_acceleration" value="true"/> <param name="odom0_differential" value="true"/> <param name="imu0_differential" value="false"/> <remap from="/odometry/filtered" to="/odometry/odom_imu" /> </node> </launch>
With this launch, the tf between odom_ekf and base_link is published and the topic /odometry/filtered shows correctly the robot position. But:
- The topic /odometry/filtered doesn't take into a count the changes at orientation. The position is more or less fine, but the orientation doesn't change.
- The remap to /odometry/odom_imu is not working.
- When the robot_localization is running these warnings appears constantly:
"[ WARN] [1417095211.082070235]: MessageFilter [target=odom_ekf ]: Dropped 100.00% of messages so far. Please turn the [ros.robot_localization.message_notifier] rosconsole logger to DEBUG for more information. [ WARN] [1417095211.082749350]: MessageFilter [target=base_link ]: Dropped 100.00% of messages so far. Please turn the [ros.robot_localization.message_notifier] rosconsole logger to DEBUG for more information."
I think first I have to solve this problems and then integrate the GPS sensor.
I'm not sure what I'm doing wrong. Some help would be greatly appreciated.
UPDATE 1:
Here is my frames.pdf
I've corrected some errors with the tf and now the robot_localization node doesn't show any errors.
The problem now is that the odometry/filetered isn't given the correct position and orientation. This is my launch file:
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<rosparam param="odom0_config">[true, true, false,
false, false, true,
true, true, false,
false, false, true,
false, false, false]
</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="two_d_mode" value="true"/>
<param name="odom0_differential" value="true"/>
<param name="imu0_differential" value="true"/>
</node>
</launch>
The odometry of odom0 is really good, here is an example of the topic publication:
---
header:
seq: 209396
stamp:
secs: 1417780933
nsecs: 699532957
frame_id: summit_a/odom
child_frame_id: summit_a/base_footprint
pose:
pose:
position:
x: -0.440918549019
y: -0.0679257786528
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.568167798045
w: 0.822912725181
covariance: [0.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
twist:
twist:
linear:
x: 0.0
y: -0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
The odometry filtered isn't working good. If the robot doesn't move, this topic shows the orientation always turning. Here is an example of the topic publication:
---
header:
seq: 8992
stamp:
secs: 1417780819
nsecs: 477535619
frame_id: summit_a/odom
child_frame_id: summit_a/base_footprint
pose:
pose:
position:
x: -0.83724089347
y: -0.0834189954256
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.117655236114
w: 0.993054502741
covariance: [0.0029236240180075704, 5.293955920339622e-23, 0.0, 0.0, 0.0, 6.017863108528476e-18, -1.0587911840678905e-22, 0.0029236240180075704, 0.0, 0.0, 0.0, -4.7450567233553104e-17, 0.0, 0.0, 4.999843715844642e-07, 1.3588785312409286e-34, 1.2380407640166175e-33, 0.0, 0.0, 0.0, 1.3588785312409286e-34, 4.997917817531666e-07, -1.0764669684467374e-56, 0.0, 0.0, 0.0, 1.238040764016617e-33, -1.0764669684467372e-56, 4.997917817531666e-07, 0.0, 6.017863108528477e-18, -4.745056723355312e-17, 0.0, 0.0, 0.0, 103.94554202745518]
twist:
twist:
linear:
x: -4.75634547929e-18
y: 5.23875168445e-19
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.00553798395805
covariance: [0.002695781736480738, 2.0610478214533238e-23, 0.0, 0.0, 0.0, 9.98187210629622e-21, 1.8738127076150572e-22, 0.002695781736480738, 0.0, 0.0, 0.0, 9.215002493653128e-20, 0.0, 0.0, 4.998750273990059e-07, 3.3137790604351043e-43, 3.019102511433211e-42, 0.0, 0.0, 0.0, 3.313779060435105e-43, 4.969126041719983e-07, -6.40125996972557e-64, 0.0, 0.0, 0.0, 3.019102511433213e-42, -6.401259969725568e-64, 4.969126041719983e-07, 0.0, 9.981872106296216e-21, 9.21500249365313e-20, 0.0, 0.0, 0.0, 0.15665066076132653]
---
My IMU only offers relative parameters of angular velocity and linear acceleration:
---
header:
seq: 33213
stamp:
secs: 1417785413
nsecs: 767263020
frame_id: summit_a/imu
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -0.00261799398202
y: -0.00418879011116
z: 0.0059341195192
angular_velocity_covariance: [0.005235987755982988, 0.0, 0.0, 0.0, 0.005235987755982988, 0.0, 0.0, 0.0, 0.005235987755982988]
linear_acceleration:
x: -0.165410732422
y: 0.134250646122
z: 9.80570288122
linear_acceleration_covariance: [0.19454589267577976, 0.0, 0.0, 0.0, 0.194857493538782, 0.0, 0.0, 0.0, 0.09814297118782998]
Here is an example of a movement in rviz. The red arroys are the original odometry, and the green arroys are the odometry_filtered returned by robot_localization.
UPDATE 2:
Sorry, I forgot copy the line of <param name="imu0" value="topic_of_your_imu"/>
I've corrected the orientatión data. Now it works better, but the filtered pose estimation isn't better than just the odometry.
I've tested it with the absolute values of the odom:
<rosparam param="odom0_config">[true, true, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
</rosparam>
The imu0 config is:
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
Here is the new test in rviz. I've recorded it in a bag file.
I've also tested it with the velocity of the odom0:
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
</rosparam>
And the same config for the imu as before.
Here is the rviz capture. And here is the bagfile.
UPDATE 3: (Tom thank you very much for your quick replies.)
My Summit isn't holonomic. It has normal wheels.
I've corrected the covariances and the velocities reported but the results are similar. I don't see any problem with the time stamps. All is running in the same machine.
Here is my last launch file for the absolute estimation, a bagfile and a capture of the results.
Here is my last launch file for the velocities, the bagfile and a capture of the results.
Just the odometry (red color) is better than the odometry filtered with IMU (green color).
UPDATE 4:
I've downloaded the last Github version of robot_localization and the results are the same. I've used your launch file and my output in rviz is ok, the same than yours. I've verified that the delay appears when the yaw velocity of the raw odometry is set to false at the input of the filter. So the main problem is that the filter can't estimate fine the yaw position or velocity without this input. The point is to know if the IMU is the reason.
The IMU doesn't have any internal clock. Its timestamp is set by "ros::time::now();". However, I don't find anything wrong with its tf. I've turned debug on but any error message appears to me. I've no idea why it shows them to you. Here is my debug.txt of robot_localization.(if it helps)
I've worked with my rosbag of velocities (This one) in order to see the same delay that you show me in your plots.
I've obtained the raw odometry messages from the Summit odom topic. raw_odometry.txt
I've obtained the odometry filtered of the new robot_localization node launched your new launch with v_yaw to true at the input of the filter.new_odometry_filtered.txt
I've obtained the odometry filtered by my rosbag (with yaw velocity of odom0 to false): imu_odometry.txt
This is the script for the octave (some comands can change in Matlab) that I used and these are some functions needed by the script. (qGetR.m,r2rpy.m)
The script shows 2 plots:
First plot is like yours third but It doesn't show any delay. The raw velocity measurements from odometry (red), the yaw velocity estimated by the positions (blue) and the yaw velocity for the new odometry_filtered (green). Note that this odometry_filtered is not from my rosbag, is from the new robot_localization of your launch file. There's no delay when the input of the filter has the yaw velocity of the raw odometry (odom0) set to true.
The second plot shows the delay between the yaw velocity of raw odometry (red) and the yaw velocity of the odometry_filtered(blue) when the input filter has the yaw velocity of the raw odometry set to false. Note that here I've used the odometry_filtered from my rosbag robot localization.(odom_imu topic)
Please, what I am doing wrong? How do you show the delay between the yaw velocity of the odometry and the yaw velocity estimated? Any ideas or suggestions would be appreciated.
UPDATE 5
Tom thank you very much, your efforts are really appreciated. Thanks to you I found what's going wrong. You're right. Summit's calculating the yaw velocity based on the movement of the wheels but taking the yaw orientation from a gyro. Obviously, the integration of the raw yaw velocity couldn't match with the raw yaw position.
I've modified the code and now here is the raw yaw position vs yaw position estimated by yaw velocity:
And here is the raw yaw velocity vs yaw position estimated by yaw position.
They fix perfectly. Zoom in:
(in response to your edit 5)
I drove a square. Here's the rosbag file (just with the ekf_localization topics).
And here is my launch file.
This is a capture of the test from rviz:
Everything works for me. The odometry filtered is better than just the raw odometry
Thanks a lot, Tom.
If you need anything and I can help you feel free to ask me.
Now, I'll try to integrate the GPS sensor to robot_localization node.
Best regards and thanks again.
Originally posted by ASoriano on ROS Answers with karma: 95 on 2014-11-27
Post score: 4
Original comments
Comment by Tom Moore on 2014-12-09:
Question before I update the answer: is this robot holonomic? It doesn't look it, but I know the Summit XLs can have omnidirectional wheels.
Comment by Tom Moore on 2014-12-15:
Comment in response to update 3: I'll take a look at this. Just so I'm clear, for the path that you drove, the robot's real-world start and end locations were exactly the same, correct?
Comment by Tom Moore on 2014-12-15:
One more thing: I keep getting errors when I attempt to filter your ROS bag files. This happened for the last file as well. The error is long, but here's the last line of it:
UnicodeDecodeError: 'ascii' codec can't decode byte 0xc2 in position 4080: ordinal not in range(128)
Comment by ASoriano on 2014-12-15:
The path that I drove finished more or less at the same location that started. Just the odometry (red) is so reliable, the odometry_filtered (green) should be very similiar to odometry(red).
Comment by ASoriano on 2014-12-15:
I don't have any problem playing my bagfiles. I'm using hydro, maybe you're using indigo and something at bagfiles changed?
Comment by Tom Moore on 2014-12-22:
Great! I'm glad it's working for you. Do you mind if I use your bag file for generating new integration tests? I ask because it would likely become part of the package, at least until I locate some web space for storing them.
Comment by ASoriano on 2014-12-22:
Of course. Feel free to use my bag file.
Comment by novak on 2017-03-29:
ASoriano, Do you have the file where you have fill the imu msg? I use rosserial_arduino to do that but I want to compare other methods.
Thank you.
Comment by ASoriano on 2017-11-02:
No, I have not. Sorry. This was 3 years ago.