66 lines
1.9 KiB
Python
66 lines
1.9 KiB
Python
|
#from helpfunctions import *
|
||
|
|
import numpy as np
|
||
|
|
import time
|
||
|
|
|
||
|
|
class OSCReceive:
|
||
|
|
from pythonosc import dispatcher
|
||
|
|
from pythonosc import osc_server
|
||
|
|
|
||
|
|
import threading
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
def handle_received_osc_joint(self,address, *args):
|
||
|
|
|
||
|
|
self.server=None
|
||
|
|
self.server_thread=None
|
||
|
|
|
||
|
|
self.oscreceiveintervals[self.oscreceiveintervals_pos]=time.time()-self.last_oscreceive
|
||
|
|
self.last_oscreceive=time.time()
|
||
|
|
self.oscreceiveintervals_pos+=1
|
||
|
|
self.oscreceiveintervals_pos%=np.size(self.oscreceiveintervals)
|
||
|
|
|
||
|
|
#print(f"Received OSC message on {address}")
|
||
|
|
joint_q_aim=[0,0,0,0,0,0]
|
||
|
|
try:
|
||
|
|
for iarg, arg in enumerate(args):
|
||
|
|
#print(f"Data: {arg}")
|
||
|
|
joint_q_aim[iarg]=float(arg)
|
||
|
|
except:
|
||
|
|
print("Data not in right format")
|
||
|
|
print("Data: "+str(args))
|
||
|
|
else:
|
||
|
|
if self.ur is not None:
|
||
|
|
self.ur.setJointQAim(joint_q_aim)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
def getCurrentReceiveFrequency(self):
|
||
|
|
return 1.0/np.mean(self.oscreceiveintervals)
|
||
|
|
|
||
|
|
def __init__(self,ip,port,_ur=None):
|
||
|
|
|
||
|
|
#osc receive statistics
|
||
|
|
self.oscreceiveintervals = np.zeros(100)
|
||
|
|
self.oscreceiveintervals_pos=0
|
||
|
|
self.last_oscreceive=0
|
||
|
|
|
||
|
|
self.ur = _ur
|
||
|
|
|
||
|
|
|
||
|
|
# Set up the dispatcher for the server
|
||
|
|
disp = self.dispatcher.Dispatcher()
|
||
|
|
disp.map("/joint", self.handle_received_osc_joint) # You can change '/filter' to any address pattern you expect
|
||
|
|
|
||
|
|
# Set up OSC server
|
||
|
|
self.server = self.osc_server.ThreadingOSCUDPServer((ip, port), disp)
|
||
|
|
print(f"Serving on {self.server.server_address}")
|
||
|
|
self.server_thread = self.threading.Thread(target=self.server.serve_forever)
|
||
|
|
self.server_thread.start()
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
def disconnect(self):
|
||
|
|
self.server.shutdown()
|
||
|
|
self.server_thread.join()
|