Untitled

#DYNAMIC SPANWER
import keyboard
import zmq
import time
import json

context = zmq.Context()
socket = context.socket(zmq.PUB)
socket.bind("tcp://*:5551")

my_dict = {}
my_dict["Data"] = {}
my_dict["Data"]["Lidar"] = {}
my_dict["Data"]["Camera"] = {}

my_dict["Data"]["Lidar"]["Publisher"] = 5560
my_dict["Data"]["Lidar"]["Subscriber"] = 5561
my_dict["Data"]["Camera"]["Publisher"] = 5562
my_dict["Data"]["Camera"]["Subscriber"] = 5563


json_out = json.dumps(my_dict)
super_header = str(len(json_out)).zfill(20)
message = str(super_header) + str(json_out)

time.sleep(1)
socket.send(message.encode())


#EDGENODE CONFIG
import keyboard
import zmq
import time
import json

context = zmq.Context()
socket = context.socket(zmq.PUB)
socket.bind("tcp://*:5561")

my_dict = {}
my_dict["Header"] = {}
my_dict["Data"] = {}
my_dict["Data"]["Lidar_Config"] = {}
my_dict["Data"]["Pose"] = {}
my_dict["Data"]["Pose"]["Position"] = {}
my_dict["Data"]["Pose"]["Orientation"] = {}
my_dict["Data"]["Pose"]["Local_Position"] = {}


my_dict["Data"]["Lidar_Config"]["Channels"] = 8
my_dict["Data"]["Lidar_Config"]["Horizontal_FOV_End"] = 359.0
my_dict["Data"]["Lidar_Config"]["Horizontal_FOV_Start"] = 0.0
my_dict["Data"]["Lidar_Config"]["Points_Per_Second"] = 56000
my_dict["Data"]["Lidar_Config"]["Range"] = 5000
my_dict["Data"]["Lidar_Config"]["Rotation_Frequency"] = 10.0
my_dict["Data"]["Lidar_Config"]["Show_Debug_Points"] = "True"
my_dict["Data"]["Lidar_Config"]["Vertical_FOV_Lower"] = -30.0
my_dict["Data"]["Lidar_Config"]["Vertical_FOV_Upper"] = 0.0
my_dict["Data"]["Lidar_Config"]["Lidar_Is_Enabled"] = "True"
my_dict["Data"]["Pose"]["Position"]["X"] = 910
my_dict["Data"]["Pose"]["Position"]["Y"] = -8690
my_dict["Data"]["Pose"]["Position"]["Z"] = 200
my_dict["Data"]["Pose"]["Local_Position"]["X"] = 0
my_dict["Data"]["Pose"]["Local_Position"]["Y"] = 0
my_dict["Data"]["Pose"]["Local_Position"]["Z"] = 100
# my_dict["Data"]["Pose"]["Orientation"]["x"] = 0.7
# my_dict["Data"]["Pose"]["Orientation"]["y"] = 0.7
# my_dict["Data"]["Pose"]["Orientation"]["z"] = 0
# my_dict["Data"]["Pose"]["Orientation"]["w"] = 0.1




json_out = json.dumps(my_dict)
super_header = str(len(json_out)).zfill(20)
message = str(super_header) + str(json_out)

time.sleep(1)
socket.send(message.encode())


#PCL VIS
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np
import zmq
import struct
import json

context = zmq.Context()
socket = context.socket(zmq.SUB)
socket.setsockopt(zmq.SUBSCRIBE, b'')
socket.setsockopt(zmq.CONFLATE, 1)

socket.connect("tcp://localhost:5560")

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

flag = True
count=0

while flag:
    for i in range(10):
        message = socket.recv()
        json_len = int(message[0:20])
        myjson = json.loads(message[20:20+json_len])
        points = np.frombuffer(message[20+json_len:], dtype=np.float32)
        if(len(points) > 0):
            count=count+1
            xs = points[0::3]
            ys = points[1::3]
            zs = points[2::3]
            ax.set_xlabel('X Label')
            ax.set_ylabel('Y Label')
            ax.set_zlabel('Z Label')
            ax.scatter(xs, ys, zs)
            plt.pause(0.00000000000001)
            plt.draw()
    plt.cla()
    ax.set_xlim3d(-10, 10)
    ax.set_ylim3d(-10, 10)
    ax.set_zlim3d(-10, 10)