distance question

[Mr-young-Hit]

How do I calculate the total distance traveled by the robot？

[hasauino]

In my experiments I used robot odometery measurement.

loop__ xnew <--- get trasformation between odom and base_link distance = norm ( xnew - xold) total_distance = totaldistance + distance xold = xnew endloop____

This is a python script I wrote to do that:

#!/usr/bin/env python

# --------Include modules---------------
from copy import copy
import rospy
from visualization_msgs.msg import Marker
from std_msgs.msg import String
from geometry_msgs.msg import Point
from nav_msgs.msg import OccupancyGrid
import actionlib_msgs.msg
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
import tf

from os import system
from random import random
from numpy import array, concatenate, vstack, delete, floor, ceil
from numpy import linalg as LA
from numpy import all as All
from time import time,sleep

# -----------------------------------------------------

# Node----------------------------------------------
def node():

    rospy.init_node('distanceCounters', anonymous=False)

# -------------------------------------------

    rate = rospy.Rate(50)

    listener = tf.TransformListener()
    sleep(1)
    listener.waitForTransform('/odom', '/base_link',
                              rospy.Time(), rospy.Duration(10.0))

    try:
        (trans, rot) = listener.lookupTransform(
            '/odom', '/base_link', rospy.Time(0))

    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        trans = [0, 0]

    xinx = trans[0]
    xiny = trans[1]

    xprev = array([xinx, xiny])
    distance = 0
    t0 = time()

# -------------------------------RRT------------------------------------------
    while not rospy.is_shutdown():
        listener.waitForTransform('/odom', '/base_link',
                                  rospy.Time(), rospy.Duration(10.0))
        try:
            (trans, rot) = listener.lookupTransform(
                '/odom', '/base_link', rospy.Time(0))

            xinx = int(trans[0]*1000)/1000.0
            xiny = int(trans[1]*1000)/1000.0
            xnew = array([xinx, xiny])

            distance += LA.norm(xnew-xprev)
            print distance, "          elapsed  ", (time()-t0), " sec"
            xprev = array([xinx, xiny])
        except:
            pass
        rate.sleep()

# _____________________________________________________________________________
if __name__ == '__main__':
    try:
        node()
    except rospy.ROSInterruptException:
        pass

[Mr-young-Hit]

Sorry for the late reply.Thank you for your patience

[hasauino]

@Mr-young-Hit Hope my answer helped, Can close this issue?