get pointclouds from client

wu_xinjun

2022-06-20 6b37ae2b15eca92b3cacac0e113290eb28ccfa8b

get pointclouds from client

 src/engine/AcquisitionMain.py

@@ -80,6 +80,8 @@
        self.CAMERA_COUNT = CAMERA_COUNT
        # microwave
        self.MICROWAVEhost = MICROWAVEhost
        # self.MICROWAVEPort = MICROWAVEhost
        self.MICROWAVEport = 9911

        # dust cleaner
        self.com = "COM11"
@@ -207,10 +209,16 @@
                    routing_key="server"
                    )

        self.mc = microwaveController()



        self.mc = microwaveController(
                    MicrowaveHost=self.MICROWAVEhost,
                    MicrowavePort=self.MICROWAVEport,
                    init_packet_threshold=100,
                    init_timeusage_threshold=3,
                    device_id=self.device_id,
                    logs_queue=self.LogsSender_queue,
                    plys_queue=self.PlysSender_queue,
                    routing_key="server"            
        )
        

    def run_rabbitmq(self):
@@ -236,15 +244,17 @@
            print(f"stop {t.name}")

    def start(self):
        try:
        # try:
        #     self.run_all()
        #     print("[*] Client is runing ...")
        #     self.CommandsReceiver_Thread.join()
        # except Exception as e:
        #     print(e)
        #     print("running error, stop all threads")
        #     self.stop_all()
        # print("main thread end.")
            self.run_all()
            print("[*] Client is runing ...")
            self.CommandsReceiver_Thread.join()
        except Exception as e:
            print(e)
            print("running error, stop all threads")
            self.stop_all()
        print("main thread end.")
        return 0
 
            

 src/engine/MessageThreads.py

@@ -9,6 +9,7 @@
from typing import List
import numpy as np
import cv2
from tomlkit import value

# # add python path of src to sys.path
# src_path = os.path.join(__file__, *(['..'] * 2))
@@ -288,7 +289,7 @@

            # extracting the msg from queue
            msg = queue.get()
            msg_pb = pb.PlyMessage()
            msg_pb = pb.PlysMessage()
            msg_pb.ParseFromString(msg)

            # save the raw pb msg 
@@ -309,7 +310,7 @@
                rows = ply.rows
                cols = ply.cols
                order = ply.order
                dt = ply.d_types
                dt = ply.dtype

                # save the pointcloud array data
                if (data_byte is None) | (len(data_byte) != data_legth):
@@ -317,7 +318,7 @@
                else:
                    # frame the bytes to array
                    data_array = np.frombuffer(data_byte,dtype=dt)
                    data_array = data_array.reshape(rows,cols)
                    data_array = data_array.reshape(rows,cols,order=order)
                    # save the data with .ply format
                    filename = f"{msg_source}_{msg_timestamp}_pointcloud{id}"
                    write_ply(data_array,self.plysmsg_folder,filename)
@@ -376,6 +377,54 @@
        # 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):

    # 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


    run =True
    while run:
        while not pointclouds.empty():
            
            value = pointclouds.get()

            if value is not 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:
                # 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
                run = False

    


    


 src/engine/Microwave.py

@@ -3,9 +3,10 @@
import time
from datetime import datetime
from typing import Union
import threading

from .MessageThreads import send_logs_tool
from ..microwave.servers import MicroWaveUDPThread
from ..microwave.servers import MicroWaveUDPThread,UDP2PointcloudThread


class micorwaveMannager(object):
@@ -13,43 +14,47 @@
    def __init__(self,
            MicrowaveHost:str,
            MicrowavePort:int,
            packet_threshold:Union[int, None],
            timeusage_threshold:Union[int, None]


                ):

        # microwave
        self.MicrowaveHost = MicrowaveHost
        self.MicrowavePort = MicrowavePort
        # udp get
        self.packet_threshold = packet_threshold
        self.timeusage_threshold = timeusage_threshold
        self._udp_get_thread = None
        # self.packet_threshold = packet_threshold
        # self.timeusage_threshold = timeusage_threshold
        self.udp_queue = queue.Queue()
        self.udp_get_stop = {"value":False}
        self.udp_get_stop = threading.Event()
        self._udp_get_thread_name = "Thread-UDPGet"
        # udp to pointcloud
        self._udp2pointcloud_thread = None
        self.pointcloud_queue = queue.Queue()
        self._udp2pointcloud_thread_name = "Thread-UDP2PointCloud"
        

    def _udp_get_thread_launcher(self):
    def _udp_get_thread_launcher(self,
                    packet_threshold:Union[int, None],
                    timeusage_threshold:Union[int, None]):
        self.udp_get_stop.clear()
        self._udp_get_thread = MicroWaveUDPThread(MicrowaveHost=self.MicrowaveHost,
                            MicrowavePort=self.MicrowavePort,
                            udp_queue=self.udp_queue,
                            udp_get_stop=self.udp_get_stop,
                            packet_threshold=self.packetThreshold,
                            timeusage_threshold=self.timeusage_threshold)
                            packet_threshold=packet_threshold,
                            timeusage_threshold=timeusage_threshold)
        self._udp_get_thread.setDaemon(True)
        self._udp_get_thread.setName(self._udp_get_thread_name)
        self._udp_get_thread.start()


    def _udp2pointcloud_thread_launcher(self):

        pass



    

        self._udp2pointcloud_thread = UDP2PointcloudThread(
                                        udp_queue = self.udp_queue,
                                        pointcloud_queue=self.pointcloud_queue
                                        )
        self._udp2pointcloud_thread.setDaemon(True)
        self._udp2pointcloud_thread.setName(self._udp2pointcloud_thread_name)
        self._udp2pointcloud_thread.start()


    

 src/engine/devices.py

@@ -4,7 +4,7 @@
import threading
import numpy as np
import os, sys
from typing import List
from typing import List, Union
import queue
from datetime import datetime

@@ -15,7 +15,8 @@

from ..protos import aerial_pb2 as pb
from .DustCleaner import DustCleaner
from .MessageThreads import send_logs_tool, send_images_tool
from .Microwave import micorwaveMannager
from .MessageThreads import send_logs_tool, send_images_tool, send_pointcloud_tool


######################################### CAMERAS ############################################
@@ -144,9 +145,36 @@


######################################## MICROWAVE ###########################################
class microwaveController(object):
    pass
class microwaveController(micorwaveMannager):
    def __init__(self, 
            MicrowaveHost: str, 
            MicrowavePort: int, 
            init_packet_threshold: Union[int, None], 
            init_timeusage_threshold: Union[int, None],
            device_id:str,
            logs_queue:queue.Queue,
            plys_queue:queue.Queue,
            routing_key:str,
            ):
        super().__init__(MicrowaveHost, MicrowavePort)
        self.device_id = device_id
        self.plys_queue = plys_queue
        self.routing_key = routing_key
        # testing the device when instance the class
        self._get_pointcloud(init_packet_threshold,init_timeusage_threshold)

    def _get_pointcloud(self, 
                    packet_threshold:Union[int, None],
                    timeusage_threshold:Union[int, None]):
        self._udp_get_thread_launcher(packet_threshold,timeusage_threshold)
        self._udp2pointcloud_thread_launcher()
        self._generate_msg()

    def _generate_msg(self):
        send_pointcloud_tool(pointclouds=self.pointcloud_queue,
                            plys_queue=self.plys_queue,
                            source=self.device_id,
                            routing_key=self.routing_key+".plys.test0")

####################################### DUST CLEANER #########################################
class dustCleanerController(DustCleaner):

 src/microwave/demo/AutoSendUDPtest.bat

@@ -4,8 +4,8 @@
cd %~dp0
echo change to path: %cd%

set aerialpython=%cd%/../../../env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
set aerialpython=%cd%/../../../.env/python.exe
set envsetting=%cd%/../../../env/SettingEnv.bat

@REM Setting the env
if not exist %aerialpython% (

 src/microwave/deviceTools.py

@@ -6,8 +6,9 @@
import open3d as o3d
from multiprocessing import  Pool
import numpy as np
import threading

from ..utils import file_fliter, write_ply
from ..utils.tools import file_fliter, write_ply
from .servers import MicroWaveReceiver,MicroWaveUDPThread
from .parsers import MicroWaveParser

@@ -18,7 +19,7 @@
    # host = '192.168.2.65' # use this host to get the udp data from microwave device
    port = 9911
    udp_queue = queue.Queue()
    udp_get_stop = {"value": False}
    udp_get_stop = threading.Event()

    if subFolder is not None:
        path = os.path.join(saveFolder,subFolder)
@@ -48,9 +49,9 @@

        while run:
            packed_count = 0
            while run or not udp_queue.empty():
            while not udp_queue.empty():
                value = udp_queue.get()
                while value is not False:
                if value is not False:
                    time_tag = datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
                    with ('{}/{}_packet{}.udp'.format(path,time_tag,packed_count), 'wb') as f:
                        f.write(udp_queue.get())
@@ -59,11 +60,11 @@
                    run = False

    except KeyboardInterrupt:
        udp_get_stop = {"value": True}
        udp_get_stop.set()
        print(time.asctime(),"Server Stopped")
    except:
        udp_get_stop = {"value": True}
        print(time.asctime(),"Server Stopped")
        udp_get_stop.set()
        print(time.asctime(),"Server Stopped with unknown reasons")

# udp2bin
def udp2bin_tool(udp_folder_Or_list):
@@ -76,7 +77,7 @@
    else:
        UDPfolder = udp_folder_Or_list
        # example pod1-2022-02-21-13-25-03_packet125.udp --> 125
        sort_lambda = lambda x:int(x.split('packet')[1][:-4])
        sort_lambda = lambda x:int(x.split('packet')[-1][:-4])
        udp_files = file_fliter(UDPfolder,'udp', sort_lambda)
        Save = True


 src/microwave/servers.py

@@ -1,13 +1,16 @@
import os, sys
from ctypes import sizeof, string_at
from socketserver import _AddressType, _RequestType, BaseRequestHandler, BaseServer, UDPServer
# from socketserver import _AddressType, _RequestType, BaseRequestHandler, BaseServer, UDPServer
from socketserver import BaseRequestHandler, BaseServer, UDPServer
import threading
from typing import Union
import warnings
import time
import queue
import numpy as np
from .components import *
from .constants import *
from .parsers import MicroWaveParser



@@ -100,6 +103,7 @@
        buf_size = len(buf)
        if buf_size == sizeof(Handshake):
            self._is_handshake(buf)
            return # there is only handshake(24 bytes) on it, end AddBuf func.

        if self.state == UDP_STATE_HAND_SHAKE:
            # after received handshake packet, we can get header block 
@@ -107,7 +111,7 @@
                # make sure to get a complete header
                result = self._get_header(buf)
            except Exception as e:
                warnings.warn(f"encounter an unknow error {e} when getting the header data in udp packets ...")
                warnings.warn(f"encounter an unknow error [{e}] when getting the header data in udp packets ...")
                result = False

            if not result:
@@ -138,6 +142,48 @@
            self.DataFrame.extend(bytes(buf))
            self._receive_footer(buf)

class UDP2PointcloudThread(threading.Thread):
    def __init__(self,
                udp_queue:queue.Queue,
                pointcloud_queue:queue.Queue,

                    
                    ):

        self.udp_queue = udp_queue
        self.pointcloud_queue = pointcloud_queue

        super().__init__()


    def run(self):
        receiver = MicroWaveReceiver()
        print(time.asctime(),"DP2Pointcloud Thread is starting to decode the udp packets... ")
        run = True

        while run:
            while not self.udp_queue.empty():
                udp = self.udp_queue.get()

                if udp is not False:
                    receiver.AddBuf(udp)
                    if receiver.DataFrameReady:
                        buf = receiver.DataFrame
                        buf_data = MicroWaveParser.ParserFrame(buf)

                        for frame in buf_data:
                            Frame_id = frame[0]
                            FrameNumber = frame[1]
                            point_cloud = np.array(frame[2])
                            if len(point_cloud) == 0 :
                                print(f"Notice: Frame {FrameNumber} is empty!")
                            else:
                                print(time.asctime(),f"Frame {FrameNumber} ready")
                                self.pointcloud_queue.put(point_cloud)
                else:
                    run = False
                    self.pointcloud_queue.put(False)
        print(time.asctime(),"DP2Pointcloud Thread finshed to decode the udp packets... ")


class MicroWaveRequestHandler(BaseRequestHandler):
@@ -145,23 +191,23 @@
    micro wave udp request handler
    """
    def __init__(self, 
        request: _RequestType, 
        client_address: _AddressType, 
        request, 
        client_address, 
        server: BaseServer) -> None:
        super().__init__(request, client_address, server)

    def handle(self):
        # Get message and client socket
        while not self.server.udp_get_stop["value"]:
        if not self.server.udp_get_stop.isSet():
            buf, _ = self.request
            packed_count = self.server.packed_count
            print(f"{time.asctime()} | Got connection from {self.client_address}, {packed_count}")
            self.server.udp_queue.put(buf)
            self.server.packed_count += 1
        else:
            UDPServer.server_close(self.server)
            self.server.udp_queue.put(False)
            print(f"{time.asctime()} | Server is stopped by thresholds or keyborad interrupt.")
        # else:
        #     UDPServer.server_close(self.server)
        #     self.server.udp_queue.put(False)
        #     print(f"{time.asctime()} | UDP-Get server is stopped by thresholds or keyborad interrupt.")



@@ -173,7 +219,7 @@
            server_address: tuple[str,int], 
            RequestHandlerClass: MicroWaveRequestHandler,
            udp_queue:queue.Queue,
            udp_get_stop:dict,
            udp_get_stop:threading.Event,
            packet_threshold:Union[int, None],
            timeusage_threshold:Union[int, None]
            ) -> None:
@@ -182,32 +228,38 @@
        self.RequestHandlerClass = MicroWaveRequestHandler
        self.udp_queue = udp_queue
        self.udp_get_stop = udp_get_stop
        self.udp_get_stop.clear()
        self.packet_threshold = packet_threshold
        self.timeusage_threshold = timeusage_threshold # mins
        self.packed_count = 0

        super().__init__(server_address, RequestHandlerClass)
        print(f"[*] | Listening on address {server_address}")

        # record start time of the server
        while self.timeusage_threshold is not None:
        if self.timeusage_threshold is not None:
            self.start_time = time.time()

    def close_request(self, request: _RequestType) -> None:
    def service_actions(self) -> None:

        # check to stop the server via muti-thresdhold
        while self.packet_threshold is not None and \
        if self.packet_threshold is not None and \
                self.packed_count >= self.packet_threshold:

            self.udp_get_stop["value"] = True
            self.udp_get_stop.set()

        while self.timeusage_threshold is not None:
        if self.timeusage_threshold is not None:
            cur_time = time.time() # seconds
            time_usage = int(cur_time - self.start_time) 

            if time_usage > 60*self.timeusage_threshold:
                self.udp_get_stop["value"] = True
                self.udp_get_stop.set()

        return super().close_request(request)
        if self.udp_get_stop.isSet():
            print(f"{time.asctime()} | UDP-Get server is stopped by thresholds or keyborad interrupt.")
            self.udp_queue.put(False)
            super().shutdown()




@@ -216,7 +268,7 @@
                MicrowaveHost:str,
                MicrowavePort:int,
                udp_queue:queue.Queue,
                udp_get_stop:dict,
                udp_get_stop:threading.Event,
                packet_threshold:Union[int, None],
                timeusage_threshold:Union[int, None]
                ):
@@ -231,9 +283,10 @@
    
    def stop(self):
        """stop the server to stop the """
        self.udp_get_stop["value"] = True
        self.udp_get_stop.set()

    def run(self):
        logs_list = []
        server = MicroWaveUDPServer(server_address=(self.MicrowaveHost,self.MicrowavePort),
                                    RequestHandlerClass=MicroWaveRequestHandler,
                                    udp_queue=self.udp_queue,
@@ -242,10 +295,14 @@
                                    timeusage_threshold=self.timeusage_threshold                                   
                                    )
        try:
            print(time.asctime(),"UDP Getting Server is starting to capture the udp packet... ")
            server.serve_forever()
        except:
            print(time.asctime(),"UDP Getting Server finshed to capture the udp packet ")
        except Exception as e:
            print(e)
            server.server_close()
            print(time.asctime(),"UDP Getting Server Stopped with Unknown Reason!")
            self.udp_get_stop.set()
            print(time.asctime(),"[ERROR] UDP Getting Server Stopped with Unknown Reason!")

        


 src/protos/aerial.proto

@@ -138,18 +138,8 @@
}


enum DataType
{
  c_int16 = 0;
  c_int32 = 1;
  c_int64 = 2;
  c_float16 = 3;
  c_float32 = 4;
  c_float = 5;
}


message PlyMessage
message PlysMessage
{
  string source = 1;
  string timestamp = 2;
@@ -160,7 +150,7 @@
    int32 rows = 2;
    int32 cols = 3;
    string order = 4;
    DataType d_types = 5;
    string dtype = 5;
    bytes data = 6;
    
  }