import os
|
import queue
|
import threading
|
import pika
|
import time
|
from datetime import datetime
|
from typing import List
|
import numpy as np
|
import cv2
|
import requests
|
|
# # add python path of src to sys.path
|
# src_path = os.path.join(__file__, *(['..'] * 2))
|
# src_path = os.path.abspath(src_path)
|
# sys.path.insert(0, src_path)
|
|
from ..protos import aerial_pb2 as pb
|
from ..utils.tools import write_ply
|
|
|
|
# class Message(object):
|
# def __init__(self,sender_id,sender_key,body,routing_key):
|
# self.sender_id = sender_id
|
# self.sender_key = sender_key
|
# self.body = body
|
# self.routing_key = routing_key
|
|
class senderThread(threading.Thread):
|
|
def __init__(self,
|
MQparams: pika.ConnectionParameters,
|
exchange_name: str,
|
exchange_type:str,
|
# routing_key:str,
|
message_queue:queue.Queue,
|
):
|
super().__init__()
|
|
self.MQparams = MQparams
|
self.exchange_name = exchange_name
|
self.exchange_type = exchange_type
|
# self.routing_key = routing_key
|
self.message_queue = message_queue
|
|
# connect to RabbitMQ
|
self.is_stop = False
|
self.connection = pika.BlockingConnection(self.MQparams)
|
|
def stop(self):
|
"""stop the thread"""
|
self.is_stop = True
|
# self.connection.close()
|
|
def run(self):
|
|
while not self.is_stop:
|
# declare a exchange
|
channel = self.connection.channel()
|
channel.exchange_declare(exchange=self.exchange_name,
|
exchange_type=self.exchange_type)
|
|
while True:
|
try:
|
if not self.message_queue.empty() and not self.is_stop:
|
|
# decode the routing key and message body from tuple list
|
message = self.message_queue.get()
|
msg_routing_key = message[0]
|
msg_body = message[1]
|
if msg_body is not False:
|
# publish a PERSISTENT message
|
channel.basic_publish(
|
exchange = self.exchange_name,
|
routing_key = msg_routing_key,
|
body = msg_body,
|
properties=pika.BasicProperties(
|
delivery_mode=pika.spec.PERSISTENT_DELIVERY_MODE
|
)
|
)
|
|
timestamp = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
caption = f"[x] {timestamp} | Sending message to [{msg_routing_key}]"
|
print(caption)
|
|
else:
|
time.sleep(1)
|
|
except Exception as e:
|
print(e)
|
break
|
|
class receiverThread(threading.Thread):
|
def __init__(self,
|
MQparams: pika.ConnectionParameters,
|
exchange_name: str,
|
exchange_type:str,
|
MQqueue_name:str,
|
binding_keys:List[str],
|
message_queue:queue.Queue,
|
):
|
super().__init__()
|
|
self.MQparams = MQparams
|
self.exchange_name = exchange_name
|
self.exchange_type = exchange_type
|
self.MQqueue_name = MQqueue_name
|
self.binding_keys = binding_keys
|
self.message_queue = message_queue
|
|
# connect to RabbitMQ
|
self.is_stop = False
|
self.connection = pika.BlockingConnection(self.MQparams)
|
|
def stop(self):
|
"""stop the thread"""
|
self.is_stop = True
|
# self.connection.close()
|
|
|
def run(self):
|
|
while not self.is_stop:
|
# declare a exchange
|
channel = self.connection.channel()
|
channel.exchange_declare(exchange=self.exchange_name,
|
exchange_type=self.exchange_type)
|
|
# declare the queue and bind it with keys
|
channel.queue_declare(queue=self.MQqueue_name, durable=True)
|
for binding_key in self.binding_keys:
|
channel.queue_bind(exchange=self.exchange_name, queue=self.MQqueue_name, routing_key=binding_key)
|
print(f"[*] Using [{binding_key}] bind [{self.MQqueue_name}] on [{self.exchange_name}]")
|
|
# consume the message
|
channel.basic_qos(prefetch_count=1)
|
channel.basic_consume(
|
queue=self.MQqueue_name,
|
on_message_callback=self.callback
|
)
|
channel.start_consuming()
|
|
def callback(self,ch, method, properties, body):
|
|
if self.is_stop:
|
ch.close()
|
if body is not False:
|
self.message_queue.put(body)
|
ch.basic_ack(delivery_tag = method.delivery_tag)
|
|
class decodeThread(threading.Thread):
|
def __init__(self,
|
queue_list:List[queue.Queue],
|
queue_tags:List[str],
|
task_folder:dict[str,str],
|
replay_host:str):
|
super().__init__()
|
|
self.queue_list = queue_list
|
self.queue_tags = queue_tags
|
self.queue_num = 0
|
self.task_folder = task_folder
|
self.replay_host = replay_host
|
self.is_stop = False
|
|
assert len(self.queue_list) == len(self.queue_tags)
|
|
def stop(self):
|
"""stop the thread"""
|
self.is_stop = True
|
|
def run(self) -> None:
|
|
# # creating a path for saving data
|
# time_tag = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
# self.save_root_folder = os.path.join(self.task_folder,time_tag)
|
# os.makedirs(self.save_root_folder)
|
|
# self.logsmsg_folder = os.path.join(self.save_root_folder,"LogsMsg")
|
# self.imagesmsg_folder = os.path.join(self.save_root_folder,"ImagesMsg")
|
# self.plysmsg_folder = os.path.join(self.save_root_folder,"PlysMsg")
|
# self.commandsmsg_folder = os.path.join(self.save_root_folder,"CommandsMsg")
|
|
# sub_folder_list = [self.logsmsg_folder,self.imagesmsg_folder,
|
# self.plysmsg_folder,self.commandsmsg_folder]
|
|
# for folder in sub_folder_list:
|
# os.mkdir(folder)
|
# print(f"[NOTICE] Creating folder: {folder}")
|
|
# process the queue list
|
self.queue_num = len(self.queue_list)
|
while self.queue_num:
|
i = 0
|
while True:
|
# select a queue
|
i_tag = i % self.queue_num
|
queue = self.queue_list[i_tag]
|
|
# make sure the queue is not empty
|
while not queue.empty() and not self.is_stop:
|
# decode the msg using cooresponding method
|
queue_tag = self.queue_tags[i_tag]
|
decode_method = self.decode_select(queue_tag)
|
decode_method(queue)
|
# next queue
|
i += 1
|
|
def decode_select(self,queue_tags:str):
|
route_dic = {
|
"Logs":self.decode_LogsMsg,
|
"Images":self.decode_ImagesMsg,
|
"Plys":self.decode_PlysMsg,
|
"Commands":self.decode_CommandsMsg
|
}
|
return route_dic[queue_tags]
|
|
def decode_LogsMsg(self,queue):
|
nums_limit = 10
|
count = 0
|
while not queue.empty() and count < nums_limit:
|
|
# extracting the msg from queue
|
msg = queue.get()
|
msg_pb = pb.LogsMessage()
|
msg_pb.ParseFromString(msg)
|
|
# process the msg
|
msg_source = msg_pb.source
|
msg_timestamp = msg_pb.timestamp
|
msg_context = msg_pb.context
|
taskuuid = msg_pb.taskuuid
|
|
save_folder = self.task_folder[taskuuid]
|
|
# save the raw pb msg
|
# curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
raw_file_name = os.path.join(save_folder,f"{msg_timestamp}-{msg_source}.LogsMsg")
|
with open(raw_file_name, 'wb') as file:
|
file.write(msg)
|
|
# display the info to the console
|
curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
notice_str = f"[x] {curr_time}|(From {msg_source}): {msg_context}"
|
print(notice_str)
|
replay_to_(host = self.replay_host,
|
message=notice_str,
|
message_type="logs",
|
taskuuid=taskuuid)
|
|
# next msg
|
count += 1
|
|
def decode_ImagesMsg(self,queue):
|
|
while not queue.empty():
|
|
# extracting the msg from queue
|
msg = queue.get()
|
msg_pb = pb.ImagesMessage()
|
msg_pb.ParseFromString(msg)
|
|
# process the msg
|
msg_source = msg_pb.source
|
msg_timestamp = msg_pb.timestamp
|
taskuuid = msg_pb.taskuuid
|
save_folder = self.task_folder[taskuuid]
|
|
# save the raw pb msg
|
# curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
raw_file_name = os.path.join(save_folder,f"{msg_timestamp}-{msg_source}.ImagesMsg")
|
with open(raw_file_name, 'wb') as file:
|
file.write(msg)
|
|
|
# process the images
|
id = 0
|
for image in msg_pb.Image:
|
imagelegth = image.length
|
image_byte = image.data
|
|
# save the images
|
if (image_byte is None) | (len(image_byte) != imagelegth ):
|
pass
|
else:
|
filename = f"{msg_timestamp}-{msg_source}-image{id}.jpg"
|
filepath = os.path.join(save_folder,filename)
|
with open(filepath, 'wb') as f:
|
f.write(image_byte)
|
|
# display the info to the console
|
curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
notice_str = f"[x] {curr_time}|(From {msg_source}): image{id} saved!"
|
print(notice_str)
|
replay_to_(host = self.replay_host,
|
message=notice_str,
|
message_type="images",
|
taskuuid=taskuuid)
|
|
# next image item
|
id += 1
|
|
def decode_PlysMsg(self,queue):
|
|
while not queue.empty():
|
|
# extracting the msg from queue
|
msg = queue.get()
|
msg_pb = pb.PlysMessage()
|
msg_pb.ParseFromString(msg)
|
|
# process the msg
|
msg_source = msg_pb.source
|
msg_timestamp = msg_pb.timestamp
|
taskuuid = msg_pb.taskuuid
|
|
save_folder = self.task_folder[taskuuid]
|
|
# save the raw pb msg
|
# curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
raw_file_name = os.path.join(save_folder,f"{msg_timestamp}-{msg_source}.PlysMsg")
|
with open(raw_file_name, 'wb') as file:
|
file.write(msg)
|
|
# process the muti ply data
|
id = 0
|
for ply in msg_pb.Plydata:
|
data_legth = ply.length
|
data_byte = ply.data
|
rows = ply.rows
|
cols = ply.cols
|
order = ply.order
|
dt = ply.dtype
|
|
# save the pointcloud array data
|
if (data_byte is None) | (len(data_byte) != data_legth):
|
pass
|
else:
|
# frame the bytes to array
|
data_array = np.frombuffer(data_byte,dtype=dt)
|
data_array = data_array.reshape(rows,cols,order=order)
|
# save the data with .ply format
|
filename = f"{msg_timestamp}-{msg_source}-pointcloud{id}"
|
write_ply(data_array,save_folder,filename)
|
|
# display the info to the console
|
curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
notice_str = f"[x] {curr_time}|(From {msg_source}): ply{id} saved!"
|
print(notice_str)
|
replay_to_(host = self.replay_host,
|
message=notice_str,
|
message_type="plys",
|
taskuuid=taskuuid)
|
|
# next ply item
|
id += 1
|
|
def decode_CommandsMsg(self,queue):
|
|
|
raise NotImplementedError("NOT IMPLEMENT!")
|
|
|
def replay_to_(host:str,
|
message:str,
|
message_type:str,
|
taskuuid:str,
|
):
|
|
if taskuuid != "init":
|
try:
|
# target = host+taskuuid
|
target = host
|
print(f"Send message to [{target}]")
|
payload = {"taskid":taskuuid, "messagetype":message_type, "message": message}
|
r = requests.post(target,params = payload,timeout=3)
|
print("send success!")
|
except Exception as e:
|
print(e)
|
print("send error!")
|
pass
|
|
|
def send_logs_tool(logs:List[str],
|
logs_queue:queue.Queue,
|
source:str,
|
routing_key:str,
|
taskuuid:str = "init"):
|
|
for i, log in enumerate(logs):
|
if log is not None:
|
# constract a logs message
|
logsMsg = pb.LogsMessage()
|
logsMsg.source = source
|
curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
logsMsg.timestamp = curr_time
|
logsMsg.context = log
|
logsMsg.taskuuid = taskuuid
|
# convert the proto object to string
|
logsMsg_bytes = logsMsg.SerializeToString()
|
# add logsmsg to queue
|
logs_queue.put([routing_key,logsMsg_bytes])
|
|
|
def send_images_tool(images:List[np.array],
|
images_queue:queue.Queue,
|
source:str,
|
routing_key:str,
|
taskuuid:str = "init"):
|
|
# constract a images message
|
imagesMsg = pb.ImagesMessage()
|
imagesMsg.source = source
|
curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
imagesMsg.timestamp = curr_time
|
imagesMsg.taskuuid = taskuuid
|
|
for img in images:
|
if img is not None:
|
success,image_encoded = cv2.imencode(".jpg",img)
|
if success:
|
image_bytes = image_encoded.tobytes()
|
image_length = len(image_bytes)
|
# add the encoded image to msg
|
image_item = imagesMsg.Image.add()
|
image_item.length = image_length
|
image_item.data = image_bytes
|
|
# converting the pb image msg to bytes
|
imageMsg_bytes = imagesMsg.SerializeToString()
|
# add the imageMsg to the queue
|
images_queue.put([routing_key,imageMsg_bytes])
|
|
def send_pointcloud_tool(pointclouds:queue.Queue,
|
plys_queue:queue.Queue,
|
source:str,
|
routing_key:str,
|
taskuuid:str = "init"):
|
|
# constract a plys message
|
plysMsg = pb.PlysMessage()
|
plysMsg.source = source
|
curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
|
plysMsg.timestamp = curr_time
|
plysMsg.taskuuid = taskuuid
|
|
empty = True
|
|
|
run =True
|
while run:
|
while not pointclouds.empty():
|
|
value = pointclouds.get()
|
|
if value is not False:
|
empty = False
|
pointcloud = np.array(value)
|
# get the attributes of the PointCloudItem
|
rows = pointcloud.shape[0]
|
cols = pointcloud.shape[1]
|
order = "C"
|
dtype = str(pointcloud.dtype)
|
data = pointcloud.tobytes(order=order)
|
length = len(data)
|
|
# add the pointcloud to the msg
|
item = plysMsg.Plydata.add()
|
item.length = length
|
item.rows = rows
|
item.cols = cols
|
item.order = order
|
item.dtype = dtype
|
item.data = data
|
|
else:
|
|
if not empty:
|
# converting the pb ply msg to bytes
|
plysMsg_bytes = plysMsg.SerializeToString()
|
# add the plysMsg to the queue
|
plys_queue.put([routing_key,plysMsg_bytes])
|
# end the outer while loop and triminate this function
|
|
else:
|
print("[x] NO pointcloud has been captured!")
|
run = False
|
else:
|
time.sleep(3)
|
|
|
|
|
|
