I'm trying to use the robot localization package with the navsat transform node for gps+imu sensor fusion. My system is Ubuntu 20.04 and ros-noetic. I know this configuration will not produce the best results, but wanted to make this implementation work first and then move on to add some odometry data from the laser_scan_matcher with a dual ekf setup (local and global). The implementation seems to be working up to a point, but in Rviz there is a missmatch in the frames/orientation of the robot. The algorithm tracks down perfectly the shape of the movement, a square test, and it also tracks down the 90 degrees turns. My problem is that the orientation output of the filtered_map topic which is the final global localization output of the robot doesn't seem to be working and the orientation part is completely the same as the (raw) imu data I'm fusing in the system. It doesn't do anything to align the orientation im giving to the filter with the global ENU frame. Right now my imu raw data is showing 0 in the north, but after using the correct magnetic declination rate and applying the yaw offset in rviz i cant make it show 0 in the global east. Still the odometry/filtered_map.orientation (output of the rl node) is identical with the raw imu data I'm feeding in the rl node. I know this configuration won't produce the best results in terms of continuity but i just want to test it and move on from having this setup working correctly. Here are my launch and config files. Basically I'm providing a static transform to align map and odom frames and I'm asking from the rl node ekf_se_map to provide me the odom <-> base_link transform.
<?xml version="1.0"?>
<launch>
<rosparam command="load" file="$(find cmd2twist)/config/ekf_map_only.yaml" />
<rosparam command="load" file="$(find cmd2twist)/config/navsat_params.yaml" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<node pkg="tf2_ros" type="static_transform_publisher" name="base_to_imu_broadcaster" args="0 0 0 0 0 0 1 base_link ext_imu" />
<node pkg="tf2_ros" type="static_transform_publisher" name="gps" args="0 0 0 0 0 0 1 base_link gps" />
<node pkg="tf2_ros" type="static_transform_publisher" name="mapodomtf" args="0 0 0 0 0 0 1 map odom" />
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
<remap from="/odometry/filtered" to="odometry/filtered_map"/>
<remap from="/gps/fix" to="/fix"/>
<remap from="/imu/data" to="/imu/data"/>
</node>
</launch>
ekf_se_map:
frequency: 10
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# -------------------------------------
# GPS signal:
odom0: /odometry/gps
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
imu0: /imu/data
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.3, 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.5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]
initial_estimate_covariance: [1e-10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-10, 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, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0, 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, 1.0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]
navsat_transform:
frequency: 10
delay: 0.0
magnetic_declination_radians: 0.08864631991
yaw_offset: 1.5707963 #IMPORTANT IF OUR IMU HAS 0 HEADING LOOKING NORTH !!! navsat_transform_node has 0 when facing east
zero_altitude: true
broadcast_cartasian_transform: true
broadcast_utm_transform: false
broadcast_utm_transform_as_parent_frame: false
publish_filtered_gps: false
use_odometry_yaw: false
wait_for_datum: false
Here are the results in rviz.
As you can see it does a pretty good job in this movement in terms of position, it even ends in the same origin, but in terms of orientation it is completely off. (For now the drift of the imu is irrelevant, the main point is that it doesn't move correctly, doesn't move in the x direction when going forward etc.) The main problem is that the result of the orientation of the filtered_map is the same as the raw imu input and that causes issues in the final result, as you can see in the pic (just a small test plot, not smth relevant).
Also the terminal output of the rl node is shown here with the inital transform.
Orientation is essential for next steps to navigate along some waypoints as the orientation must be known for navigating there correctly. Is there something wrong with my setup and the orientation isn't fused properly? Thank you very much in advance and sorry for the long message.
EDIT 1
So, I think that the results are correct that way and the orientation should indeed be the output of the imu since there is no other source of orientation fused. I think that the visualization in rviz will always be in respect to the ENU frame and this wont be a problem later for navigation. It all comes down to the alignment of the robots' orientation with the ENU frame. If this is set correctly then the movement/orientation relation makes sense. My main problem now is that this relation isn't always on point and i have some runs where the robot moves indeed to the correct direction but others that it doesnt. I still am not sure why this is the case. Can this be caused due to wrong initial IMU message? Is there a way to assure that the transformation will be correct.
Nevertheless, the movement depicted makes sense and the map frame correctly depicts the ENU frame (x going up when robot moves to the north, y going up when robot going east). This can also be seen in the graph for a rectangle movement.
and the recorded topics:
Of course the results are as good as the inputs, so i will try testing with laser odometry to include a continuous source and see the results. Is there any other way to guarantee the ENU robot's orientation transformation? The main thing is that everything works, when providing a navigation goal. The GPS coordinates <-> map transformation seems to be working with the fromLL service provided by navsat.
EDIT 2 I extended the implementation to include lidar measurements and extended it to a dual ekf setup. The first, fusing:
- pose_stamped (pose with covariance msg) x,y,yaw
- IMU (yaw, wz)
- cmd_vel (twist with covariance msg) vx, wz (as the robot can only take velocity commands in x or yaw vel.
sends the base_link <-> odom transform while the second one fuses:
- odometry/gps (output of navsat node) x,y
- pose_stamped (as 1st instance) DIFFERENTIAL mode, x,y
- IMU (yaw, wz)
- cmd_vel (twist with covariance msg) vx, wz
and broadcasts the map <-> odom transform.
this can also be seen in the yaml file.
# For parameter descriptions, please refer to the template parameter files for each node.
ekf_se_odom: # Used only for broadcasting odom to base_link transforms
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_footprint
#base_link_frame: base_link
world_frame: odom
twist0: /cmd_vel
twist0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
twist0_queue_size: 10
twist0_nodelay: true
twist0_differential: false
twist0_relative: false
#twist0_rejection_threshold: 2
# -------------------------------------
# Laser scanmatching odometry:
pose0: /laser_pose
pose0_config: [true, true, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]
pose0_queue_size: 10
pose0_nodelay: true
pose0_differential: false
pose0_relative: false
#pose0_rejection_threshold: 5
# --------------------------------------
# imu configure:
imu0: /imu/data
#imu0: /imu
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: fals
process_noise_covariance: [1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.3, 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.5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]
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, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0, 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, 1.0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]
ekf_se_map:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_footprint
#base_link_frame: base_link
world_frame: map
# -------------------------------------
# GPS signal:
odom0: /odometry/gps
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
#odom0_pose_rejection_threshold: 3
# -------------------------------------
pose0: /laser_pose
pose0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
pose0_queue_size: 10
pose0_nodelay: true
pose0_differential: true
pose0_relative: false
twist0: /cmd_vel
twist0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
twist0_queue_size: 10
twist0_nodelay: true
twist0_differential: false
twist0_relative: false
imu0: /imu/data
#imu0: /imu
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 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, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.3, 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.5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]
initial_estimate_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 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, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0, 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, 1.0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]
the navsat subscribes to the /fix /imu/data and odometry/filtered_map topic from the 2nd ekf instance as seen in the launch file:
<?xml version="1.0"?>
<launch>
<node pkg="cmd2twist" type="cmd2twist" name="cmd_vel_to_twist_node" output="screen" />
<node pkg="cmd2twist" type="accfilter" name="accFilter" output="screen" />
<node pkg="cmd2twist" type="laserpose" name="laserPose" output="screen" />
<node pkg="cmd2twist" type="gps_filter_node" name="gps_filter_node" output="screen" />
<rosparam command="load" file="$(find cmd2twist)/config/ekf_vel.yaml" />
<rosparam command="load" file="$(find cmd2twist)/config/navsat_params.yaml" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<!--node pkg="tf2_ros" type="static_transform_publisher" name="base_to_imu_broadcaster" args="0 0 0.35 0 0 0 1 base_link ext_imu" /-->
<node pkg="tf2_ros" type="static_transform_publisher" name="base_to_imu_broadcaster" args="0 0 0 0 0 0 1 base_link ext_imu" />
<!--node pkg="tf2_ros" type="static_transform_publisher" name="gps" args="0 0 0.2 0 0 0 1 base_footprint gps" /-->
<node pkg="tf2_ros" type="static_transform_publisher" name="gps" args="0 0 0 0 0 0 1 base_link gps" />
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
<remap from="/odometry/filtered" to="odometry/filtered_map"/>
<remap from="/gps/fix" to="/fix"/>
<remap from="/imu/data" to="/imu/data"/>
</node>
</launch>
The result looks pretty solid in rviz, still it only moves with respect to the ENU frame, but I'm not entirely sure if the fusion of the laser odometry and the gps coordinates make sense, as i can see different behaviour in the output of the lidar odometry and the gps localization. However I'm quite happy with the total result in rviz.
and here as a gif. The movement corresponds nicely to the actual movement of the robot.
My main issue now is that i can't yet tackle big jumps coming from the gps data, as I cant think of a recovery behaviour when the gps signal is lost or the rtk communication with the gps base, as can be seen in the lat/long plots. In that case there are big jumps in the localization.
Anyway I don't think i should really focus on that case, as in order to fuse the gps properly i need to fix the gps data and not have so big jumps in there. (either problem with reliability of current gps module NS-HP-GN5, or the radio communication between rover and base or even the current testing location). I'm just concerned now if both ekfs work as intended.
One thing that also strongly concerns me is that the odom and map frame have quite a difference in rviz visualization and the odom frame keeps moving, which i cant quite understand. I know that this transformation should compensate for drift but it moves quite a lot and not in a smooth way. It can be seen in the following picture.
So I'm wandering if this configuration could work and if I'm fusing odometry with IMU and the gps data in a correct way. The visualization looks quite well as long as the gps stream is stable. Any feedback is appreciated. Thanks in advance!
