from socketserver import BaseRequestHandler, UDPServer
|
import sys # NOQA: E402
|
sys.path.append('../') # NOQA: E402
|
import time
|
import sched
|
import pika
|
from protos import aerial_pb2 as pb
|
import uuid
|
import cv2
|
import logging
|
import numpy as np
|
import requests
|
import threading # NOQA: E402
|
import MicroWaveParser
|
import queue
|
|
IMAGE_WIDTH = 3840
|
IMAGE_HEIGHT = 2880
|
CAMERA_COUNT = 4
|
|
micro_wave_parser = MicroWaveParser.MicroWaveParser()
|
udp_queue = queue.Queue()
|
|
class MicroWaveUDPServer(BaseRequestHandler):
|
def handle(self):
|
# Get message and client socket
|
global micro_wave_parser
|
msg, sock = self.request
|
print("received a udp packet, len={}".format(len(msg)))
|
micro_wave_parser.AddBuf(msg)
|
if micro_wave_parser.PointCloudReady:
|
udp_queue.put(micro_wave_parser.PointCloud)
|
micro_wave_parser.PointCloudReady = False
|
|
|
class AerialClient:
|
def __init__(self, host: str, user: str, password: str, request_queue: str, device_id: str, api_url: str, interval) -> None:
|
self.host: str = host
|
self.user: str = user
|
self.password: str = password
|
self.request_queue: str = request_queue
|
self.device_id: str = device_id
|
self.message_send: int = 0
|
self.api_url: str = api_url
|
self.cameras: list = []
|
self._def_callback_routes()
|
self.next_time = 0
|
self.interval = interval
|
|
def _set_logging(self):
|
logging.basicConfig(level=logging.INFO,
|
format='%(asctime)s %(levelname)s %(message)s',
|
datefmt='%Y-%m-%d %H:%M:%S')
|
self.logger = logging.getLogger(__name__)
|
|
def _send_data(self, req: pb.RequestCommand) -> None:
|
req.device_id = self.device_id
|
body = req.SerializeToString()
|
self.channel.basic_publish(
|
exchange='',
|
routing_key=self.request_queue,
|
body=body)
|
self.message_send += 1
|
return
|
|
def _call(self, req: pb.RequestCommand, timeout: int = 10) -> pb.ResponseCommand:
|
req.device_id = self.device_id
|
self.response = None
|
self.corr_id = str(uuid.uuid4()) # 生成一个服务端返回消息的队列名
|
|
body = req.SerializeToString()
|
response = requests.post(self.api_url, data=body, headers={
|
'Content-Type': 'application/x-protobuf'})
|
resp = pb.ResponseCommand()
|
resp.ParseFromString(response.content)
|
return resp
|
|
def _connect(self) -> None:
|
credentials = pika.PlainCredentials(self.user, self.password)
|
parameters = pika.ConnectionParameters(
|
credentials=credentials, host=self.host)
|
|
# 连接队列服务器
|
self.connection = pika.BlockingConnection(parameters)
|
self.channel = self.connection.channel()
|
|
result = self.channel.queue_declare(queue='', exclusive=True)
|
self.callback_queue = result.method.queue
|
self.channel.basic_consume(
|
queue=self.callback_queue,
|
on_message_callback=self._on_response,
|
auto_ack=True)
|
|
def _on_response(self, ch, method, props, body) -> None:
|
if self.corr_id == props.correlation_id:
|
self.response = body
|
|
def _send_point_cloud_cmd(self, point_cloud):
|
req = pb.RequestCommand()
|
req.sys_command = pb.SysCommand.MICRO_WAVE_COMMAND
|
req.sub_command = pb.SubCommand.MICRO_WAVE_SUB_COMMAND
|
data1 = pb.MicroWaveMessage()
|
data1.length = len(point_cloud)
|
data1.data = bytes(point_cloud)
|
data1_str = data1.SerializeToString()
|
req.data1.length = len(data1_str)
|
req.data1.data = data1_str
|
self._send_data(req)
|
|
def _send_image_cmd(self):
|
data1 = pb.ImagesMessage()
|
for i in range(0, len(self.cameras)):
|
ret, frame = self.cameras[i].read()
|
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 95]
|
result, encimg = cv2.imencode('.jpg', frame, encode_param)
|
if frame is not None:
|
np_array = np.asarray(encimg) # frame 转换为numpy数组
|
byte_array = np_array.tobytes() # numpy数组转换为byte数组
|
if i == 0:
|
data1.image1 = byte_array
|
elif i == 1:
|
data1.image2 = byte_array
|
elif i == 2:
|
data1.image3 = byte_array
|
elif i == 3:
|
data1.image4 = byte_array
|
elif i == 4:
|
data1.image5 = byte_array
|
data1_str = data1.SerializeToString()
|
|
req = pb.RequestCommand()
|
req.sys_command = pb.SysCommand.FIVE_G_COMMAND
|
req.sub_command = pb.SubCommand.FIVE_G_SEND_IMAGE_SUB_COMMAND
|
req.data1.length = len(data1_str)
|
# print(f'data length={req.data1.length}')
|
req.data1.data = data1_str
|
self._send_data(req)
|
|
def _send_power_on_message(self) -> pb.ResponseCommand:
|
print("send power on message")
|
req = pb.RequestCommand()
|
req.sys_command = pb.SysCommand.SYS_COMMAND
|
req.sub_command = pb.SubCommand.POWER_ON_SUB_COMMAND
|
return self._call(req)
|
|
def _send_power_off_message(self) -> pb.ResponseCommand:
|
print("send power off message")
|
req = pb.RequestCommand()
|
req.sys_command = pb.SysCommand.SYS_COMMAND
|
req.sub_command = pb.SubCommand.POWER_OFF_SUB_COMMAND
|
return self._call(req)
|
|
def _capture_loop(self) -> None:
|
while True:
|
this_time = time.time()
|
if this_time > self.next_time:
|
self._send_image_cmd()
|
print(f"send image message: {self.send_image_count}")
|
self.send_image_count += 1
|
self.next_time += self.interval
|
|
if not udp_queue.empty():
|
point_cloud = udp_queue.get()
|
self._send_point_cloud_cmd(point_cloud)
|
if cv2.waitKey(1) & 0xFF == ord('q'):
|
break
|
for camera in self.cameras:
|
camera.release()
|
|
def _close(self) -> None:
|
self.connection.close()
|
|
def _open_cameras(self):
|
"""
|
Test the ports and returns a tuple with the available ports and the ones that are working.
|
"""
|
self.cameras = []
|
self.working_ports = []
|
for dev_port in range(CAMERA_COUNT):
|
camera = cv2.VideoCapture(dev_port, cv2.CAP_DSHOW)
|
camera.set(cv2.CAP_PROP_FRAME_WIDTH, IMAGE_WIDTH)
|
camera.set(cv2.CAP_PROP_FRAME_HEIGHT, IMAGE_HEIGHT)
|
camera.set(cv2.CAP_PROP_FPS, 1)
|
if camera.isOpened():
|
is_reading, img = camera.read()
|
w = camera.get(3)
|
h = camera.get(4)
|
if is_reading:
|
print("Port %s is working and reads images (%s x %s)" %
|
(dev_port, h, w))
|
self.working_ports.append(dev_port)
|
self.cameras.append(camera)
|
|
def _on_sys_command(self, req: pb.RequestCommand):
|
print('syscommand received')
|
return
|
|
def _default(self, req: pb.RequestCommand):
|
print('default handler')
|
return
|
|
def _def_callback_routes(self):
|
self.routes = {
|
pb.SysCommand.SYS_COMMAND: self._on_sys_command,
|
}
|
|
def _on_callback_request(self, ch, method, properties, body):
|
print('callback received')
|
# req = pb.RequestCommand()
|
# req.ParseFromString(body)
|
# dispatcher = self.routes.get(req.sys_command, self._default)
|
# resp = pb.ResponseCommand()
|
# dispatcher(req)
|
ch.basic_ack(delivery_tag=method.delivery_tag)
|
return
|
|
def _init_callback(self, host: str, user: str, password: str, queue: str):
|
credentials = pika.PlainCredentials(user, password)
|
parameters = pika.ConnectionParameters(
|
credentials=credentials, host=host)
|
connection = pika.BlockingConnection(parameters)
|
self.callback_channel = connection.channel()
|
self.callback_channel.queue_declare(queue=queue, durable=False)
|
self.callback_channel.basic_qos(prefetch_count=1)
|
self.callback_channel.basic_consume(
|
on_message_callback=self._on_callback_request, queue=queue)
|
|
def callback_consuming(self):
|
self.callback_channel.start_consuming()
|
|
def _start_callback(self):
|
t = threading.Thread(target=self.callback_consuming)
|
t.start()
|
|
def start_capture_thread(self):
|
self.next_time = time.time() + self.interval
|
threading.Thread(target=self._capture_loop).start()
|
|
def _udp_loop(self):
|
host = ''
|
port = 9911
|
self.udpserver = UDPServer((host, port), MicroWaveUDPServer)
|
print(time.asctime(),'UDP server started on port', port)
|
self.udpserver.serve_forever()
|
|
def start_udp_thread(self):
|
threading.Thread(target=self._udp_loop).start()
|
|
def start(self) -> None:
|
self._open_cameras()
|
if len(self.cameras) == 0:
|
print("No camera available")
|
return
|
self._init_callback(self.host, self.user, self.password, self.device_id + "_callback")
|
self._start_callback()
|
self._connect()
|
resp = self._send_power_on_message()
|
if resp.state == pb.ResponseState.SUCCESS:
|
print("received power on response SUCCESS, start send images")
|
self.send_image_count = 0
|
self.start_capture_thread()
|
self.start_udp_thread()
|
else:
|
print("power on failed")
|
