3D_Display_Source_01.py

## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2017 Intel Corporation. All Rights Reserved.
 
#20230506
 
#####################################################
##              Align Depth to Color               ##
#####################################################
 
# First import the library
import pyrealsense2 as rs
# Import Numpy for easy array manipulation
import numpy as np
# Import OpenCV for easy image rendering
import cv2
import copy
 
import socket
import threading
import sys
from tkinter import *
import tkinter as tk
from tkinter import scrolledtext
from PIL import Image, ImageTk, ImageOps
 
PORT = 9998
 
class Com:
    def __init__(self,host):
        self.host=host
        self.connected=False
        self.soc=None
        self.handle_thread=None
        self.message_window=None
    def set_message_window(self,window):
        self.message_window=window
    def is_connected(sekf):
        return self.connected
    def connect(self):
        print("re connect to host "+self.host)
        try:
            if self.soc !=None:
                self.soc.connect((self.host,PORT))
            else:
                #socket.socket ... socket を作成し、socに代入
                self.soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                #IPアドレスがhostのserver socketにsocを接続
                self.soc.connect((self.host,PORT))
            if self.handle_thread==None:
                #受信担当の関数handlerを物threadでうごかすhandle_threadを生成。
                self.handle_thread=threading.Thread(target=self.handler)
                #handle_threadをstart
                self.handle_thread.start()
            self.connected=True
            if self.message_window:
                self.message_window.write_message("connected to "+self.host+":"+str(PORT))
        except:
            print("connect error.")
            if self.message_window:
                self.message_window.write_message("error when connect to "+self.host+":"+str(PORT))
            self.connected=False
                
    def send_command(self,command):
        if self.soc!=None:
            try:
                self.soc.send(bytes(command,"utf-8"))
            except KeyboardInterrupt:
                if self.message_window:
                    self.message_window.write_message("error when sends "+command + " to "+self.host+":"+str(PORT))
                self.soc.close()
                exit()
        else:
            self.message_window.write_message("no socket when sends "+command + " to "+self.host+":"+str(PORT))
    #受信の処理。送信threadとは別に、並行して処理を行う。
    def handler(self):
        print("at client, connected, start handler")
        while True:
            try:
                data = self.soc.recv(1024)
                line="[receive]- {}".format(data.decode("utf-8"))
                print(line)
                if self.message_window:
                    self.message_window.write_message("from "+self.host+":"+str(PORT)+", "+line)
                
            except socket.error:
                if self.message_window:
                    self.message_window.write_message(self.host+":"+str(PORT)+" closed")
                self.soc.close()
                self.soc=None
                break
        exit()
    def close(self):
        self.soc.close()
        self.soc=None
 
class Coms:
    def __init__(self):
        self.coms=[None,None,None,None,None,None,None,None]
        self.coms[0]=Com("192.168.13.21")
        self.coms[1]=Com("192.168.13.19")
        self.coms[2]=Com("192.168.13.20")
        self.coms[3]=Com("192.168.13.22")
        self.coms[4]=Com("192.168.13.12")
        self.coms[5]=Com("192.168.13.14")
        self.coms[6]=Com("192.168.13.15")
        #self.coms[7]=Com("192.168.13.16")
        self.coms[7]=Com("192.168.13.7")
        #self.coms[7]=Com("192.168.1.22")
    def connect_all(self):
        for i in range(8):
            try:
                if self.coms[i]!=None:
                    print("connect to coms["+str(i)+"].. ")
                    self.coms[i].connect()
                else:
                    print("coms["+str(i)+"] is None, connect, ")
            except:
                print("coms["+str(i)+"] connect error")
    def set_message_window(self,window):
        for i in range(8):
            if self.coms[i]!=None:
                self.coms[i].set_message_window(window)
            else:
                print("coms["+str(i)+"] is None, set_message_window, ")
    def send_command_to_i(self,i,command):
        if self.coms[i]!=None:
            self.coms[i].send_command(command)
    def send_command_to_all(self,command):
        for i in range(8):
            if self.coms[i]!=None:
                self.coms[i].send_command(command)
    def close_all(self):
        for i in range(8):
            if self.coms[i]!=None:
                self.coms[i].close()
            
class Command_Panel:
    def __init__(self,root,coms):
        self.coms=coms
        self.root=root
        self.root.title("command panel")
        self.root.configure(background="white")
        self.root.geometry("810x1300")
        self.command_field_for_ith_server=Entry(self.root,width=40)
        self.command_field_for_ith_server.place(x=80,y=50)
        self.ith_server_field=Entry(self.root,width=5)
        self.ith_server_field.place(x=400,y=50)
        self.ith_enter_button=Button(self.root,text="enter",bd=4,bg='white',command=self.enter_ith_command,width=8,relief=RIDGE)
        self.ith_enter_button.place(x=500,y=50)
        self.command_field_for_all_server=Entry(self.root,width=50)
        self.command_field_for_all_server.place(x=80,y=90)
        self.all_enter_button=Button(self.root,text="enter",bd=4,bg='white',command=self.enter_all_command,width=8,relief=RIDGE)
        self.all_enter_button.place(x=500,y=90)
        self.message_frame=tk.Frame(self.root,width=50,height=30)
        self.message_frame.place(x=80,y=120)
        self.message_area=scrolledtext.ScrolledText(self.message_frame)
        self.message_area.pack()
        self.connect_button=Button(self.root, text="connect",bd=4,bg='white',command=self.click_connect_button, width=8, relief=RIDGE)
        self.connect_button.place(x=80,y=10)
        self.near_depth_field=Entry(self.root,width=5)
        self.near_depth_field.place(x=200,y=10)
        self.near_depth_field.insert(tk.END,"0.1")
        self.far_depth_field=Entry(self.root,width=5)
        self.far_depth_field.place(x=300,y=10)
        self.far_depth_field.insert(tk.END,"1.0")
 
        self.sending=BooleanVar()
        self.real_sense_sending_check=Checkbutton(self.root,variable=self.sending,text="real sense sending")
        self.real_sense_sending_check.place(x=400,y=10)
        self.sending.set(False)
        
        self.bg_removed_canvas=tk.Canvas(self.root,width=320,height=240)
        #self.bg_removed_canvas.bind('<Button-1>',self.bg_removed_canvas_click)
        self.bg_removed_canvas.place(x=80,y=600)
        self.depth_colormap_canvas=tk.Canvas(self.root,width=320,height=240)
        #self.depth_colormap_canvas.bind('<Button-1>',self.bg_removed_canvas_click)
        self.depth_colormap_canvas.place(x=400,y=600)
 
        self.pic_panel_0_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_0_canvas.place(x=80,y=850)
        self.pic_panel_1_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_1_canvas.place(x=240,y=850)
        self.pic_panel_2_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_2_canvas.place(x=400,y=850)
        self.pic_panel_3_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_3_canvas.place(x=560,y=850)
        self.pic_panel_4_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_4_canvas.place(x=80,y=970)
        self.pic_panel_5_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_5_canvas.place(x=240,y=970)
        self.pic_panel_6_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_6_canvas.place(x=400,y=970)
        self.pic_panel_7_canvas=tk.Canvas(self.root,width=160,height=120)
        self.pic_panel_7_canvas.place(x=560,y=970)
 
    def enter_ith_command(self):
        command=self.command_field_for_ith_server.get()
        ith_s=self.ith_server_field.get()
        ith=int(ith_s)
        print("send "+command+" to "+str(ith))
        self.write_message("send "+command+" to "+str(ith))
        self.coms.send_command_to_i(ith,command)
        self.command_field_for_ith_server.delete(0,END)
 
    def enter_all_command(self):
        command=self.command_field_for_all_server.get()
        print("send "+command+" to all")
        self.write_message("send "+command+" to all")
        self.coms.send_command_to_all(command)
        self.command_field_for_all_server.delete(0,END)
 
    def write_message(self, message):
        self.message_area.insert(tk.END,message)
        self.message_area.insert(tk.END,'\n')
 
    def click_connect_button(self):
        self.coms.connect_all()
        
class Get_Cross_Sections:
    def __init__(self,coms,panel):
        # Create a pipeline
        self.pipeline = rs.pipeline()
        self.coms=coms
        self.panel=panel
 
        # Create a config and configure the pipeline to stream
        #  different resolutions of color and depth streams
        self.config = rs.config()
        self.is_quitting=False
 
        # Get device product line for setting a supporting resolution
        self.pipeline_wrapper = rs.pipeline_wrapper(self.pipeline)
        self.pipeline_profile = self.config.resolve(self.pipeline_wrapper)
        self.device = self.pipeline_profile.get_device()
        self.device_product_line = str(self.device.get_info(rs.camera_info.product_line))
 
        self.found_rgb = False
        for s in self.device.sensors:
            if s.get_info(rs.camera_info.name) == 'RGB Camera':
                self.found_rgb = True
                break
        self.oh=480
        self.ow=640
        if not self.found_rgb:
            print("The demo requires Depth camera with Color sensor")
            exit(0)
        self.config.enable_stream(rs.stream.depth, self.ow, self.oh, rs.format.z16, 30)
        if self.device_product_line == 'L500':
            self.config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
        else:
            self.config.enable_stream(rs.stream.color, self.ow, self.oh, rs.format.bgr8, 30)
 
        # Start streaming
        self.profile = self.pipeline.start(self.config)
        # Getting the depth sensor's depth scale (see rs-align example for explanation)
        self.depth_sensor = self.profile.get_device().first_depth_sensor()
        self.depth_scale = self.depth_sensor.get_depth_scale()
        print("Depth Scale is: " , self.depth_scale)
 
        # We will be removing the background of objects more than
        #  clipping_distance_in_meters meters away
        self.clipping_distance_in_meters = 1 #1 meter
 
        self.streaming_thread=threading.Thread(target=self.streaming_loop)
        self.streaming_thread.start()
 
    def streaming(self):
        # Create an align object
        # rs.align allows us to perform alignment of depth frames to others frames
        # The "align_to" is the stream type to which we plan to align depth frames.
        self.align_to = rs.stream.color
        self.align = rs.align(self.align_to)
        #print("depth range near(m):")
        #self.d_near=input()
        self.d_near=float(self.panel.near_depth_field.get())
        print("depth range far(m):")
        #self.d_far=input()
        self.d_far=float(self.panel.far_depth_field.get())
        self.clipping_near = float(self.d_near) / self.depth_scale
        print("depth range: ("+str(self.d_near)+" - "+str(self.d_far)+")")
        self.ping_distance_in_meters = float(self.d_far) #1 meter
        self.ping_distance = self.clipping_distance_in_meters / self.depth_scale
        self.clipping_distance = self.clipping_distance_in_meters / self.depth_scale
        # Get frameset of color and depth
        frames = self.pipeline.wait_for_frames()
        # frames.get_depth_frame() is a 640(ow)x360(oh) depth image
 
        # Align the depth frame to color frame
        aligned_frames = self.align.process(frames)
 
        # Get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image
        color_frame = aligned_frames.get_color_frame()
 
        # Validate that both frames are valid
        if not aligned_depth_frame or not color_frame:
            return   #continue
 
        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())
 
        # Remove background - Set pixels further than clipping_distance to grey
        grey_color = 153
        black_color = 0
        depth_image_3d = np.dstack((depth_image,depth_image,depth_image)) #depth image is 1 channel, color is 3 channels
        print("depth_image_3d[100,100]=",end="")
        print(depth_image_3d[100,100])
        self.bg_removed = np.where((depth_image_3d > self.clipping_distance) | (depth_image_3d <= self.clipping_near), black_color, color_image)
        print("bg_removed[100,100]=",end="")
        print(self.bg_removed[100,100])
 
        # Render images:
        #   depth align to color on left
        #   depth on right
        self.depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
        print("bg_removed[0,0]=",end="")
        print(self.bg_removed[0,0])
        print("bg_removed.shape=",end="")
        print(self.bg_removed.shape)
 
        d_min=0
        d_max=0
        self.d_interval=(self.clipping_distance - self.clipping_near)/8
        self.pic_plane=[[],[],[],[],[],[],[],[]]
        first=0
        xii=-1
        for ii in range(0,8):
            wa=copy.deepcopy(self.bg_removed)
            self.pic_plane[ii]=np.zeros_like(wa)
            #print("pic_plane["+str(ii)+"]=",end="")
            #print(pic_plane[ii].shape)
        for iw in range(0,self.ow):
            for ih in range(0,self.oh):
                dx=depth_image[ih,iw]
                if d_min>dx:
                    d_min=dx
                if d_max<dx:
                    d_max=dx
                dxx=dx-self.clipping_near
                pxx=dxx/self.d_interval
                ipxx=int(pxx)
                if 0<=ipxx and ipxx<=7:
                    self.pic_plane[ipxx][ih,iw]=self.bg_removed[ih,iw]
        if self.panel.sending.get():
            
            #
            # sin 3D display
            #    w: 32.5cm, 16
            #    h: 50.0cm, 75
            #    z: 16.0cm, 8
            #
            # for i in each ii, for each ih,
            #    send "set4 ix(in ih) iy(in iw) r0,g0,b0 ... r3,g3,b3"
            #    to i's display's p
            #
            self.coms.send_command_to_all("clear plane")
            for ii in range(8):
                for ih in range(75):
                    #
                    # pic_plane's h:480(oh), 3d-display's h':75
                    #   h=(480/75)*h'
                    #
                    h=(480/75)*(74-ih)
                    for iy in range(4):
                        #
                        # pic_plane's w:640->320, 3d-display's w w':16
                        #   offset 160
                        #   w=160+(320/16)*w'
                        #  
                        #
                        # send "setpic4 ix iy r0 g0 b0 ... r3 g3 b3"
                        #
                        sending="setpic4 "+str(ih)+" "+str(iy*4)+" "
                        send_flag=False
                        for ipxx in range(4):
                            w=160+(320/16)*(iy*4+ipxx)
                            dx=depth_image[int(h),int(w)]
                            dxx=dx-self.clipping_near
                            pxx=dxx/self.d_interval
                            if int(pxx)==ii:
                                send_flag=True
                                print("h="+str(h)+",w="+str(w)+",pxx="+str(pxx))
                                print("color=",end="")
                                print(self.pic_plane[int(pxx)][int(h),int(w)])
                                print("color2=",end="")
                                print(self.bg_removed[int(h),int(w)])
                                ccxx=self.bg_removed[int(h),int(w)]
                                ccx=[0,0,0]
                                cw=ccxx[0]
                                ccx[0]=ccxx[2]
                                ccx[1]=ccxx[1]
                                ccx[2]=cw
                            else:
                                ccx=[0,0,0]
                            for cx in range(3):
                                sending=sending+str(ccx[cx])+" "
                        if send_flag:
                            self.panel.write_message("send "+sending+" to "+str(ii))
                            self.coms.send_command_to_i(ii,sending+'\n')
                        #else:
                            #self.panel.write_message("not send "+sending+" to "+str(ii))                                
            self.coms.send_command_to_all("show plane")
        print("d_min=",end="")
        print(d_min)
        print("d_max=",end="")
        print(d_max)
 
        if xii!=-1:
            print("pic_plane["+str(xii)+"]["+str(xih)+","+str(xiw)+"]=",end="")
            print(self.pic_plane[xii][xih,xiw])
            print("bg_removed["+str(xih)+","+str(xiw)+"]=",end="")
            print(self.bg_removed[xih,xiw])
        else:
            print("xii==-1")
        self.display()
        key = cv2.waitKey(1)
        # Press esc or 'q' to close the image window
        if key & 0xFF == ord('q') or key == 27:
            cv2.destroyAllWindows()
            self.is_quitting=True
 
    def streaming_loop(self):
        # Streaming loop
        try:
            while True:
                print("start streaming.")
                self.streaming()
                if self.is_quitting:
                    return
        finally:
            print("streaming_thread,finally")
            self.pipeline.stop()
    def display_panel_to_canvas(self,panel,canvas):
        '''画像をCanvasに表示する'''
 
        # フレーム画像の取得
        #ret, frame = self.capture.read()
    
        # BGR→RGB変換
        px = cv2.cvtColor(panel, cv2.COLOR_BGR2RGB)
        # NumPyのndarrayからPillowのImageへ変換
        pil_image = Image.fromarray(px)
 
        # キャンバスのサイズを取得
        canvas0_width = canvas.winfo_width()
        canvas0_height = canvas.winfo_height()
 
        # 画像のアスペクト比（縦横比）を崩さずに指定したサイズ（キャンバスのサイズ）全体に画像をリサイズする
        pil_image = ImageOps.pad(pil_image, (canvas0_width, canvas0_height))
 
        # PIL.ImageからPhotoImageへ変換する
        photo_image = ImageTk.PhotoImage(image=pil_image)
        self.imgs.append(photo_image)
 
        # 画像の描画
        canvas.create_image(
                canvas0_width / 2,       # 画像表示位置(Canvasの中心)
                canvas0_height / 2,                   
                image=photo_image  # 表示画像データ
                )
 
 
    def display(self):
        self.imgs=[]
        self.display_panel_to_canvas(self.bg_removed,self.panel.bg_removed_canvas)
        self.display_panel_to_canvas(self.depth_colormap,self.panel.depth_colormap_canvas)
        self.display_panel_to_canvas(self.pic_plane[0],self.panel.pic_panel_0_canvas)
        self.display_panel_to_canvas(self.pic_plane[1],self.panel.pic_panel_1_canvas)
        self.display_panel_to_canvas(self.pic_plane[2],self.panel.pic_panel_2_canvas)
        self.display_panel_to_canvas(self.pic_plane[3],self.panel.pic_panel_3_canvas)
        self.display_panel_to_canvas(self.pic_plane[4],self.panel.pic_panel_4_canvas)
        self.display_panel_to_canvas(self.pic_plane[5],self.panel.pic_panel_5_canvas)
        self.display_panel_to_canvas(self.pic_plane[6],self.panel.pic_panel_6_canvas)
        self.display_panel_to_canvas(self.pic_plane[7],self.panel.pic_panel_7_canvas)
        # disp_image()を10msec後に実行する
        #self.disp_id = self.panel.root.after(10, self.display)
 
 
if __name__=="__main__":
    root=Tk()
    coms=Coms()
    p=Command_Panel(root,coms)
    coms.set_message_window(p)
    cs=Get_Cross_Sections(coms,p)
    root.mainloop()

最新の20件