ubuntu连接多个realsense d435

Posted 2021-03-21 西北逍遥

ubuntu连接多个realsense d435

import pyrealsense2 as rs
import numpy as np
import cv2
import time
import datetime

import threading
from IPython import embed

#from worker import get_worker, get_ptx
from realsense_device_manager_old import DeviceManager
from depth_utils import get_depth, get_warner, save_img, warn_aloud
from usb_utils import get_usb_controller


CAM_ORDER = [\'837212070531\',\'818312070327\'] 

ori_w =1280
ori_h = 720
fps = 15

det_w = 512
det_h = 320 # x%64==0
det_ratio_w = det_w / float(ori_w)
det_ratio_h = det_h / float(ori_h)
det_ratio = (det_ratio_w, det_ratio_h)

show_h = 288
show_w = 512

        
def main(debug, noangle):
    if noangle:
        SETCONFIGS = {
                \'837212070531\': None,
                \'818312070327\':None,
                }
    else:
        SETCONFIGS = {
                \'837212070531\': (\'mid\', 45),
                \'818312070327\':{\'left\',45},
                }                

    ## test save data
    test_data = []
    average_data = []

    # Configure depth and color streams
    config = rs.config()
    config.enable_stream(rs.stream.depth, ori_w, ori_h, rs.format.z16, fps)
    config.enable_stream(rs.stream.color, ori_w, ori_h, rs.format.bgr8, fps) 

    # Use the device manager class to enable the devices and get the frames
    device_manager = DeviceManager(rs.context(), config)
    device_manager.enable_all_devices()
    frames_dict_q = []

    def get_frames_process():
        tmp = device_manager.wait_frames()
        frames_dict_q.append(tmp)

    

    frames_dict_q.append(device_manager.wait_frames())
    try:
        cnt = 0
        while True:
            if cnt==0:
                ts = time.time()


            ###### batch to get boxes
            imgs = []
            ks = []
            box_dict = {}
            frames_dict = frames_dict_q.pop(0)
            for k,v in frames_dict.items():
                depth_image, color_image = v
                img = cv2.resize(color_image, \\
                        (int(det_w), int(det_h)), \\
                        interpolation=cv2.INTER_AREA)
                imgs.append(img)
                ks.append(k)

            # start process for get frames
            t = threading.Thread(target=get_frames_process)
            t.start()

            
            ###### get depth after boxes
            imgs = {} 
            txts = []
            is_warns = 0
            for k,v in frames_dict.items():
                depth_image, color_image = v
                
                img = cv2.resize(color_image, (show_w,show_h), \\
                    interpolation=cv2.INTER_AREA)
                imgs[k] = img
                

            # Show images
            if len(imgs)==3:
                img1row = np.hstack((imgs[CAM_ORDER[0]], imgs[CAM_ORDER[2]]))
                img2row = np.hstack((imgs[CAM_ORDER[1]], np.zeros([show_h,show_w,3],dtype=np.uint8)))
                tot_img = np.concatenate([img1row, img2row])
            else:
                tot_img = np.concatenate([v for k,v in imgs.items()])
            cv2.namedWindow(\'RealSense\', cv2.WINDOW_AUTOSIZE)
            cv2.imshow(\'RealSense\', tot_img)
            cv2.waitKey(1)

            


    finally:
        # Stop streaming
        device_manager.disable_streams()
        cv2.destroyAllWindows()

if __name__==\'__main__\':
    import argparse
    parser = argparse.ArgumentParser()
    parser.add_argument(\'-d\',\'--debug\',action=\'store_true\', default=False)
    parser.add_argument(\'-a\',\'--noangle\',action=\'store_true\', default=False)
    args = parser.parse_args()
    main(args.debug, args.noangle)