How to get robot position in map?
I'm having problems overlaying a trajectory onto an occupancy grid plotted using Matplotlib
with the correct alignment.
My question is how do I get the robot position in the map frame so that when I plot both using Matplotlib
the robot trajectory is in the right place?
Note that I am using cartographer_ros
.
EDIT: Here's what I've tried to get the robot pose so far. It allows me to plot the trajectory but it doesn't have the same rotation as the map and is not aligned to it, although the XY alignment may well be a plotting issue rather than to do with ROS
. It only produces sensible looking results when required_position = "odom"
and frame = "base_link"
, which does not make sense to me because I would of thought that it should be like: required_position = "base_link"
and frame = "map"
.
def get_map_pose(self, required_position, frame):
tf_listener = tf.TransformListener()
tf_listener.waitForTransform(required_position, frame, rospy.Time(), rospy.Duration(5.0))
while not rospy.is_shutdown():
try:
now = rospy.Time.now()
tf_listener.waitForTransform(required_position, frame, now, rospy.Duration(5.0))
(t, r) = tf_listener.lookupTransform(required_position, frame, now)
euler_r = list(tf.transformations.euler_from_quaternion(r))
return t, euler_r
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
Does robot-pose-publisher not do whet you need? http://wiki.ros.org/robot_pose_publisher
This seems to only be available for Kinetic, I'm using Melodic.