navsat_transform_node map frame wrong orientation unless facing east [closed]
Hey everyone,
I am currently running a differential drive setup with an RTK gps, IMU and wheel odometry. I am using the robot_localization ROS as a dual node setup as advised here for fusing GPS data.
As i understand the map and odom frame's are world fixed and start and the robot's initial pose. My problem is, the map frame only orientates correctly if the robot starts off facing EAST (with a yaw_offset of 0.0). If i start in any other orientation upon driving around a little bit, the map frame rotates on the spot and there is an offset between the odom frame and map frame. This is an issue as i am performing GPS waypoint navigaiton however if the map frame is in the wrong orientation, the transformed pose from UTM coords to the map frame also has the wrong orientation so the robot end's up going in the complete wrong direction!
Below is a screenshot of the map pose with the robot starting in an orientation that was not east:
EKF PARAMS
ekf_se_odom:
frequency: 50
two_d_mode: true
sensor_timeout: 0.1
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /warthog_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
# imu fused using madgwick imu filter
imu0: /imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, true, false]
imu0_differential: false
imu0_nodelay: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: false
use_control: false
# 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 ...
The IMU drifting to some random direction seems like it might be caused by using the magnetometer as part of the IMU orientation. Getting the magnetometer to output information that is usable would require quite a bit of work as there are changing magnetic fields on the robot you are using and in the environment most likely. So I would most likely start by not using the magnetometer as input at first.
The issue with the map orientation being correct only when starting your robot up facing east comes from the fact that you can't know the initial orientation of your robot from any one GPS reading. So it defaults to 0. If you want correct absolute heading you will need to get it from somewhere.
You say you are using RTK gps. It should be possible to get a somewhat accurate absolute heading estimation quite quickly if you ...(more)
By getting the accurate heading estimation I mean filtering your gps readings in some node and calculating it based on the readings.
Thanks for the help, after running some test's with another IMU placed in a different position i have been able to see that where the current IMU is placed, the magnetometer is getting significant interference. I am now setting up a gnss heading receiver to provide the accurate earth referenced heading, and not relying on the imu's magnetometer.
Can you also please include a sample message for each sensor input?
Has this been solved? If so, can you please update or close the question?