publish_vel_dist.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Quaternion, Vector3
from sensor_msgs.msg import Imu
from std_msgs.msg import *

import math

class VariableMaintainer():
	def __init__(self):
		self._time = False
        self._velocity = [0,0,0]
        self._position = [0,0,0]
	
	@property
	def time(self):
		return self._time

	@time.setter
	def time(self, value):
		self._time = value
    
    @property
	def velocity(self):
		return self._velocity

	@velocity.setter
	def velocity(self, value):
		self._velocity = value

    @property
	def position(self):
		return self._position

	@position.setter
	def position(self, value):
		self._position = value



def publishVelDist(msg, vmaint):
    velPub = rospy.Publisher('calc_velocity', Vector3)
    posPub = rospy.Publisher('calc_position', Vector3)
    if not vmaint.time:     # This means this is the first run
        vmaint.time = msg.header.secs + msg.header.nsecs*math.pow(10,-9)
        vmaint.velocity = [0,0,0]
        vmaint.position = [0,0,0]
    else:
        ctime = msg.header.secs + msg.header.nsecs*math.pow(10,-9)
        tdelt = ctime - vmaint.time
        accel = [msg.linear_acceleration.x,
                 msg.linear_acceleration.y,
                 msg.linear_acceleration.z]
        map(lambda x: x*timedelt, accel)
        vmaint.velocity = [vmaint.velocity[i]+accel[i] for i in range(3)]
        map(lambda x: x*timedelt, accel)
        vmaint.velocity = [vmaint.velocity[i]+accel[i] for i in range(3)]
        vmaint.time = ctime
        v = Vector3(vmaint.velocity[0],vmaint.velocity[1],vmaint.velocity[2])
        p = Vector3(vmaint.position[0],vmaint.position[1],vmaint.position[2])
        velPub.publish(v)
        posPub.publish(p)

def main():
	# Collect first 50 entries and average them.
	rospy.init_node("publish_vel_dist")
    timemaint = VariableMaintainer()
	rospy.Subscriber('imu', Imu, publishVelDist, vmaint)
	rospy.spin()

if __name__=='__main__':
	main()