asimat_y's answer is correct, but I'll restate the problem and solution in greater detail to better document the problem.
Problem
The pose and origin of /map
frame (as created by navsat_transform
) is not where you want or expect it to be. E.g:
This is the robot just after boot. The frame in the top-left is /map
which should be right under the robot because the robot is currently sitting on the lattitude/longitude the /map
origin should be fixed to.
Cause
The pose and origin of /map
can be set using the 'datum' ros parameter in a launch file for navsat_transform
. However, for reasons I don't quite understand, if you run this launch file on your robot when the robot boots (and sometimes when you run the launch file after boot-up as well) the origin and orientation may not be correct.
There may be other reason for changing the pose e.g. you want to reset it in the field.
Solution
The set_datum service can be used to manually reset the orientation and origin of /map
. To do this open a terminal (making sure your ROS environment variables are configured!) and type:
rosservice call /datum <tab>
You should then see something like this:
user@robot_ip_address:~$ rosservice call /datum "geo_pose:
position:
latitude: 0.0
longitude: 0.0
altitude: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0"
Fill out all the relevant parameters and then press enter
to set the new origin and orientation. Remember that for the orientation you must have w = 1.0 at a minimum! (x,y,z,w = 0 will result in a NAN error). Verify that this worked by checking your frames again, e.g.
Now /map
is correctly set to the latitude/longitude that the robot is currently sitting on.
You can also call the set_datum service from a file and change it that way. This is preferable if you want to reset the datum regularly. To do this in a python script, use the following code:
from geographic_msgs.msg import GeoPose
from robot_localization.srv import *
rospy.wait_for_service('datum')
try:
geo_pose = rospy.ServiceProxy('datum', SetDatum)
response = geo_pose(newMapPose)
except rospy.ServiceException, e:
rospy.loginfo ("Service call failed: %s"%e)
Where newMapPose
is a GeoPose message containing the coordinates you want to reset your frame origin to.