Question

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

Asked by enthusiast.australia on 2019-11-17 20:32:06 UTC

Answer 1

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.

Asked by Delb on 2019-11-18 02:59:25 UTC

Comments

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):
        data.header.stamp= rospy.Time.now()
        data.header.frame_id = "odom"
        self.pub.publish(data)
if __name__ == '__main__':
    try:
        rospy.init_node('move_turtlebot', anonymous=True)
        odom = OdometryModifier()

Asked by enthusiast.australia on 2019-11-18 06:17:11 UTC

Answer 2

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()

Asked by Choco93 on 2019-11-18 06:46:49 UTC

Comments

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

Asked by Delb on 2019-11-18 08:32:49 UTC

Thanks, my bad, wrote this in a hurry so forgot that :D

Asked by Choco93 on 2019-11-18 08:40:01 UTC

Distance traveled by robot using odometry

Comments

Answers

Comments

Comments