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)