Adds a ROS node

ros/vive_tracker/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vive_tracker)
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+)
+catkin_package(
+  CATKIN_DEPENDS rospy std_msgs
+)
+catkin_install_python(PROGRAMS
+  src/vive_tracker.py
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/vive_tracker
+)
+

ros/vive_tracker/launch/node.launch

@@ -0,0 +1,13 @@
+<launch>
+
+  <arg name="confidence_thresh" default="800" />
+  <arg name="frame_id" default="vive" />
+  <arg name="port" default="/dev/ttyUSB0" />
+
+  <node pkg="vive_tracker" name="vive_tracker" type="src/vive_tracker.py" ns="vive_tracker" output="screen" >
+    <param name="confidence_thresh" value="$(arg confidence_thresh)" />
+	<param name="frame_id" value="$(arg frame_id)" />
+    <param name="port" value="$(arg port)" />
+  </node>
+
+</launch>

ros/vive_tracker/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package>
+  <name>vive_tracker</name>
+  <version>0.0.1</version>
+  <description>
+    This package polls the vive_tracker via serial and publish it as a pose.
+  </description>
+  <maintainer email="[email protected]">Bryan Ribas</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.r1b4z01d.com/</url>
+  <url type="bugtracker">https://github.com/r1b4z01d/vive-diy-position-sensor/issues</url>
+  <url type="repository">https://github.com/r1b4z01d/vive-diy-position-sensor</url>
+  <author>Bryan Ribas</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>rospy</run_depend>
+<run_depend>std_msgs</run_depend>
+</package>

ros/vive_tracker/src/vive_tracker.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+import rospy
+import time
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Time
+import serial
+
+
+def init():
+   # print "init"
+    global ser
+    ret = ser.readline()
+    if not ret.startswith('OBJ0'):
+    	ser.write(b'continue')
+    	time.sleep(0.5)
+    	ret = ser.readline()
+    	rospy.loginfo(ret)
+    	if not ret.startswith('OBJ0'):
+        	ser.write(b'o')
+
+def vive_tracker():
+    global ser
+    rospy.init_node('vive_tracker', anonymous=True)
+    rate = rospy.Rate(30) # 10hz
+    try:
+        ser = serial.Serial(port='/dev/ttyACM0', baudrate=57600, timeout=1.0)
+        ret = ser.readline()
+        rospy.loginfo(ret)
+    	if not ret.startswith('OBJ0'):
+             init()
+    except:
+        print "failed"
+        raise
+    #Define Publisher for topics
+    pubPose = rospy.Publisher('/vive/pose', PoseStamped, queue_size=1 )
+
+    while not rospy.is_shutdown():
+        try:
+            res=ser.readline()
+            if res.startswith('OBJ0'):
+                rawData = res.split("\t")
+                if len(rawData) == 7:
+                    poseStamped=PoseStamped()
+                    poseStamped.header.frame_id = "/vive"
+                    poseStamped.header.stamp = rospy.Time.now()
+                    poseStamped.pose.position.x = float(rawData[3])*-1
+                    poseStamped.pose.position.y = float(rawData[5])
+                    poseStamped.pose.position.z = float(rawData[4])
+
+                    #Publish Pose, ID and take a nap
+                    pubPose.publish(poseStamped)
+
+                    rate.sleep()
+        except Exception as e:
+            #You mad bro?
+            rospy.loginfo(e)
+
+
+if __name__ == '__main__':
+    try:
+        vive_tracker()
+    except rospy.ROSInterruptException:
+        cap.release()