Solucion merge

camino.py

+from enum import Enum
+
+class Bloque(Enum):
+    Edificio = 0
+    Calle_Izquierda_Derecha = 1
+    Calle_Arriba_Abajo = 2
+    Calle_Arriba_Derecha = 3
+    Calle_Derecha_Abajo = 4
+    Calle_Abajo_Izquierda = 5
+    Calle_Izquierda_Arriba = 6
+    Calle_Izquierda_Arriba_Derecha = 7
+    Calle_Arriba_Derecha_Abajo = 8
+    Calle_Derecha_Abajo_Izquierda = 9
+    Calle_Abajo_Izquierda_Arriba = 10
+    Calle_Arriba_Derecha_Abajo_Izquierda = 11
+
+class Direccion(Enum):
+    Arriba = 0
+    Derecha = 1
+    Abajo = 2
+    Izquierda = 3
+
+class Casilla:
+
+    def __init__(self,x:int,y:int,codigo_bloque:str):
+        self.x = x
+        self.y = y
+        self.bloque = Bloque(int(codigo_bloque))
+
+    def __str__(self):
+        return f'x : {self.x} | y : {self.y} | {self.bloque}'
+    
+    
+    
+
+    
+
+
+def encontrarCamino(stringMapa,posicionInicial, posicionFinal):
+
+    def mapaMatrix(mapa):
+        segmentos = [mapa[i:i+2] for i in range(0, len(mapa), 2)]
+        matriz = []
+        for i in range(7):
+            fila = segmentos[i*5:(i+1)*5]
+            matriz.append(fila)
+
+        return matriz
+
+    matriz = mapaMatrix(stringMapa)
+
+    #distOptima : int = maxsize
+    def matrizAdyacencia(matriz):
+
+        def convertirMatrizCasillas(matriz):
+            matCas = []
+            for i in range(len(matriz)):
+                fila = []
+                for j in range(len(matriz[0])):
+                    fila.append(Casilla(i,j,matriz[i][j]))
+                matCas.append(fila)
+
+            return matCas
+
+        def dentroTablero(coord):
+            if(coord[0] < len(matriz) and coord[0] >= 0 and coord[1] < len(matriz[0]) and coord[1] >= 0):
+                return True
+            return False
+        
+        matrizCasillas = convertirMatrizCasillas(matriz=matriz)
+
+        matrizAdy = []
+        for i in range(len(matriz)):
+            fila = []
+            for j in range(len(matriz[0])):
+                fila.append(matrizCasillas[i][j].bloque.value)
+            matrizAdy.append(fila)
+
+        return matrizAdy
+
+
+    matriz = matrizAdyacencia(matriz)
+
+    solOptima = []
+    direcOptima = []
+    sol = [posicionInicial]
+    direcciones = []
+
+    def caminoVueltraAtras(direccion,posicion):
+        global distOptima
+        
+        #print(sol)
+
+        def factible(direc,pos):
+
+            def movimientoAceptado(direccion : Direccion, bloque : int, bloqueSiguiente : int):
+
+                def moviAcept(direccion : Direccion, bloque : int):
+                    #print(direccion.name, bloque)
+                    if(direccion == Direccion.Arriba):
+
+                        #arriba = [2,4,5,8,9,10,11]
+                        arriba = [2,3,6,7,8,10,11]
+                        if(bloque in arriba):
+                            return True
+                        return False
+                    if(direccion == Direccion.Abajo):
+                        #abajo = [2,3,6,7,8,10,11]
+                        abajo = [2,4,5,8,9,10,11]
+                        if(bloque in abajo):
+                            return True
+                        return False
+                        #if(bloquePosterio.value == 2 or bloquePosterio.value == 3
+                        #   or bloquePosterio.value == 6 or bloquePosterio.value == 7
+                        #   or bloquePosterio.value == 10 or bloquePosterio.value == 11):
+                        #    return True
+                        #return False
+                    if(direccion == Direccion.Izquierda):
+                        #izquierda = [1,3,4,7,8,9,11]
+                        izquierda = [1,5,6,7,9,10,11]
+                        if(bloque in izquierda):
+                            return True
+                        return False
+                    if(direccion == Direccion.Derecha):
+                        #derecha = [1,6,7,9,10,11]
+                        derecha = [1,3,4,7,8,9,11]
+                        if(bloque in derecha):
+                            return True
+                        return False
+                    
+                direcSiguiente = Direccion((direccion.value + 2) % 4)
+                if(moviAcept(direccion,bloque) and moviAcept(direcSiguiente,bloqueSiguiente)):
+                    return True
+                return False
+            #pos = sol[-1]
+            
+            if(pos == posicionInicial):                
+                #print(f"Descartado Inicio {direccion} : {pos}")
+                return False
+                
+            if(pos in sol):
+                #print(f"Descartado Usada {direccion} : {pos}")
+                return False
+
+            if(pos[0] < 0 or pos[0] >= len(matriz) or pos[1] < 0 or pos[1] >= len(matriz[0])):
+                #print(f"Descartado Fuera de Rango {direccion} : {pos}")
+                return False
+            
+            if(matriz[pos[0]][pos[1]] == 0):
+                #print(f"Descartado Edificio {direccion} : {pos}")
+                return False           
+
+            if(not movimientoAceptado(direc,matriz[sol[-1][0]][sol[-1][1]],matriz[pos[0]][pos[1]])):
+                #print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}")
+                return False
+
+            #if(len(sol)+1 >= distOptima):
+            if(len(solOptima) > 0):
+                if(len(sol)+1 >= len(solOptima)):
+                    #print(f"Descartado Muy Largo {direccion} : {pos}")
+                    return False            
+
+            return True
+
+
+        if(factible(direccion,posicion)):          
+
+            
+            sol.append(posicion)
+            direcciones.append(direccion)
+            
+            pos = sol[-1]
+            #bq = mAdy[pos[0]][pos[1]]
+            #mAdy[pos[0]][pos[1]] = 0
+
+            if(pos != posicionFinal):
+                caminoVueltraAtras(Direccion.Arriba,(pos[0]-1,pos[1]))
+                caminoVueltraAtras(Direccion.Abajo,(pos[0]+1,pos[1]))
+                caminoVueltraAtras(Direccion.Derecha,(pos[0],pos[1]+1))
+                caminoVueltraAtras(Direccion.Izquierda,(pos[0],pos[1]-1))
+            else:
+                if(len(solOptima) > 0):
+                    if(len(sol) < len(solOptima)):
+                        #distOptima = len(sol)
+                        solOptima.clear()
+                        direcOptima.clear()
+                        for i in range(len(direcciones)):
+                            solOptima.append(sol[i])
+                            direcOptima.append(direcciones[i])                    
+                        solOptima.append(sol[-1])
+                else:
+                    #distOptima = len(sol)
+                    solOptima.clear()
+                    direcOptima.clear()
+                    for i in range(len(direcciones)):
+                        solOptima.append(sol[i])
+                        direcOptima.append(direcciones[i])                    
+                    solOptima.append(sol[-1])
+
+
+            #mAdy[pos[0]][pos[1]] = bq
+
+            sol.pop()
+            direcciones.pop()
+        
+
+    caminoVueltraAtras(Direccion.Arriba,(posicionInicial[0]-1,posicionInicial[1]))
+    caminoVueltraAtras(Direccion.Abajo,(posicionInicial[0]+1,posicionInicial[1]))
+    caminoVueltraAtras(Direccion.Derecha,(posicionInicial[0],posicionInicial[1]+1))
+    caminoVueltraAtras(Direccion.Abajo,(posicionInicial[0],posicionInicial[1]-1))
+
+    # Devuelve el recogido realizado tanto en posiciones como en direcciones
+    #return solOptima,direcOptima
+
+    return direcOptima
+
+
+    
+
+
+
+
+
+    
+
+
+#02 02 00 01 05 
+#03 07 05 00 02 
+#00 04 11 09 06 
+#01 10 03 10 00 
+#00 02 00 08 01 
+#01 10 01 10 00 
+#01 06 01 07 01
+
+matriz = [["02","02","00","01","05"],
+          ["03","07","05","00","02"],
+          ["00","04","11","09","06"],
+          ["01","10","03","10","00"],
+          ["00","02","00","08","01"],
+          ["01","10","01","10","00"],
+          ["01","06","01","07","01"]]
+
+mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
+#mAdy = matrizAdyacencia(matriz)
+
+#pos = mAdy[6][0]
+#mAdy[6][0] = 2
+
+sol = encontrarCamino(mapa,(5,2),(6,0))
+
+#mAdy[6][0] = pos
+
+for s in sol:
+    print(s) 
\ No newline at end of file

conexion.py

+import paho.mqtt.client as mqtt
+import json
+
+class Conexion:   
+    
+
+    def __init__(self, topics,on_msg):
+
+        
+        with open(file="conexionConfig.json",mode='r') as f:
+            data = json.load(f)
+            self.__broker_address = data['ipbroker']
+            self.__port = int(data['portbroker'])
+
+            # Configuración del broker MQTT
+            #self.__broker_address = "192.168.48.245"  # Cambia por la dirección de tu broker MQTT
+            #self.__broker_address = "192.168.0.100"  # Cambia por la dirección de tu broker MQTT
+            #self.__port = 1883  # Puerto predeterminado para MQTT
+        #self.__topic = ["A3-467/GrupoL","map"]
+        self.__topic = topics
+
+
+        #def on_message(client, userdata, message):
+        #    print("message received " ,str(message.payload.decode("utf-8")))
+        #    print("message topic=",message.topic)
+        #    print("message qos=",message.qos)
+        #    print("message retain flag=",message.retain)
+        #    #self.__listaPendientes.append(f'{message.topic}-{message.payload.decode("utf-8")}')
+        ########################################
+        #broker_address="192.168.0.19"
+        #broker_address="iot.eclipse.org"
+        print("creating new instance")
+        client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)        
+        #client.on_message = on_message
+        #client.on_message=on_message #attach function to callback
+        client.on_message = on_msg
+        print("connecting to broker")
+        client.connect(self.__broker_address,self.__port) #connect to broker
+        client.loop_start() #start the loop
+
+        if(len(self.__topic) == 0):
+            print(f" Subscrito a {self.__topic}")
+        else:
+            for tp in self.__topic:
+                client.subscribe(tp)
+                print(f" Subscrito a {tp}")
+
+        #self.__client = self.__connect_mqtt()
+        #self.inicializar()
+        #self.__client.loop_start()
+
+
+        #self.subscribe(cliente=client,topic=self.__topic)
+
+        self.__client = client
+        print("Inicializado")
+
+        #self.__listaPendientes = listaPendientes
+        #self.__client.loop_forever()
+
+        
+
+    def publicar(self,topic,msg):
+        
+        client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)        
+        client.connect(self.__broker_address, self.__port)
+        result = client.publish(topic,msg)
+        client.disconnect()
+        #result = self.__client.publish(self.__topic[0],msg)
+        # result: [0, 1]
+        status = result[0]
+        if status == 0:
+            print(f"Send `{msg}` to topic `{topic}`")
+        else:
+            print(f"Failed to send message to topic {topic}")   
+
+    def desconectar(self):
+        self.__client.loop_stop()
+        self.__client.disconnect()
+        print("Desconectado")
+
+    def desubscribir(self,topic):
+        self.__client.unsubscribe(topic)
+        print(f"Desubscrito topic : {topic}")
+
+
+# Función para enviar mensajes MQTT al broker
+def send_mqtt_message( topic,message):
+    client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)
+    client.connect("192.168.0.19" , 1883)
+    client.publish(topic, message)
+    client.disconnect()
+
+#    def publish(client):
+#        msg_count = 1
+#        while True:
+#            #time.sleep(1)
+#            msg = f"messages: {msg_count}"
+#            result = client.publish(topic, msg)
+#            # result: [0, 1]
+#            status = result[0]
+#            if status == 0:
+#                print(f"Send `{msg}` to topic `{topic}`")
+#            else:
+#                print(f"Failed to send message to topic {topic}")
+#            msg_count += 1
+#            if msg_count > 5:
+#                break
+
+
+    #def __subscribe(client: mqtt, topic):
+
+    #def run():
+    #    client = connect_mqtt()
+    #    client.loop_start()
+    #    publish(client)
+    #    client.loop_stop()
+
+    #def run():
+    #    client = connect_mqtt()
+    #    subscribe(client)
+    #    client.loop_forever()
+

conexionConfig.json

+{
+    "ipbroker" :"192.168.0.19",
+    "portbroker" : "1883",
+    "topicSubsRobot" : ["A3-467/GrupoL/Robot","A3-467/GrupoL/Sinc"],
+    "topicSubsInterfaz" : ["A3-467/GrupoL/Interfaz","map","A3-467/GrupoL/Sinc"],
+    "topicSendRobot" : "A3-467/GrupoL/Interfaz",
+    "topicSendInterfaz" : "A3-467/GrupoL/Robot",
+    "separadorMensaje" : ":",
+    "prefijoMensajes" : {   
+                            "mapa":"map",
+                            "pedido":"ped",
+                            "posicion":"pos"
+                        }
+}
\ No newline at end of file

main_robot.py

+import mensajeRobot as msg
+
+msgRobot = msg.MensageRobot()
+
+mapa = msgRobot.getMapa()
+print(f"Mapa recibido {mapa}")
+
+posicion= (0,6)
+msgRobot.sendPosicion(posicion)
+
+pedido = msgRobot.getPedido()
+print(f"Pedido recibido {pedido}")
\ No newline at end of file

mensajeRobot.py

+#from typing import Any
+import conexion as cn
+import time
+import json
+
+
+
+
+class MensageRobot:    
+
+
+    def __init__(self) -> None:
+        
+        def on_message(client, userdata, message):    
+            print(f'Recive Topic:{message.topic} Mensage:{str(message.payload.decode("utf-8"))}')
+            if(message.topic == self.topicSubs[-1]):
+                    if(str(message.payload.decode("utf-8")) != str(self.__class__)):
+                        self.sinc = True
+            else:
+                self.mensaje = str(message.payload.decode("utf-8"))
+
+        with open(file="conexionConfig.json",mode='r') as f:
+            data = json.load(f)
+
+            #self.topicSubs = ["A3-467/GrupoL/Robot"]
+            self.topicSubs = data['topicSubsRobot']
+            #self.topicSend = ["A3-467/GrupoL/Interfaz"]
+            self.topicSend = data['topicSendRobot']
+
+            #self.prefMsg = {"mapa":'map',
+            #                "pedido":'ped',
+            #                "posicion":'pos'}    
+            
+            self.prefMsg = data['prefijoMensajes']
+            self.sepMsg = data['separadorMensaje']
+        
+        self.conex = cn.Conexion(self.topicSubs,on_msg=on_message)
+        self.mensaje = ""
+        self.sinc = False
+        self.__sincronizacion()
+
+
+    def __sincronizacion(self):
+        while(not self.sinc):
+            self.conex.publicar(self.topicSubs[-1],str(self.__class__))
+            time.sleep(5)  
+        self.conex.desubscribir(self.topicSubs[-1])      
+        self.conex.publicar(self.topicSubs[-1],str(self.__class__))
+
+
+    def getMapa(self):
+
+        while(self.mensaje.split(self.sepMsg)[0] != self.prefMsg['mapa']):
+            time.sleep(1)
+
+        return self.mensaje.split(self.sepMsg)[1]
+    
+    def getPedido(self):
+        
+        self.conex.publicar(self.topicSend,self.prefMsg['pedido'] + self.sepMsg)
+
+        while(self.mensaje.split(self.sepMsg)[0] != self.prefMsg['pedido']):
+            print(f"Esperando Pedido - {self.mensaje.split(self.sepMsg)[0],self.prefMsg['pedido']}")
+            time.sleep(1)
+
+        return self.mensaje.split(self.sepMsg)[1]
+    
+    def sendPosicion(self,posicion):
+        self.conex.publicar(self.topicSend,self.prefMsg['posicion'] + self.sepMsg + str(posicion))
+        
+    
+
+    
+
+
+
+        
\ No newline at end of file