add sender

message_sender.py

+# -*- coding: utf-8 -*-
+"""
+Created on Fri Mar  4 14:28:51 2022
+
+@author: s3340992
+"""
+
+from abc import ABC
+from shapely.geometry import Point, LineString
+from shapely.geometry.polygon import Polygon
+import numpy as np
+from datetime import datetime, timedelta
+import datetime
+import time
+import json
+from vehicle import vehicle
+from node import node
+import paho.mqtt.client as mqtt
+import cv2
+import tkinter
+
+# MQTT setup
+the_hostname = "AGV_simulator"
+MQTT_SERVER = "192.168.43.195"
+#MQTT_SERVER = "broker.hivemq.com"
+MQTT_PATH = "VDA5050_AgvState"
+MQTT_PORT = 1883
+
+# vis_parameters
+height = 900
+width = 1000
+color = ["red", "green", "blue", "cyan", "yellow", "magenta"]
+vehicle_radius = 20
+
+
+
+
+def play_videoFile(filePath,mirror=False):
+    cap = cv2.VideoCapture(filePath)
+    cv2.namedWindow('Video Life2Coding',cv2.WINDOW_AUTOSIZE)
+    frameTime = 200
+    while True:
+        ret_val, frame = cap.read()
+        if mirror:
+            frame = cv2.flip(frame, 1)
+        cv2.imshow('Video Life2Coding', frame)
+        if cv2.waitKey(frameTime) == 27:
+            break  # esc to quit
+    cv2.destroyAllWindows()
+    
+def draw_veh(canvas, pos_x, pos_y, tag, color):
+    canvas.create_oval(pos_x - vehicle_radius,
+                       height - pos_y - vehicle_radius,
+                       pos_x + vehicle_radius,
+                       height - pos_y + vehicle_radius,
+                       fill=color,
+                       tag=tag)
+
+def abs_move(self, tag, new_x, new_y):
+    # Get the current object position
+    x, y, *_ = self.bbox(self.find_withtag(tag)[0])
+    for obj in self.find_withtag(tag):
+        # Move the object
+        self.move(obj, (new_x-x), (new_y-y))
+        
+    
+# Monkey patch the `abs_move` method
+tkinter.Canvas.abs_move = abs_move	
+	
+def visualization(canvas, tag, pos, color):
+    height = 900
+    canvas.abs_move(tag, pos[0]- vehicle_radius,
+                       height - pos[1]- vehicle_radius)
+    canvas.update()
+
+def send_msg(client, msg):
+    print(msg)
+    client.publish(MQTT_PATH, msg)
+
+def create_message(header_id, veh_num, time_delta, x , y, theta, time_now, last_node_id, battery_state, error_list, load_list):
+        #time_msg = time_null + timedelta(seconds=time_delta)
+        time_msg = time_now
+        timestamp = str(time_msg.date())
+        timestamp += "T"
+        timestamp += str(time_msg.time())
+        timestamp += "+00:00"
+        
+        battery_state_msg = {
+            "batteryCharge" : str(battery_state)
+            }
+        error_msg = {
+            "errorType" : error_list[0],
+            "errorLevel" : error_list[1]
+            }
+        position_msg = {
+            "x" : x,
+            "y" : y,
+            "theta" : theta
+            }
+        load_msg = {
+            "loadId" : load_list[0]
+            }
+        json_msg = {
+            "headerId" : header_id,
+            "timeStamp" : timestamp,
+            "manufacturer" : "TL_TUD",
+            "serialNumber" : veh_num,
+            "lastNodeId" : last_node_id,
+            "batteryState": battery_state_msg,
+            "errors" : [error_msg],
+            "agvPosition" : position_msg,
+            "loads" : [load_msg]
+            }
+        msg = json.dumps(json_msg)
+        print (msg)
+        return msg
+
+        
+def message_filter(list_pos, list_nodes, time_lastupdate):
+    pos = [list_pos[2], list_pos[3]]
+    if pos in list_nodes:
+        return False
+    elif ((list_pos[1] - time_lastupdate) > 30):
+        return False
+    else:
+        return True
+
+# The callback for when the client receives a CONNACK response from the server.
+def on_connect(client, userdata, flags, rc):
+    if rc == 0:
+        print("Connected successfully")
+    else:
+        print("Connect returned result code: " + str(rc))
+
+# The callback for when a PUBLISH message is received from the server.
+def on_message(client, userdata, msg):
+    print("Received message: " + msg.topic + " -> " + msg.payload.decode("utf-8"))
+
+#--------------------------------------------------------------------
+
+with open('simulation_runs/raw_output/log.json') as json_file:
+    raw_data_7 = json.load(json_file)
+# create the client
+client = mqtt.Client()
+client.on_connect = on_connect
+client.on_message = on_message
+
+# enable TLS
+#client.tls_set(tls_version=mqtt.ssl.PROTOCOL_TLS)
+#client.username_pw_set("phone", "IPOSframework123")
+client.connect(MQTT_SERVER, MQTT_PORT)
+#client.loop_start()
+#client.subscribe(MQTT_PATH)
+
+
+veh_1 = vehicle("1", "TUD", 0, 0, 100, 0, "0")
+veh_2 = vehicle("2", "TUD", 0, 0, 100, 0, "0")
+veh_3 = vehicle("3", "TUD", 0, 0, 100, 0, "0")
+veh_4 = vehicle("4", "TUD", 0, 0, 100, 0, "0")
+veh_5 = vehicle("5", "TUD", 0, 0, 100, 0, "0")
+
+# create node list
+node_list = []
+# create node and appending them to the list 
+node_list.append(node("1", 18, 10))
+node_list.append(node("2", 6, 3.12))
+node_list.append(node("3", 14, 3.12))
+node_list.append(node("4", 14, 16.92))
+node_list.append(node("5", 6, 16.92))
+node_list.append(node("6", 2, 10))
+
+# play video
+#play_videoFile('Beispielvideo.mp4',False)
+
+#cap = cv2.VideoCapture('Beispielvideo.mp4')
+#cv2.namedWindow('Video Life2Coding',cv2.WINDOW_AUTOSIZE)
+#mirror = False
+
+# create canvas
+root = tkinter.Tk()
+root.title("simVis")
+root.resizable(False, False)
+canvas = tkinter.Canvas(root, width=width, height=height)
+canvas.pack()
+canvas_factor=50
+
+for i in range(5):
+    draw_veh(canvas, 0, 0, 'veh'+str(i+1), color[i])
+
+actual_time = 0
+time_interval = 0.02
+timeBegin = time.time()
+time_null = time.time()
+last_time_frame = 0
+for loadpoint in node_list:
+    canvas.create_oval(loadpoint.x*canvas_factor - 5, height - loadpoint.y*canvas_factor - 5, loadpoint.x*canvas_factor + 5, height - loadpoint.y*canvas_factor + 5, fill='dark orange')
+for i in range(len(raw_data_7)):
+#    if (float(raw_data_7[i][1]) > last_time_frame):
+#        ret_val, frame = cap.read()
+#        if mirror:
+#            frame = cv2.flip(frame, 1)
+#        cv2.imshow('Video Life2Coding', frame)
+#    last_time_frame = raw_data_7[i][1]
+    
+    while (float(raw_data_7[i][1]) > actual_time):  
+        timeEnd = time.time()
+        timeElapsed = timeEnd - timeBegin
+        actual_time += time_interval+timeElapsed//time_interval
+        time.sleep(time_interval-timeElapsed % time_interval)
+        timeBegin = time.time()
+        continue
+    else:
+        #time_start = time.time()
+        if (raw_data_7[i][0] == 1):
+            veh_1.update_pos(float(raw_data_7[i][2]), float(raw_data_7[i][3]), raw_data_7[i][1], node_list)
+            visualization(canvas, "veh1", [veh_1.pos_x*canvas_factor, veh_1.pos_y*canvas_factor], color[raw_data_7[i][0]-1])
+            #print("veh_1 updated!")
+            if (veh_1.send_msg):
+                msg = create_message("1", "1", float(raw_data_7[i][1]), veh_1.pos_x, veh_1.pos_y, veh_1.theta, datetime.datetime.now(), veh_1.last_node_id, veh_1.battery_state, ["0", "WARNING"], ["1"])
+                client.publish(MQTT_PATH, msg)
+                veh_1.last_msg_time = actual_time
+                veh_1.send_msg = False
+        elif (raw_data_7[i][0] == 2):
+            veh_2.update_pos(float(raw_data_7[i][2]), float(raw_data_7[i][3]), float(raw_data_7[i][1]), node_list)
+            visualization(canvas, "veh2", [veh_2.pos_x*canvas_factor, veh_2.pos_y*canvas_factor], color[raw_data_7[i][0]-1])
+            if (veh_2.send_msg):
+                msg = create_message("2", "2", float(raw_data_7[i][1]), veh_2.pos_x, veh_2.pos_y, veh_2.theta, datetime.datetime.now(), veh_2.last_node_id, veh_2.battery_state, ["0", "WARNING"], ["2"])
+                client.publish(MQTT_PATH, msg)
+                veh_2.last_msg_time = actual_time
+                veh_2.send_msg = False
+        elif (raw_data_7[i][0] == 3):
+            veh_3.update_pos(float(raw_data_7[i][2]), float(raw_data_7[i][3]), float(raw_data_7[i][1]), node_list)
+            visualization(canvas, "veh3", [veh_3.pos_x*canvas_factor, veh_3.pos_y*canvas_factor], color[raw_data_7[i][0]-1])
+            if (veh_3.send_msg):
+                msg = create_message("3", "3", float(raw_data_7[i][1]), veh_3.pos_x, veh_3.pos_y, veh_3.theta, datetime.datetime.now(), veh_3.last_node_id, veh_3.battery_state, ["0", "WARNING"], ["3"])
+                client.publish(MQTT_PATH, msg)
+                veh_3.last_msg_time = actual_time
+                veh_3.send_msg = False
+        elif (raw_data_7[i][0] == 4):
+            veh_4.update_pos(float(raw_data_7[i][2]), float(raw_data_7[i][3]), float(raw_data_7[i][1]), node_list)
+            visualization(canvas, "veh4", [veh_4.pos_x*canvas_factor, veh_4.pos_y*canvas_factor], color[raw_data_7[i][0]-1])
+            if (veh_4.send_msg):
+                msg = create_message("4", "4", float(raw_data_7[i][1]), veh_4.pos_x, veh_4.pos_y, veh_4.theta, datetime.datetime.now(), veh_4.last_node_id, veh_4.battery_state, ["0", "WARNING"], ["4"])
+                client.publish(MQTT_PATH, msg)
+                veh_4.last_msg_time = actual_time
+                veh_4.send_msg = False
+        elif (raw_data_7[i][0] == 5):
+            veh_5.update_pos(float(raw_data_7[i][2]), float(raw_data_7[i][3]), float(raw_data_7[i][1]), node_list)
+            visualization(canvas, "veh5", [veh_5.pos_x*canvas_factor, veh_5.pos_y*canvas_factor], color[raw_data_7[i][0]-1])
+            if (veh_5.send_msg):
+                msg = create_message("5", "5", float(raw_data_7[i][1]), veh_5.pos_x, veh_5.pos_y, veh_5.theta, datetime.datetime.now(), veh_5.last_node_id, veh_5.battery_state, ["0", "WARNING"], ["5"])
+                client.publish(MQTT_PATH, msg)
+                veh_5.last_msg_time = actual_time
+                veh_5.send_msg = False
+        #time_elapsed = time.time()-time_start
+        #print(time_elapsed)
+        
+        
+
+