def func(input, output): print 'Odometry process running...' odometry_queue = Queue() laser_queue = Queue() viewer = http_viewer.HttpViewer(port_web_server, laser_queue, odometry_queue) S = float(input.get()) #print 'S=' + str(S) anterior = [0, 0, 0] output.put('m') msg = input.get().split("\n") L_ini = float(msg[4].split(',')[1]) R_ini = float(msg[8].split(',')[1]) anterior = [L_ini, R_ini, 0, 0, 0] motReq = False quite = False while not quite: try: msg = input.get_nowait() if msg == "q": print "NO ME CIERRO JODER" printVerbose("quite", verboseCommands) quite = True viewer.quit() elif msg == "o2": output.put( str(anterior[2]) + "," + str(anterior[3]) + "," + str(anterior[4])) elif msg[0] == "m": #Obtencion de datos del motor printVerbose("motors", verboseCommands) print "ENTRO A ODOMETRY MODULE M" msg = msg.split( "\n") #dentro de msg se encuentran los valores del motor L = float(msg[4].split(',')[1]) R = float(msg[8].split(',')[1]) anterior = odometry(anterior, L, R) odometry_queue.put([(anterior[2], anterior[3]), (100, 100)]) print "\rOdometry: x:" + str(anterior[2]) + " y:" + str( anterior[3]) + " theta:" + str(anterior[4] * 180 / math.pi) motReq = False elif msg[0] == "l": #Obtencion de datos del laser printVerbose("laser", verboseCommands) msg = msg[ 1:] #dentro de msg se encuentran los valores del laser elif not motReq: #odometry(anterior,L,R) output.put("motors") motReq = True time.sleep(0.1) except Empty: if not motReq: #odometry(anterior,L,R) output.put("motors") motReq = True time.sleep(0.1)
sensor = [] for i in range(0, 5): sensor.append(dist_max) for j in range(-60 + i * 24, -60 + 24 + i * 24): if int(laser[j][3]) == 0: if (int(laser[j][1]) < sensor[i]): sensor[i] = int(laser[j][1]) return sensor if __name__ == "__main__": odometry_queue = Queue() laser_queue = Queue() viewer = http_viewer.HttpViewer(port_web_server, laser_queue, odometry_queue) print "To open the viewer go to: http:\\\\192.168.100.1:" + str( port_web_server) print "To see the log run in a shell the next comnnad: 'tail -f log.txt'" print "Press 'Q' to stop the execution." # Iniciamos ------------------------------- global ser ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05) print 'INI: SerialPort -> on' envia(ser, 'TestMode On', 0.2) envia(ser, 'PlaySound 1') print 'INI: TestMode -> on' envia(ser, 'SetMotor RWheelEnable LWheelEnable') print 'INI: SetMotor -> on' envia(ser, 'SetLDSRotation On', 0.2, False)
def func(input, output): print 'Odometry process running...' laser_set = Set() odometry_queue = Queue() laser_queue = Queue() viewer = http_viewer.HttpViewer(port_web_server, laser_queue, odometry_queue) S = float(input.get()) #print 'S=' + str(S) anterior = [0, 0, 0] output.put('m') msg = input.get().split("\n") L_ini = float(msg[4].split(',')[1]) R_ini = float(msg[8].split(',')[1]) anterior = [L_ini, R_ini, 0, 0, 0] laserMotor = True motReq = False lasReq = False numOdometrys = 0 quite = False while not quite: try: msg = input.get_nowait() if msg == "q": printVerbose("quite", verboseCommands) quite = True viewer.quit() elif msg == "o2": output.put( str(anterior[2]) + "," + str(anterior[3]) + "," + str(anterior[4])) elif msg[0] == "m": #Obtencion de datos del motor printVerbose("motors", verboseCommands) msg = msg.split( "\n") #dentro de msg se encuentran los valores del motor L = float(msg[4].split(',')[1]) R = float(msg[8].split(',')[1]) anterior = odometry(anterior, L, R) odometry_queue.put([(anterior[2], anterior[3]), (100, 100)]) print "\rOdometry: x:" + str(anterior[2]) + " y:" + str( anterior[3]) + " theta:" + str(anterior[4] * 180 / math.pi) motReq = False if (numOdometrys > 5): laserMotor = False numOdometrys += 1 elif msg[0] == "l": #Obtencion de datos del laser printVerbose("laser", verboseCommands) msg = msg[ 1:] #dentro de msg se encuentran los valores del laser laser_points = get_laser(msg) var = polar_to_cart(laser_points) if len(var) > 0: var = cart_to_world(var, anterior) for i in var: tuple = (int(i[0]), int(i[1])) if tuple not in laser_set: laser_queue.put([(i[0], i[1]), (100, 100)]) laser_set.add(tuple) lasReq = False laserMotor = True numOdometrys = 0 elif not motReq: #odometry(anterior,L,R) output.put("motors") motReq = True time.sleep(0.1) except Empty: if not motReq and not lasReq: if laserMotor: #odometry(anterior,L,R) print "\nEnviem Calc Odometry" output.put("motors") motReq = True time.sleep(0.1) else: print "\nEnviem Calc Laser" output.put("laser") lasReq = True time.sleep(0.1)
Pk1[1][1]) + ' Pk_thth: ' + str(Pk1[2][2]) # Funcion ejecutada por el thread auxiliar encargado de gestionar la entrada de datos no bloqueante def f(q): r = '' while r != "q" and r != "b": r = getch() q.put(r) if __name__ == "__main__": # Initialize WebServer laser_queue = Queue() pose_queue = Queue() viewer = http_viewer.HttpViewer(port_web_server, laser_queue, pose_queue) print "To open the viewer go to: http:\\\\192.168.100.1:" + str( port_web_server) # Initialize Robot global ser ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05) envia(ser, 'TestMode On') envia(ser, 'PlaySound 1') # Habilitamos motores y laser envia(ser, 'SetMotor RWheelEnable LWheelEnable') envia(ser, 'SetLDSRotation On') #Initialize Robot parameters speed = 0 # en mm/s tita_dot = 0
envia(ser,'TestMode On', 0.2) envia(ser,'PlaySound 1', 0.3) envia(ser,'SetMotor RWheelEnable LWheelEnable', 0.2) envia(ser,'SetLDSRotation On',0.1) old_settings = termios.tcgetattr(sys.stdin) tty.setcbreak(sys.stdin.fileno()) port_web_server=123 r_queue = Queue() l_queue = Queue() viewer = http_viewer.HttpViewer(port_web_server, l_queue, r_queue) #viewer.main_http_server() # Parametros Robot. S = 121.5 # en mm distancia_L = 0 # en mm distancia_R = 0 # en mm speed = 0 # en mm/s tita_dot = 0 tiempo = 20 direccion = 0 L_ini, R_ini = get_motors() x_world=x_ini y_world=y_ini theta_world=theta_ini
def viewer_start(self, port_web_server): self.laser_queue = Queue() self.odometry_queue = Queue() self.viewer = http_viewer.HttpViewer(port_web_server, self.laser_queue, self.odometry_queue) debug("Started Http Viewer at port %i" % port_web_server)