Example #1
0
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
Example #2
0
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'