navsat_transform_node map frame wrong orientation unless facing east
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, 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: 50
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: "/home/alec/debug_ekf.txt"
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
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
odom1: /bunkbot_localization/odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
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: true
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: false
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-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: [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]
NAVSAT PARAMS
navsat_transform:
broadcast_utm_transform: true
delay: 3.0
frequency: 50
magnetic_declination_radians: 0.19146262 # Set this depdending on origin location in the world
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false
yaw_offset: 0.0
zero_altitude: true
I will also note, i have tried many orientations of yawoffset, if i start the robot in some random orientation X that is not EAST, and then find the yawoffset value that the map frame aligns correctly and does not rotate once driving around, this will work however, as soon as i start the robot in any different orientation Y, the map orientation is completely off again.
Also in case this is an issue regarding the way i am transforming UTM to map pose, this is currently how i am doing it:
def convert_UTMPose_to_MapPose(UTMPoint):
UTMPose = convert_UTMPoint_to_PoseStamped(UTMPoint)
MapPose = PoseStamped()
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length
listener = tf2_ros.TransformListener(tf_buffer)
notDone = True
while notDone:
try:
transform = tf_buffer.lookup_transform("odom", "utm", rospy.Time(0), rospy.Duration(1.0))
MapPose = tf2_geometry_msgs.do_transform_pose(UTMPose, transform)
notDone = False
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
print e
rospy.sleep(1)
continue
#print MapPose
return MapPose
Any help would be greatly appreciated!
UPDATE
I'm wondering if this issue is IMU related, i see strange behaviour out of my filtered imu data which uses the madgwick filter to fuse gryo, accel and mag data into an orientation, the bellow gif show's this behaviour, you can notice that the yaw (orientation z) does change when i rotate the robot, however when stationary it always "drifts" back to some random value?
Asked by agurman on 2019-04-07 23:36:44 UTC
Comments
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 drive straight for some time. Then you can apply the yaw offset you derived like that to the world transform.
Asked by Reamees on 2019-04-08 03:15:49 UTC
By getting the accurate heading estimation I mean filtering your gps readings in some node and calculating it based on the readings.
Asked by Reamees on 2019-04-08 03:33:59 UTC
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.
Asked by agurman on 2019-04-09 20:49:50 UTC
Can you also please include a sample message for each sensor input?
Asked by Tom Moore on 2019-05-15 04:06:29 UTC
Has this been solved? If so, can you please update or close the question?
Asked by Tom Moore on 2019-08-22 06:25:58 UTC