Hello,
We are running two instances of robot_localization node (as recommended) in order to fuse the information provide by an imu, odometry and GPS-RTK (ardusimple). The first instance of robot_localizaiton fuses only relative values using the imu (imu/data) and the odometry data (/odom) and setting world_frame param to odom. The result is /odometry/filtered. The second instance fuses the information that comes from the imu (imu/data), the odometry (/odom) and the odometry from gps (/odometry/gps) obatined using the navsat_transform_node. In this case, world_frame is set to map. So, we obtained the transform from odom to base_link from the first robot_localization node and the transform from map to odom using the second one.
The navsat_tranform_node use the information provided by /odometry/filtered, by the gps (/gps/fix) and the heading provied also with a specific setup of our ardusimple gps-rtk (different from the other imu we use). We have checked that the /odometry/gps provided by this node is correctly oriented and the accuracy in terms of position and orientation is good enough.
These are the main parameters of the relative configuration:
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /odom
odom0_config: [true, true, false,
false, false, true,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: true
odom0_relative: false
imu0: /imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true
The params used for the other robot_localizacion instance are:
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom0: /odom
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: false
odom0_differential: true
odom0_relative: false
# Odometry that comes from gps data (navsat_transform_node)
odom1: /odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 10
odom1_pose_rejection_threshold: 5
odom1_twist_rejection_threshold: 1
odom1_nodelay: true
imu0: /imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 10
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true
Using these configurations, the odom frame is continously jumping with repect to map frame. You can see the effect in the next video:
The issue is observed when the robot starts moving.
Any suggestions?
Thanks in advance
Edit: adding sensor message samples
/odom
header:
seq: 403
stamp:
secs: 1583947075
nsecs: 926265907
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: -9.40900611877
y: 158.370346069
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.986108124256
w: 0.166104823351
covariance: [100.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, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0]
twist:
twist:
linear:
x: 0.769959211349
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.0118001652882
covariance: [0.0010000000474974513, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.999999747378752e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.10000000149011612]
/imu/data
header:
seq: 48708
stamp:
secs: 1583947076
nsecs: 843026786
frame_id: "imu_link"
orientation:
x: -0.0161804026
y: 0.9498769128
z: 0.311858797
w: -0.0129913191
orientation_covariance: [0.002741552146694444, 0.0, 0.0, 0.0, 0.002741552146694444, 0.0, 0.0, 0.0, 0.007615422629706791]
angular_velocity:
x: -0.00821807715882
y: -0.0841824721231
z: -0.0744751334365
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: 0.490252647003
y: 0.347585064919
z: 10.5457498725
linear_acceleration_covariance: [0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438]
/imu/data/gps
header:
seq: 351
stamp:
secs: 1583947076
nsecs: 860885572
frame_id: "gps_link"
orientation:
x: 0.0
y: -0.0
z: -0.472689193861
w: -0.881229213092
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0003760701364841649]
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.0
y: 0.0
z: 0.0
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
/ublox_gps_rover/fix
header:
seq: 3381
stamp:
secs: 1583947076
nsecs: 999846882
frame_id: "gps_link"
status:
status: 2
service: 0
latitude: 41.6821695
longitude: -0.88929
altitude: 249.578
position_covariance: [0.0007840000000000001, 0.0, 0.0, 0.0, 0.0007840000000000001, 0.0, 0.0, 0.0, 0.0010890000000000001]
position_covariance_type: 2
The local robot localization launch file is:
<launch>
<!-- ekf localization -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true">
<rosparam command="load" file="$(find navigation)/config/ekf_params.yaml" />
<param name="imu0" value="/imu/data"/>
<param name="odom0" value="/odom"/>
</node>
</launch>
The navsat_transform node launch file is:
<launch>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<!-- <param name="magnetic_declination_radians" value="0.0079"/> -->
<!-- Using imu data with heading -->
<param name="yaw_offset" value="3.14"/>
<param name="use_odometry_yaw" value="false"/>
<param name="zero_altitude" value="true"/>
<param name="broadcast_utm_transform" value="true"/>
<param name="frequency" value="5.0"/>
<param name="wait_for_datum" value="false"/>
<!-- With imu -->
<remap from="/imu/data" to="/imu/data/gps" />
<remap from="/gps/fix" to="/ublox_gps_rover/fix" />
</node>
</launch>
An the global robot_localization launch file:
<launch>
<!-- ekf localization with gps-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom_gps" clear_params="true" output="screen">
<rosparam command="load" file="$(find navigation)/config/ekf_gps_heading_params.yaml" />
<param name="imu0" value="/imu/data"/>
<param name="odom0" value="/odom"/>
<param name="odom1" value="/odometry/gps"/>
<remap from="odometry/filtered" to="odometry/filtered_gps" />
</node>
</launch>
Originally posted by tseco on ROS Answers with karma: 43 on 2020-03-24
Post score: 0
Original comments
Comment by tseco on 2020-04-15:
Any suggestions about this issue?
Thanks in advance!
Comment by Tom Moore on 2020-05-04:
Please include sample messages from every sensor input.
Comment by tseco on 2020-05-04:
I have a 200 M bag, so I am going to reduce it. Apart from that, we are using a node in order to convert the heading data provided by the ardusimple gps-rtk to a sensor_msgs/Imu type. Do you need also this code? I will upload also the configuration and launch files.
Comment by Tom Moore on 2020-05-04:
I won't get the cycles to replay bags and review code, I'm afraid, so if you could just paste a single sample input message from every sensor in the question, we'll go from there. Thanks.
Comment by tseco on 2020-05-04:
I have just added some samples for the sensors.
Comment by tseco on 2020-05-13:
Hi Tom, did you have the chance to have a look to the data? Thanks in advance!
Comment by Tom Moore on 2020-05-19:
Edited the question and added the required info. This is not a thread format, so you should just update your question, rather than posting answers. Also, I was just looking for one sensor message per sensor, so I fixed that, too.
