Select Git revision
message_sender.py 8.95 KiB
# -*- coding: utf-8 -*-
"""
Created on Fri Mar 4 14:28:51 2022
@author: s3340992
"""
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 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 = ["green", "blue", "gray", "violet", "yellow", "rot"]
vehicle_radius = 20
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)