def test_stop_all():
osc = OSCThreadServer()
sock = osc.listen(default=True)
host, port = sock.getsockname()
osc.listen()
assert len(osc.sockets) == 2
osc.stop_all()
assert len(osc.sockets) == 0
osc.listen(address=host, port=port)
assert len(osc.sockets) == 1
osc.stop_all()
address, port = osc.getaddress(sock)
osc.bind(b'/count', count, sock=sock)
for i, pattern in enumerate(patterns):
for safer in (False, True):
timeout = time() + DURATION
sent = 0
received = 0
while time() < timeout:
send_message(b'/count',
pattern,
address,
port,
sock=SOCK,
safer=safer)
sent += 1
sleep(10e-9)
size = len(format_message(b'/count', pattern)[0]) / 1000.
print(
f"{i}: safe: {safer}\t",
f"sent:\t{sent}\t({sent / DURATION}/s)\t({sent * size / DURATION:.2f}MB/s)\t" # noqa
f"received:\t{received}\t({received / DURATION}/s)\t({received * size / DURATION:.2f}MB/s)\t" # noqa
f"loss {((sent - received) / sent) * 100}%")
if family == 'unix':
os.unlink(address)
osc.stop_all()
class OSC_OT_OSCPyServer(OSC_OT_OSCServer):
bl_idname = "nodeosc.oscpy_operator"
bl_label = "OSCMainThread"
_timer = None
count = 0
#####################################
# CUSTOMIZEABLE FUNCTIONS:
inputServer = "" #for the receiving socket
outputServer = "" #for the sending socket
# setup the sending server
def setupInputServer(self, context, envars):
self.dispatcher = dispatcher.Dispatcher()
# setup the receiving server
def setupOutputServer(self, context, envars):
#For sending
self.outputServer = OSCClient(envars.udp_out, envars.port_out)
self.outputServer.send_message(b'/NodeOSC', [b'Python server started up'])
print("OSCPy Server sended test message to " + envars.udp_out + " on port " + str(envars.port_out))
def sendingOSC(self, context, event):
oscMessage = {}
# gather all the ouput bound osc messages
make_osc_messages(bpy.context.scene.NodeOSC_outputs, oscMessage)
# and send them
for key, args in oscMessage.items():
values = []
if isinstance(args, (tuple, list)):
for argum in args:
if type(argum) == str:
argum = bytes(argum, encoding='utf-8')
values.append(argum)
else:
if type(args) == str:
args = bytes(args, encoding='utf-8')
values.append(args)
self.outputServer.send_message(bytes(key, encoding='utf-8'), values)
# add method
def addMethod(self, address, data):
pass #already set during creation of inputserver
# add default method
def addDefaultMethod(self):
pass #already set during creation of inputserver
# start receiving
def startupInputServer(self, context, envars):
print("Create OscPy Thread...")
# creating a blocking UDP Server
# Each message will be handled sequentially on the same thread.
self.inputServer = OSCThreadServer(encoding='utf8', default_handler=OSC_callback_oscpy)
sock = self.inputServer.listen(address=envars.udp_in, port=envars.port_in, default=True)
print("... server started on ", envars.port_in)
# stop receiving
def shutDownInputServer(self, context, envars):
print("OSCPy Server is shutting down...")
self.inputServer.stop() # Stop default socket
print(" stopping all sockets...")
self.inputServer.stop_all() # Stop all sockets
print(" terminating server...")
self.inputServer.terminate_server() # Request the handler thread to stop looping
self.inputServer.join_server() # Wait for the handler thread to finish pending tasks and exit
print("... OSCPy Server is shutdown")
class Riot(QObject):
acc = pyqtSignal(float, float, float)
gyro = pyqtSignal(float, float, float)
mag = pyqtSignal(float, float, float)
tmp = pyqtSignal(float)
btn = pyqtSignal(int)
switch = pyqtSignal(int)
euler = pyqtSignal(float, float, float, float)
quat = pyqtSignal(float, float, float, float)
analog = pyqtSignal(float, float)
face = pyqtSignal(int)
def __init__(self, id):
super().__init__()
self.osc = OSCThreadServer()
self.sock = None
self.riot_id = id
self.prev_x = self.prev_y = self.prev_z = 0
self.prev_gx = self.prev_gy = self.prev_gz = 0
def OSCcallback(self, *args):
self.acc.emit(args[0], args[1], args[2])
self.gyro.emit(args[3], args[4], args[5])
self.mag.emit(args[6], args[7], args[8])
self.tmp.emit(args[9])
self.btn.emit(args[10])
self.switch.emit(args[11])
self.analog.emit(args[12], args[13])
self.quat.emit(args[14], args[15], args[16], args[17])
self.euler.emit(args[18], args[19], args[20], args[21])
def start(self):
try:
port = 8000 + self.riot_id
self.sock = self.osc.listen(address='0.0.0.0',
port=port,
default=True)
address = '/' + str(self.riot_id) + '/raw'
self.osc.bind(address.encode(), self.OSCcallback)
print('riot id', self.riot_id, "started on port:", port)
except KeyboardInterrupt:
self.osc.stop_all()
print('stopped all for keyboard interupt')
def stop(self):
self.osc.stop_all()
def intensity(self, _x, _y, _z):
x = _x * _x
y = _y * _y
z = _z * _z
x = x + 0.8 * self.prev_x
y = y + 0.8 * self.prev_y
z = z + 0.8 * self.prev_z
self.prev_x = x
self.prev_y = y
self.prev_z = z
norm = x + y + z
return norm, x, y, z
def spin_filtered(self, _x, _y, _z):
x = _x + 0.8 * self.prev_gx
y = _y + 0.8 * self.prev_gy
z = _z + 0.8 * self.prev_gz
self.prev_gx = x
self.prev_gy = y
self.prev_gz = z
return x, y, z
class riot_osc():
'''classe principale pour le controle du drone par le capteur'''
#NOTTODO : fonction atterissage;
#TODO : si tous les modes (surtout avec multiranger en même temps - si thread parallel -> probleme?)
#todo : orientation fonctionne (lag visible (- une seconde ~250ms)), altitude: pbs pas trop deterministe
# vitesses max évitement
#bien modifiée en paramètres mais evolution dynamique bizarre, tests multi-ranger pas faits.
#cnotrol manuel fonctionne avec clavier directionnel + u et d pour altitude (plutot reactif (comme orientaiton voir moins))
def __init__(self, multiranger, motion_commander, cf = None, to_alt = 0.5, graph = False):
self.osc = OSCThreadServer()
#les valeurs d'angle renvoyée par la centrale gyroscopique sur les 3 axes
self.euler_x = 0
self.euler_y = 0
self.euler_z = 0
#les angles renvoyés lors du signal précedent
self.ante_x = 0
self.ante_z = 0
self.ante_y = 0
#la valeur d'angle de la commande pour faire tourner le drone
self.angle = 0
#compte le nombre de fois où la différence entre la valeur d'angle recu et la valeur précédente,
#pour éviter de surcharger le drone de commande
self.count = 0
#booléen renseignant sur l'état du drone : en vol ou non
self.keep_flying = True
#altitude du drone
self.z = to_alt
#vitesse d'évitement initiale du drone
self.velocity = 0.5
#initialisation des valeurs d'angle sur x et z au commencement du programme
self.orig_x = 0
self.orig_z = 0
#variables propres au drones
self.cf = cf
self.multiranger = multiranger
self.motion_commander = motion_commander
#variable indiquant si le programme doit commencer à compter (ou pas) le nombre
#de valeur d'angle non nul
self.flag = False
self.sock = 0
#Initialisation du thread_multirange
#ATTENTION A DES POSSIBLES PROBLEMES, LE DRONE EST SUREMENT UNE RESSOURCE CRITIQUE
self.thread_multirange = Thread(target = self.multirange_push)
self.thread_multirange.start()
def set_orig(self, *args, axes = "all"):
#fonction lancée au début du programme qui récupère les valeurs initiales du capteur pour définir une origine pour la suite
if axes == "all" or axes == "x":
self.orig_x = self._normalize__(args[18],0)
if axes == "all" or axes == "z":
self.orig_z = self._normalize__(args[20],0)
self.osc.unbind(b'/0/raw',self.set_orig)
self.osc.bind(b'/0/raw',self.callback)
def _normalize__(self, value, orig):
#fonction qui prend en paramètre une valeur renvoyé par le capteur et l'origine définit au lancement
#qui normalise cette valeur entre 0 et 360
value = round(value)
if value < 0:
value = 360 + value
value -= orig
if value < 0 : value += 360
return value
def callback(self, *args):
#fonction principale de callback liée au capteur
self.euler_z = self._normalize__(args[20], self.orig_z)
try:
self.euler_y = round(args[19])
except:
self.euler_y = -90
#print(self.euler_x, self.euler_y, self.euler_z)
if abs(self.euler_z) <= 10 and abs(self.euler_y) <= 10:
#appel de la fonction de contrôle de l'orientation
self.callback_x(*args)
elif abs(self.euler_z - 90) <= 10 and abs(self.euler_y) <= 10:
#appel de la fonction d'atterrissage
self.landing()
elif abs(self.euler_z - 270) <= 10 and abs(self.euler_y) <= 10:
#appel de la fonction de contrôle de la vitesse d'évitement
self.set_velocity(*args)
elif abs(self.euler_z - 180) <= 15 and abs(self.euler_y) <= 12:
#appel de la fonction de vol stationaire
self.idle()
elif abs(self.euler_y - 90) <= 10:
#appel de la fonction de contrôle de l'altitude
self.callback_z(*args)
elif abs(self.euler_y + 90) <= 20:
#appel de la fonctoin pour controler manuellement le drone
self.manual_control()
else:
pass
def landing(self):
#fonction d'atterrisage, besoin de valider par appui sur espace
print("appuyer sur espace quelque seconde pour atterrir et ensuite arrêter le drone")
if keyboard.is_pressed("space"):
self.keep_flying = False
print("Atterrissage...")
def idle(self):
#fonction de vol stationnaire
print("idle...")
pass
def set_velocity(self, *args):
#fonction pour modifier la vitesse d'évitement du multiranger
self.euler_x = self._normalize__(args[18],self.orig_x)
angle = self.euler_x - self.ante_x
if abs(angle) >= 180 :
n_angle = angle % 180
angle = n_angle if angle > 0 else -n_angle
if angle != 0 :
delta = angle/360
if delta <= self.velocity: self.velocity += delta
print("La vitesse d'évitement est maintenant de = ", self.velocity, " m/s. ", angle)
self.ante_x = self.euler_x
def callback_z(self, *args):
#fonction de contrôle de l'altitude
self.euler_x = self._normalize__(args[18],self.orig_x)
angle = self.euler_x - self.ante_x
if abs(angle) >= 180 :
n_angle = angle % 180
angle = n_angle if angle > 0 else -n_angle
if angle != 0:
self.flag = True
self.angle += angle
else:
if self.flag : self.count += 1
if self.count == 10:
if self.cf != None :
if abs(self.angle) > 0:
if self.angle <= 0:
self.motion_commander.down(abs(self.angle)/360,0.5)
self.z -= abs(self.angle)/360
if self.z < 0.15:
self.keep_flying = False
else:
self.motion_commander.up(self.angle/720,0.5)
self.z += abs(self.angle)/360
self.count = 0
self.angle = 0
self.flag = False
self.ante_x = self.euler_x
def callback_x(self, *args):
#fonction de contrôle de l'orientation (lacet)
self.euler_x = self._normalize__(args[18],self.orig_x)
angle = self.euler_x - self.ante_x
if abs(angle) >= 180 :
n_angle = angle % 180
angle = n_angle if angle > 0 else -n_angle
if angle != 0:
self.flag = True
self.angle += angle
else:
if self.flag : self.count += 1
if self.count == 10:
if self.cf != None :
if abs(self.angle) > 0:
if self.angle >= 0:
self.motion_commander.turn_left(abs(self.angle), 180)
else:
self.motion_commander.turn_right(abs(self.angle), 180)
self.count = 0
self.angle = 0
self.flag = False
self.ante_x = self.euler_x
def manual_control(self):
#fonction de contrôle manuel du drone
velocity_x = 0
velocity_y = 0
velocity_z = 0
VELOCITY = 0.5
if keyboard.is_pressed("up"):
velocity_x += VELOCITY
if keyboard.is_pressed("down"):
velocity_x -= VELOCITY
if keyboard.is_pressed("left"):
velocity_y -= VELOCITY
if keyboard.is_pressed("right"):
velocity_y += VELOCITY
if keyboard.is_pressed("u"):
velocity_z += VELOCITY
if keyboard.is_pressed("d"):
velocity_z -= VELOCITY
self.motion_commander.start_linear_motion(velocity_x, velocity_y, velocity_z)
print("velocity_x = ", velocity_x, " velocity_y = ", velocity_y, " velocity_z = ", velocity_z)
def sock_connect(self):
#fonction qui établit la connection avec le capteur au lancement
self.sock = self.osc.listen(address='0.0.0.0', port=8000, default=True)
def run(self):
#fonction principale qui établit la position d'origine puis lie la fonction de callback au capteur
#et lance une boucle infinie tant que la fonction d'aterrrissage n'est pas lancée
try:
self.sock_connect()
self.osc.bind(b'/0/raw',self.set_orig)
print(self.keep_flying)
while self.keep_flying:
sleep(0.05)
except KeyboardInterrupt:
self.osc.stop_all()
print('stopped all for keyboard interupt')
self.thread_multirange.join()
def stop(self):
#fonction qui libère le capteur à la fin du programme
self.osc.stop(self.sock)
print("Stopped OCS")
def multirange_push(self):
#fonction appelée par le thread du multiranger qui permet de la faire tourner en concurrence avec les fonctions
#de contrôle de l'orientation et autre
def is_close(range):
MIN_DISTANCE = 0.2 # m
if range is None:
return False
else:
return range < MIN_DISTANCE
while self.keep_flying:
velocity_x = 0
velocity_y = 0
if is_close(self.multiranger.front):
velocity_x -= self.velocity
if is_close(self.multiranger.back):
velocity_x += self.velocity
if is_close(self.multiranger.left):
velocity_y -= self.velocity
if is_close(self.multiranger.right):
velocity_y += self.velocity
if is_close(self.multiranger.up):
self.keep_flying = False
self.motion_commander.start_linear_motion(
velocity_x, velocity_y, 0)
sleep(0.1)
