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)