From fa0d7de50ac1f2a248e27bedf647e376e9c508cf Mon Sep 17 00:00:00 2001 From: r1b4z01d Date: Thu, 14 Feb 2019 21:00:59 -0600 Subject: [PATCH 1/6] untested commit of ros node --- .DS_Store | Bin 0 -> 6148 bytes ros/.DS_Store | Bin 0 -> 6148 bytes ros/vive_tracker_node/.DS_Store | Bin 0 -> 6148 bytes ros/vive_tracker_node/CMakeLists.txt | 14 ++++ ros/vive_tracker_node/launch/node.launch | 13 ++++ ros/vive_tracker_node/package.xml | 21 ++++++ ros/vive_tracker_node/src/vive_tracker.py | 76 ++++++++++++++++++++++ 7 files changed, 124 insertions(+) create mode 100644 .DS_Store create mode 100644 ros/.DS_Store create mode 100644 ros/vive_tracker_node/.DS_Store create mode 100755 ros/vive_tracker_node/CMakeLists.txt create mode 100755 ros/vive_tracker_node/launch/node.launch create mode 100755 ros/vive_tracker_node/package.xml create mode 100644 ros/vive_tracker_node/src/vive_tracker.py diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..1c89aa20524a11e1033ef1e436fd46db981a8631 GIT binary patch literal 6148 zcmeHK%}xR_5N-jr8)D>OBFA35aeyq!$z+3g@Mf|`4{BgHOJoz=5_XX&V)ixkjeG)M z$C$t$2vKV`Vew`87GmNw$&twQaz7v9FF4%r(3DcX0Cfs zlWwy$t{d5 zq8Cww!~iis3=jir#eh8qM0u^MrecW!V&EqX;Qk zY0rRomD?usILVt^Rpnh@%UfnQ+Y1AU86>;M1& literal 0 HcmV?d00001 diff --git a/ros/.DS_Store b/ros/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..a6fa88e01f64346f95b2f5bdc18619287bbf0432 GIT binary patch literal 6148 zcmeHK%}N6?5T4YkSwyTL=rI>>Jyhz?Nm!~Lya_9MP^r7_qKkDS?Y2d0WnV+z$S3f1 zoJmqqs^CRYnSseSnam`cFH3#^Ks37X3P1$_Br0La!{P&>cG58^IS-*QeKaA5HiU2n zmt)cF_>BzE+RZ}`Qq% z{-96^eTRibvvpuW*GFnE6Oy1!ZwW%>(05o^#1<4`QxR>da8C?j)6uV7obRx(XwyNM zk+B~)vT!dHVMa&4GT|V6i(E1T%)omFCTz7r=l{;<@BjBnTw?~9fq%t-C~pVb4V;oY zTgN6xXRSg#MJ1uQ!s2TR8s;d*SUQTAP>rBpk%8ztEG(i2g)ahz1}>O^A7$VLH+)&n literal 0 HcmV?d00001 diff --git a/ros/vive_tracker_node/.DS_Store b/ros/vive_tracker_node/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..ed42222fad1315c2ecfdae4b430ec945199277a5 GIT binary patch literal 6148 zcmeHKISv9b4733uBpOP}e1RWC2wuPkxC)9D6o`H+-o?`x9|dTkg90>`oJkx{qD-+~ zi-^uI`V2+Xjsa zPys4H1*iZO__zXDVn^eTpUeZP02TOk1?>A!;D$A^4fIb325$j?1BBf$_g(^6EC8&D zZ6G2r4Jt6Gnk|L~9r2QRHL(p0x@a~Znm229DC)Q4{Nm}NHIO3}paRbd^kX@(`oDy~ z>HnWeTu}ik@K*}xV6j@v@uaM+oyS?NE$}tma&B-l%$ + + + + + + + + + + + + diff --git a/ros/vive_tracker_node/package.xml b/ros/vive_tracker_node/package.xml new file mode 100755 index 0000000..5f14d9e --- /dev/null +++ b/ros/vive_tracker_node/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_node/src/vive_tracker.py b/ros/vive_tracker_node/src/vive_tracker.py new file mode 100644 index 0000000..374ec8f --- /dev/null +++ b/ros/vive_tracker_node/src/vive_tracker.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python +import rospy +import time +import rospkg +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Int16, Time +from tf.msg import tfMessage +import numpy as np +import cv2 +import cv2.aruco as aruco +import serial + + +def init(): + # print "init" + global ser + ser.write(b'continue') + time.sleep(0.5) + + +def vive_tracker(): + global ser + rospy.init_node('vive_tracker', anonymous=True) + rate = rospy.Rate(30) # 10hz + portname = rospy.get_param('~port') + rospy.loginfo('trying to connect to %s', portname) + try: + ser = serial.Serial(port=portname, baudrate=56700, timeout=1.0) + ret = ser.readline() + + if not ret.startswith('OBJ0'): + init() + + except: + print "failed" + rospy.logfatal('cannot connect to port %s', portname) + 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'): + rospy.loginfo(res) + rawData = res.split(",") + if len(rawData) == 7: + # Double check it is pos data + if rawData[0]=='POS' and rawData[2]>=0: + confidence = int(rawData[2]) + if confidence >= conf_thresh: + poseStamped=PoseStamped() + poseStamped.header.frame_id = "/vive" + poseStamped.header.stamp = rospy.Time.now() + poseStamped.pose.position.x = float(rawData[3]) + poseStamped.pose.position.y = float(rawData[5]) + poseStamped.pose.position.z = float(rawData[4]) + poseStamped.pose.orientation.x = rawData[3] + poseStamped.pose.orientation.y = rawData[5] + poseStamped.pose.orientation.z = rawData[4] + poseStamped.pose.orientation.w = rawData[6] + + #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() \ No newline at end of file From 62e2c7f5517c34ff2179a82b703bba06e923b918 Mon Sep 17 00:00:00 2001 From: Bryan Ribas Date: Thu, 14 Feb 2019 21:02:14 -0600 Subject: [PATCH 2/6] Delete .DS_Store --- .DS_Store | Bin 6148 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 .DS_Store diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index 1c89aa20524a11e1033ef1e436fd46db981a8631..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6148 zcmeHK%}xR_5N-jr8)D>OBFA35aeyq!$z+3g@Mf|`4{BgHOJoz=5_XX&V)ixkjeG)M z$C$t$2vKV`Vew`87GmNw$&twQaz7v9FF4%r(3DcX0Cfs zlWwy$t{d5 zq8Cww!~iis3=jir#eh8qM0u^MrecW!V&EqX;Qk zY0rRomD?usILVt^Rpnh@%UfnQ+Y1AU86>;M1& From 177abd2ee5c5e7201f66e6abd509f7df431901ca Mon Sep 17 00:00:00 2001 From: Bryan Ribas Date: Thu, 14 Feb 2019 21:02:24 -0600 Subject: [PATCH 3/6] Delete .DS_Store --- ros/.DS_Store | Bin 6148 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 ros/.DS_Store diff --git a/ros/.DS_Store b/ros/.DS_Store deleted file mode 100644 index a6fa88e01f64346f95b2f5bdc18619287bbf0432..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6148 zcmeHK%}N6?5T4YkSwyTL=rI>>Jyhz?Nm!~Lya_9MP^r7_qKkDS?Y2d0WnV+z$S3f1 zoJmqqs^CRYnSseSnam`cFH3#^Ks37X3P1$_Br0La!{P&>cG58^IS-*QeKaA5HiU2n zmt)cF_>BzE+RZ}`Qq% z{-96^eTRibvvpuW*GFnE6Oy1!ZwW%>(05o^#1<4`QxR>da8C?j)6uV7obRx(XwyNM zk+B~)vT!dHVMa&4GT|V6i(E1T%)omFCTz7r=l{;<@BjBnTw?~9fq%t-C~pVb4V;oY zTgN6xXRSg#MJ1uQ!s2TR8s;d*SUQTAP>rBpk%8ztEG(i2g)ahz1}>O^A7$VLH+)&n From 6d0b3b9dc1b435cc357c3ae22c1de4a81e9536d4 Mon Sep 17 00:00:00 2001 From: Bryan Ribas Date: Thu, 14 Feb 2019 21:02:33 -0600 Subject: [PATCH 4/6] Delete .DS_Store --- ros/vive_tracker_node/.DS_Store | Bin 6148 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 ros/vive_tracker_node/.DS_Store diff --git a/ros/vive_tracker_node/.DS_Store b/ros/vive_tracker_node/.DS_Store deleted file mode 100644 index ed42222fad1315c2ecfdae4b430ec945199277a5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6148 zcmeHKISv9b4733uBpOP}e1RWC2wuPkxC)9D6o`H+-o?`x9|dTkg90>`oJkx{qD-+~ zi-^uI`V2+Xjsa zPys4H1*iZO__zXDVn^eTpUeZP02TOk1?>A!;D$A^4fIb325$j?1BBf$_g(^6EC8&D zZ6G2r4Jt6Gnk|L~9r2QRHL(p0x@a~Znm229DC)Q4{Nm}NHIO3}paRbd^kX@(`oDy~ z>HnWeTu}ik@K*}xV6j@v@uaM+oyS?NE$}tma&B-l%$ Date: Thu, 14 Feb 2019 21:42:03 -0600 Subject: [PATCH 5/6] works but needs testing --- .../CMakeLists.txt | 0 .../launch/node.launch | 2 +- .../package.xml | 0 ros/vive_tracker/src/vive_tracker.py | 63 +++++++++++++++ ros/vive_tracker_node/src/vive_tracker.py | 76 ------------------- 5 files changed, 64 insertions(+), 77 deletions(-) rename ros/{vive_tracker_node => vive_tracker}/CMakeLists.txt (100%) rename ros/{vive_tracker_node => vive_tracker}/launch/node.launch (75%) rename ros/{vive_tracker_node => vive_tracker}/package.xml (100%) create mode 100755 ros/vive_tracker/src/vive_tracker.py delete mode 100644 ros/vive_tracker_node/src/vive_tracker.py diff --git a/ros/vive_tracker_node/CMakeLists.txt b/ros/vive_tracker/CMakeLists.txt similarity index 100% rename from ros/vive_tracker_node/CMakeLists.txt rename to ros/vive_tracker/CMakeLists.txt diff --git a/ros/vive_tracker_node/launch/node.launch b/ros/vive_tracker/launch/node.launch similarity index 75% rename from ros/vive_tracker_node/launch/node.launch rename to ros/vive_tracker/launch/node.launch index 2b02c9f..d30cbd5 100755 --- a/ros/vive_tracker_node/launch/node.launch +++ b/ros/vive_tracker/launch/node.launch @@ -4,7 +4,7 @@ - + diff --git a/ros/vive_tracker_node/package.xml b/ros/vive_tracker/package.xml similarity index 100% rename from ros/vive_tracker_node/package.xml rename to ros/vive_tracker/package.xml diff --git a/ros/vive_tracker/src/vive_tracker.py b/ros/vive_tracker/src/vive_tracker.py new file mode 100755 index 0000000..ad972c6 --- /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]) + 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() diff --git a/ros/vive_tracker_node/src/vive_tracker.py b/ros/vive_tracker_node/src/vive_tracker.py deleted file mode 100644 index 374ec8f..0000000 --- a/ros/vive_tracker_node/src/vive_tracker.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python -import rospy -import time -import rospkg -from geometry_msgs.msg import PoseStamped -from std_msgs.msg import Int16, Time -from tf.msg import tfMessage -import numpy as np -import cv2 -import cv2.aruco as aruco -import serial - - -def init(): - # print "init" - global ser - ser.write(b'continue') - time.sleep(0.5) - - -def vive_tracker(): - global ser - rospy.init_node('vive_tracker', anonymous=True) - rate = rospy.Rate(30) # 10hz - portname = rospy.get_param('~port') - rospy.loginfo('trying to connect to %s', portname) - try: - ser = serial.Serial(port=portname, baudrate=56700, timeout=1.0) - ret = ser.readline() - - if not ret.startswith('OBJ0'): - init() - - except: - print "failed" - rospy.logfatal('cannot connect to port %s', portname) - 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'): - rospy.loginfo(res) - rawData = res.split(",") - if len(rawData) == 7: - # Double check it is pos data - if rawData[0]=='POS' and rawData[2]>=0: - confidence = int(rawData[2]) - if confidence >= conf_thresh: - poseStamped=PoseStamped() - poseStamped.header.frame_id = "/vive" - poseStamped.header.stamp = rospy.Time.now() - poseStamped.pose.position.x = float(rawData[3]) - poseStamped.pose.position.y = float(rawData[5]) - poseStamped.pose.position.z = float(rawData[4]) - poseStamped.pose.orientation.x = rawData[3] - poseStamped.pose.orientation.y = rawData[5] - poseStamped.pose.orientation.z = rawData[4] - poseStamped.pose.orientation.w = rawData[6] - - #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() \ No newline at end of file From 7924a082eda73784b070e89f65eaa72b23235bd7 Mon Sep 17 00:00:00 2001 From: Bryan Ribas Date: Thu, 14 Feb 2019 21:56:11 -0600 Subject: [PATCH 6/6] inverted axis --- ros/vive_tracker/src/vive_tracker.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/vive_tracker/src/vive_tracker.py b/ros/vive_tracker/src/vive_tracker.py index ad972c6..97eaf34 100755 --- a/ros/vive_tracker/src/vive_tracker.py +++ b/ros/vive_tracker/src/vive_tracker.py @@ -43,7 +43,7 @@ def vive_tracker(): poseStamped=PoseStamped() poseStamped.header.frame_id = "/vive" poseStamped.header.stamp = rospy.Time.now() - poseStamped.pose.position.x = float(rawData[3]) + poseStamped.pose.position.x = float(rawData[3])*-1 poseStamped.pose.position.y = float(rawData[5]) poseStamped.pose.position.z = float(rawData[4])