Added distance and velocity publishing

src/publish_vel_dist.py

@@ -0,0 +1,72 @@
+#!/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()