diff --git a/ros/vive_tracker/CMakeLists.txt b/ros/vive_tracker/CMakeLists.txt new file mode 100755 index 0000000..80bfee7 --- /dev/null +++ b/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 +) + diff --git a/ros/vive_tracker/launch/node.launch b/ros/vive_tracker/launch/node.launch new file mode 100755 index 0000000..d30cbd5 --- /dev/null +++ b/ros/vive_tracker/launch/node.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/ros/vive_tracker/package.xml b/ros/vive_tracker/package.xml new file mode 100755 index 0000000..5f14d9e --- /dev/null +++ b/ros/vive_tracker/package.xml @@ -0,0 +1,21 @@ + + + vive_tracker + 0.0.1 + + This package polls the vive_tracker via serial and publish it as a pose. + + Bryan Ribas + BSD + + http://www.r1b4z01d.com/ + https://github.com/r1b4z01d/vive-diy-position-sensor/issues + https://github.com/r1b4z01d/vive-diy-position-sensor + Bryan Ribas + + catkin + rospy + std_msgs + rospy +std_msgs + diff --git a/ros/vive_tracker/src/vive_tracker.py b/ros/vive_tracker/src/vive_tracker.py new file mode 100755 index 0000000..97eaf34 --- /dev/null +++ b/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()