ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Distance traveled by robot using odometry

hello. I want to calculate the distance travel by my robot. I can subscribe to odometry message which gives me positioning information, but my path is not continuously growing, rather i am moving in a square. But i want to calculate the total distance travel by robot. Any suggestions? Some times, x is increasing, sometimes decreasing, after 3 or 4 rounds, i want to calculate how much the robot has actually traveled, how can i do that. i am using kinetic with python

edit retag close merge delete

are you moving at constant speed or some average speed?

No, speed is not constant. Rather a range is given in move_base.

Please be aware that anything based solely on (local) Odometry is going to be(come) wildly inaccurate after a while due to wheel slippage, integration errors, linearisation, modelling errors, etc, etc.

yes i know about odometry errors, thanks for informing

Sort by » oldest newest most voted

Something like this should work

#!/usr/bin/env python
import rospy
from math import sqrt
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry

class OdometryModifier:

def __init__(self):
self.sub = rospy.Subscriber("odom", Odometry, self.callback)
self.pub = rospy.Publisher('odom2', Odometry, queue_size=10)
self.total_distance = 0.
self.previous_x = 0
self.previous_y = 0
self.first_run = true

def callback(self, data):
if(self.first_run):
self.previous_x = data.pose.pose.position.x
self.previous_y = data.pose.pose.position.y
x = data.pose.pose.position.x
y = data.pose.pose.position.y
d_increment = sqrt((x - self.previous_x) * (x - self.previous_x) +
(y - self.previous_y)(y - self.previous_y))
self.total_distance = self.total_distance + d_increment
print("Total distance traveled is {.2f}m".format(self.total_distance))
self.pub.publish(data)
self.previous_x = data.pose.pose.position.x
self.previous_y = data.pose.pose.position.y
self.first_run = false

if __name__ == '__main__':
try:
rospy.init_node('move_turtlebot', anonymous=True)
odom = OdometryModifier()

more

1

You have to update the previous pose variables at the end of the callback.

Thanks, my bad, wrote this in a hurry so forgot that :D You can calculate between each odometry message the distance traveled by the robot with the formula :

dist = sqrt(dx² + dy²)


With dx = new_x - old_x and dy = new_y - old_y. So you have to save the previous odometry value and calculate the dx and dy after each odometry message received (or maybe each seconds instead). And then you add each distto get the total distance.

more

Below i have my subscriber code. I get the basic idea and i had the same approach in mind, but i am unable to program it. My programming is not good. I will be thankful if you help me. Suppose i have these goals followed in 5 steps, my path for x is, 0 ->2->4.5->0->7->1. How to program to calculate total distance?

#!/usr/bin/env python
import rospy
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry

class OdometryModifier:

def __init__(self):
self.sub = rospy.Subscriber("odom", Odometry, self.callback)
self.pub = rospy.Publisher('odom2', Odometry, queue_size=10)

def callback(self, data):
self.pub.publish(data)
if __name__ == '__main__':
try:
rospy.init_node('move_turtlebot', anonymous=True)
odom = OdometryModifier()