#! /usr/bin/env python

""" A script for reading rosbag data. """

import rosbag
import math

if __name__ == '__main__':
	bag = rosbag.Bag('b-zgrada-minimal.bag')

	x_vel = [Pose.pose.pose.position.x for (topic, Pose, t) in bag.read_messages(topics=['/pioneer/pose'])]

	y_vel = [Pose.pose.pose.position.y for (topic, Pose, t) in bag.read_messages(topics=['/pioneer/pose'])]



	t = [t.to_time() for (topic, Pose, t) in bag.read_messages(topics=['/pioneer/pose'])]


	s = 0

	for i in range(0, len(x_vel)):
		s=s + (pow((x_vel[i]-x_vel[i - 1]),2) + pow((y_vel[i] - y_vel[i - 1]),2))**(0.5)
	print s