Can navsat_transform be used to transform a pose from latlong to odom?
I'm using a gps to localize a mobile robot and ekf_localization_node
together with a navsat_transform_node
to transform the robot location into a local odom
frame.
I would like to be able to send the robot gps-coordinates and use the functions from robot_localization
to transform them into the odom
frame. However, navsat_transform
only publishes odom->utm
and I don't think there are any latlong->utm
conversion exposed to the user.
What I'm trying right now is to use the utm package in python to convert latlong coodrinates into utm and then using the utm->odom
transform provided by navsat_transform_node
to get the local coordinates:
utm_coords = utm.from_latlon(latlong[0], latlong[1])
# create PoseStamped message to set up for do_transform_pose
utm_pose = PoseStamped()
utm_pose.header.frame_id = 'utm'
utm_pose.position.x = utm_coords[0]
utm_pose.position.y = utm_coords[1]
# get the utm->odom transform using tf2_ros
tfbuffer = tf2_ros.Buffer()
tflistener = tf2_ros.TransformListener(tfbuffer)
T = tfbuffer.lookup_transform('odom', 'utm', rospy.Time())
# apply the transform
odom_pose = tf2_geometry_msgs.do_transform_pose(utm_pose, T)
It seems to work, but I would prefer to use the conversions implemented in robot_localization if possible, for consistency.
There is a c++ header file in the robot_localization package that allows you to convert from lat/lon to utm. You can replace your python node to use this instead.
Also just be aware, that different lat/lon -> utm conversion packages use very slightly different estimates, that can throw off measurements by a few meters. Make sure that you always use the exact same package/function to do your converting throughout your system.