Integrate UM7 IMU and Adafruit GPS with robot_localization
I'm trying to estimate the position of a robot using the robotlocalization ROS package. Currently I use a UM7 Orientation Sensor wich has their own ROS package (um7driver) and a Adafruit Ultimate GPS Breakout version 3. To read the GPS data I use the nmeaserialdriver. I read a lot of answers in this page and documents as REP-103 and REP-105, but I'm still not able to configure correctly the nodes and topics to obtain an output in /odometry/filtered or /odometry/gps topics.
I'm usign the navsattransformnode to convert the GPS raw data (fix) to a odometry/gps message. This odometry/gps message is an input for the ekflocalizationnode. In addition the odometry message (output) of the ekflocalizationnode is an input for the navsattransformnode in a circular path as were described in: How to fuse IMU & GPS using robot_localization
I think that my IMU configuration is ENU compliant. My launchers files are described below with samples of gps/fix and imu/data messages, unfortunately I can't upload an imageof my rqt_graph. Any comments and suggestions will be appreciated!!!
IMU Message Sample:
---
header:
seq: 352
stamp:
secs: 1491248090
nsecs: 477437036
frame_id: imu
orientation:
x: 0.0580077504
y: 0.0024169896
z: 0.9721333587
w: -0.2270291759
orientation_covariance: [0.002741552146694444, 0.0, 0.0, 0.0, 0.002741552146694444, 0.0, 0.0, 0.0, 0.007615422629706791]
angular_velocity:
x: 8.9495899038e-05
y: -0.000212560517745
z: 0.000418925544915
angular_velocity_covariance: [1.0966208586777776e-06, 0.0, 0.0, 0.0, 1.0966208586777776e-06, 0.0, 0.0, 0.0, 1.0966208586777776e-06]
linear_acceleration:
x: -4.19586237482
y: 1.07622469897
z: 8.44690478507
linear_acceleration_covariance: [0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438]
---
GPS Message Sample:
---
header:
seq: 54
stamp:
secs: 1491248749
nsecs: 594259977
frame_id: /gps
status:
status: 0
service: 1
latitude: -33.0408566667
longitude: -71.6299133333
altitude: 132.4
position_covariance: [3.9204, 0.0, 0.0, 0.0, 3.9204, 0.0, 0.0, 0.0, 15.6816]
position_covariance_type: 1
---
EDIT1: Sorry but I forgot add my launch (configuration) files.
This is my IMU_GPS.lauch
<launch>
<!--________________________ IMU ____________________________ -->
<node pkg="um7" type="um7_driver" name="um7_driver" respawn="true" output="screen">
<param name="port" value="/dev/ttyUSB0"/>
<param name="frame_id" type="string" value="imu"/>
<param name="baud" type="int" value="115200"/>
<param name="mag_updates" type="bool" value="true"/>
</node>
<!--________________________ GPS ____________________________ -->
<node pkg="nmea_navsat_driver" type="nmea_serial_driver" name="nmea_serial_driver" respawn="true" output="screen">
<param name="port" value="/dev/ttyUSB1"/>
<param name="baud" value="9600"/>
</node>
</launch>
And this is my EKF.launch
<launch>
<!--________________________ base_link -> IMU ____________________________ -->
<!--<node pkg="tf" type="static_transform_publisher" name="imu" args="0 0 0 0 0 0 1 base_link imu 20" />-->
<node pkg="tf" type="static_transform_publisher" name="bl_gps" args="0 0 0 0 0 0 1 base_link /gps" />
<node pkg="tf" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link imu" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" 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"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="map"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom1" value="/odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom1_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[true, true, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom1_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom1_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="odom1_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
</node>
<!-- navsat_transform -->
<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="1.570796327"/>
<param name="zero_altitude" value="true"/>
<param name="broadcast_utm_transform" value="true"/>
<param name="publish_filtered_gps" value="true"/>
<param name="use_odometry_yaw" value="false"/>
<param name="wait_for_datum" value="false"/>
<remap from="/odometry/filtered" to="/odometry/filtered"/>
<remap from="/gps/fix" to="/fix"/>
</node>
</launch>
In addition I add the rqt_graph obtained with my configuration:
Asked by Razek on 2017-04-03 14:57:47 UTC
Answers
Sorry, dropped the ball on this question. I see a couple things:
The most glaring issue is that your only
nav_msgs/Odometry
input starts withodom1
. Per the wiki, for each input message type, you have to start the index at 0. In other words, changeodom1
toodom0
everywhere in your configuration. As it is now, the EKF is ignoring that input.Not critical, but you have your IMU configured to provide X and Y position data (first two true values in
odom1_config
). Those will be ignored, since we only take in orientation, angular velocity, and linear acceleration from IMUs.
Asked by Tom Moore on 2017-07-06 05:45:10 UTC
Comments
Great first question! I've upvoted, and you should now have enough karma to post your rqt_graph.
Asked by lindzey on 2017-04-05 04:35:30 UTC
Thank you for adding sample messages. Can you also provide your configuration file(s) for the EKF(s) and
navsat_transform_node
?Asked by Tom Moore on 2017-04-11 04:30:06 UTC
Hi Tom, I just edited the question with the data that you required me. Sorry I forgot upload the data in the initial post. Thanks for your time.
Asked by Razek on 2017-04-11 13:56:32 UTC