Vive postion transport via UDP

RunOnUbuntu_UDPToROS_TF.py

#!/usr/bin/env python
import socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('0.0.0.0',13593))

import rospy
import tf
import bson
from threading import Thread, Lock
rospy.init_node("vtrans")
tfLock=Lock()
running=True

datas={} 
lastTime=0
def recvthread():
    global datas,lastTime
    timeOffset=0.001
    
    remoteTimeDelta=None
    for i in range(20) :
        p=sock.recv(1500)
    while running:
        p=sock.recv(1500)
        now=rospy.Time.now().to_sec()
        #print p
        try:
            data=bson.loads(p)
        except ValueError:
            print "BSON value error"
            return
        if(None is remoteTimeDelta):remoteTimeDelta=data["t"]-now
        d=data["t"]-now
        #data["t"] will convert ros time via data["t"] - remoteTimeDelta -timeOffest

        if( d-remoteTimeDelta<0):
            remoteTimeDelta=remoteTimeDelta*0.999+0.001*d
        else:
            remoteTimeDelta=remoteTimeDelta*0.9+0.1*d
        with tfLock:
            lastTime=data["t"]-remoteTimeDelta-timeOffset
            for name,x,y,z,qx,qy,qz,qw in data["p"]:
                datas[name]=[lastTime,[x,y,z],[qx,qy,qz,qw]]
            
                
                


    
r = rospy.Rate(20) 
br = tf.TransformBroadcaster()
th_recv=Thread(name='recv', target=recvthread)

th_recv.start()

import tf    
tfl = tf.TransformListener()
count=0
while not rospy.is_shutdown():
    count+=1
    with tfLock:
        for name,d in datas.iteritems():
            if(lastTime-d[0]<1):
                br.sendTransform(d[1],d[2],rospy.Time(d[0]),"v_"+name,"v_base")
        
    r.sleep()
running=False
th_recv.join()
sock.close()

RunOnWindows_sendViaUDP.py

masterIP="10.0.1.108"

import time
import openvr
import numpy as np
from Quaternion import Quat
import bson

import socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('0.0.0.0',13592))
openvr.init(openvr.VRApplication_Scene)
poses_t = openvr.TrackedDevicePose_t * openvr.k_unMaxTrackedDeviceCount
poses = poses_t()
dev_cont=[]
dev_track=[]

for i in range(openvr.k_unMaxTrackedDeviceCount):
    devClass=openvr.IVRSystem().getTrackedDeviceClass(i)
    if(devClass==0):continue
    serial=openvr.IVRSystem().getStringTrackedDeviceProperty(i,openvr.Prop_SerialNumber_String )

    print(devClass,serial )
        
    if(devClass==2):dev_cont.append(i)
    if(devClass==4):dev_track.append(i)
    

while(True):
    openvr.VRCompositor().waitGetPoses(poses, len(poses), None, 0)
    t=time.time()
    data={}
    data["t"]=t
    data["p"]=[]        
    for j in range(openvr.k_unMaxTrackedDeviceCount):
        devClass=openvr.IVRSystem().getTrackedDeviceClass(j)
        if(devClass==0):continue
        serial=str(devClass)+openvr.IVRSystem().getStringTrackedDeviceProperty(j,openvr.Prop_SerialNumber_String).decode('utf-8')
        
        pose = poses[j].mDeviceToAbsoluteTracking
        x=pose[0][3]
        z=pose[1][3]
        y=-1* pose[2][3]        
        rot=[v[:3] for v in pose[:3]]
        q=Quat(np.array(rot))
    
        data["p"].append( (serial,x,y,z,q.q[0],-q.q[2],q.q[1],q.q[3]) )
        #print(serial,x,y,z,q)
    
    sock.sendto(bson.dumps(data), (masterIP, 13593))
    
    #time.sleep(0.2)

openvr.shutdown()