compute_distance_traveled.py

Go to the documentation of this file.

#!/usr/bin/env python
'''
compute_distance_traveled.py
Mac Mason <mac@cs.duke.edu>

Just figure out how far a robot has gone, from /tf.
'''
import math
import roslib
roslib.load_manifest('semanticmodel')
import rospy
import tf

rospy.init_node('compute_distance_traveled')
listener = tf.TransformListener()

# Our total distance
distance = 0.0
old_trans = None

rate = rospy.Rate(100)
while not rospy.is_shutdown():
  try:
    (trans, _) = listener.lookupTransform('/map',
                                          '/base_link',
                                          rospy.Time(0))
    if old_trans == None:
      old_trans = trans

    delta = (trans[0] - old_trans[0], trans[1] - old_trans[1])
    distance += math.sqrt((delta[0] * delta[0]) + (delta[1] * delta[1]))
    rospy.loginfo("Distance: %f" % distance)
    old_trans = trans

  except (tf.LookupException, 
          tf.ConnectivityException,
          tf.ExtrapolationException):
    print "tf is whining!"

  rate.sleep()