def ondeIr():
global prox_area
global terminou
global num_prateleiras_arrumadas
global parar_arrumar
for i in range(0, 6):
i += 1
if num_prateleiras_arrumadas == 7:
parar_arrumar = True
return
else:
if prox_area == Areas.CASA:
rospy.logwarn('Como ficaram Areas A: %s, %s, %s, %s', Areas.A1,
Areas.A2, Areas.A3, Areas.A4)
rospy.logwarn('Como ficaram Areas B: %s, %s, %s, %s', Areas.B1,
Areas.B2, Areas.B3, Areas.B4)
while prox_area == Areas.CASA:
rospy.logwarn('Lendo os postes....')
prox_area = lendoPostes()
if areaOrganizada(prox_area, Objetos.NONE):
num_prateleiras_arrumadas += 1
ligandoLeds(sinalizaLeitura(prox_area), False)
ligandoLeds2(cores, True, 5)
rospy.logwarn('Area ja organizada, bora pra Proxima')
prox_area = lendoPostes()
else:
num_prateleiras_arrumadas += 1
ligandoLeds(sinalizaLeitura(prox_area), False)
return prox_area
def ondeIr():
global prox_area
global terminou
global num_prateleiras_arrumadas
global parar_arrumar
global setar_postes
global seq_postes
for i in range (0, 6):
i += 1
if num_prateleiras_arrumadas == 4:
parar_arrumar = True
return
else:
if prox_area == Areas.CASA:
rospy.logwarn('Como ficaram Areas A: %s, %s, %s, %s', Areas.A1, Areas.A2, Areas.A3, Areas.A4)
rospy.logwarn('Como ficaram Areas B: %s, %s, %s, %s', Areas.B1, Areas.B2, Areas.B3, Areas.B4)
'''
while prox_area == Areas.CASA:
rospy.logwarn('Lendo os postes....')
#prox_area = lendoPostes()
prox_area = lendoPostes(setar_postes, seq_postes)
setar_postes += 1
'''
if areaOrganizada(prox_area, Objetos.NONE):
num_prateleiras_arrumadas += 1
alinhaVoltandoCasa()
ligandoLeds(sinalizaLeitura(prox_area), False)
ligandoLeds2(cores, True, 5)
rospy.logwarn('Area ja organizada, bora pra Proxima')
#prox_area = lendoPostes()
alinhaParaLeitura()
prox_area = lendoPostes(setar_postes, seq_postes)
setar_postes += 1
else:
num_prateleiras_arrumadas += 1
alinhaVoltandoCasa()
ligandoLeds(sinalizaLeitura(prox_area), False)
return prox_area
def execute(self, userdata):
global objeto
global terminou
global areas
global prox_area_aux
global prox_area
rospy.logwarn("Area Atual: %s, Area Desejada: %s, Area Auxiliar: %s", userdata.area_atual, userdata.area_des, userdata.area_aux)
if userdata.area_atual[0] == Areas.CASA[0]:
desligandoLeds()
terminou = True
return 'fim_org'
if userdata.area_atual[0] == userdata.area_des[0]:
rospy.logwarn('Estou na Area Desejada com o objeto %s, buffer %s', userdata.area_atual[4], buffer_empty)
if not areaOrganizada(userdata.area_atual, objeto) and not areaComObjDesejado(userdata.area_des, userdata.area_aux):
success = True
terminou = True
userdata.prox_area = Areas.CASA
return 'comeca_org'
if areaOrganizada(userdata.area_atual, objeto) and buffer_empty:
rospy.logwarn('Estou na Prateleira Desejada com Objeto DESEJADO na primeira passada')
success = True
terminou = True
userdata.prox_area = Areas.CASA
return 'comeca_org'
if not areaOrganizada(userdata.area_atual, objeto) and buffer_empty:
rospy.logwarn('Estou na Prateleira Desejada com Objeto AUXILIAR')
if len(userdata.area_atual[4]) == 0:
userdata.prox_area = Areas.CASA
success = True
terminou = True
return 'comeca_org'
if userdata.area_aux == Areas.CASA:
userdata.prox_area = Areas.CASA
success = True
terminou = True
return 'comeca_org'
userdata.area_parc = userdata.area_atual
userdata.prox_area = Areas.BUFFER
return 'pegar_obj'
if areaOrganizada(userdata.area_atual, objeto) and not buffer_empty:
rospy.logwarn('Estou na Prateleira Desejada com Objeto DESEJADO')
userdata.area_parc = userdata.area_atual
userdata.prox_area = Areas.BUFFER
return 'deixar_obj'
elif userdata.area_atual[0] == Areas.BUFFER[0]:
rospy.logwarn('Estou no Buffer')
if buffer_empty == True:
rospy.logwarn('Estou no Buffer e vou deixar o Objeto AUXILIAR')
userdata.area_parc = userdata.area_atual
userdata.prox_area = userdata.area_aux
return 'deixar_obj'
if buffer_empty == False:
rospy.logwarn('Estou no Buffer e vou pegar o Objeto AUXILIAR')
userdata.area_parc = userdata.area_atual
userdata.prox_area = userdata.area_aux
return 'pegar_obj'
elif userdata.area_atual[0] == userdata.area_aux[0]:
rospy.logwarn('Estou na Prateleira Auxiliar, Buffer: %s', buffer_empty)
if areaComObjDesejado(userdata.area_des, userdata.area_atual) and not buffer_empty:
rospy.logwarn('Estou na Prateleira Auxiliar e vou pegar o Objeto DESEJADO')
rospy.logwarn('%s', userdata.area_atual)
userdata.area_parc = userdata.area_atual
userdata.prox_area = userdata.area_des
return 'pegar_obj'
if not areaComObjDesejado(userdata.area_des, userdata.area_atual) and buffer_empty:
rospy.logwarn('Estou na Prateleira Auxiliar e vou deixar o Objeto AUXILIAR e ir pra casa')
success = True
terminou = True
userdata.area_parc = userdata.area_atual
userdata.prox_area = Areas.CASA
return 'deixar_obj'
rospy.logerr('Deu algum erro, vou deixar o Objeto e ir pra casa')
success = False
terminou = True
userdata.prox_area = Areas.CASA
return 'deixar_obj'
