Ask Your Question
0

Distance traveled by robot using odometry

asked 2019-11-17 19:32:06 -0600

enthusiast.australia gravatar image

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 flag offensive close merge delete

Comments

are you moving at constant speed or some average speed?

Choco93 gravatar imageChoco93 ( 2019-11-18 03:22:20 -0600 )edit

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

enthusiast.australia gravatar imageenthusiast.australia ( 2019-11-18 05:10:30 -0600 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2019-11-18 05:59:50 -0600 )edit

yes i know about odometry errors, thanks for informing

enthusiast.australia gravatar imageenthusiast.australia ( 2019-11-18 06:00:51 -0600 )edit

2 Answers

Sort by » oldest newest most voted
1

answered 2019-11-18 05:46:49 -0600

Choco93 gravatar image

updated 2019-11-18 07:41:19 -0600

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()
edit flag offensive delete link more

Comments

1

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

Delb gravatar imageDelb ( 2019-11-18 07:32:49 -0600 )edit

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

Choco93 gravatar imageChoco93 ( 2019-11-18 07:40:01 -0600 )edit
0

answered 2019-11-18 01:59:25 -0600

Delb gravatar image

updated 2019-11-18 01:59:57 -0600

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.

edit flag offensive delete link more

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()
enthusiast.australia gravatar imageenthusiast.australia ( 2019-11-18 05:17:11 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-11-17 19:32:06 -0600

Seen: 35 times

Last updated: Nov 18