From beaf3ed893ef879bf635873912db62edb97a7c3d Mon Sep 17 00:00:00 2001
From: Nandan Tumu <10603428+nandantumu@users.noreply.github.com>
Date: Mon, 12 Feb 2018 14:51:27 -0500
Subject: [PATCH] Added distance and velocity publishing

---
 src/publish_vel_dist.py | 72 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 72 insertions(+)
 create mode 100644 src/publish_vel_dist.py

diff --git a/src/publish_vel_dist.py b/src/publish_vel_dist.py
new file mode 100644
index 0000000..bbe976a
--- /dev/null
+++ b/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()
\ No newline at end of file