3D_source_ex02.py

## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2017 Intel Corporation. All Rights Reserved.
 
#20230622
 
#####################################################
##              Align Depth to Color               ##
#####################################################
 
# First import the library
import os,sys
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
import tkinter.ttk as ttk
from tkinter import messagebox
from tkinter import filedialog
import time
 
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_address(self,address):
        self.host=address
        self.connect()
    def connect(self):
        print("re connect to host "+self.host)
        if self.soc !=None:
            print("close the previous host, "+self.soc.gethostname())
            try:
                self.soc.close()
                self.soc=None
                time.sleep(2.0)
            except:
                print("close failed")
        try:
            #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))
        except Exception as e:
            print("connect error.")
            if self.message_window:
                self.message_window.write_message("error when connect to "+self.host+":"+str(PORT))
                self.connected=False
        #受信担当の関数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))
                
    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:
            if self.host is not None:
                self.message_window.write_message("no socket when sends "+command + " to "+self.host+":"+str(PORT))
            else:
                self.message_window.write_message("no host assigned when sends "+command)
    #受信の処理。送信threadとは別に、並行して処理を行う。
    def handler(self):
        print("at client, connected, start handler")
        while self.soc is not None:
            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 Exception as e:
                if self.message_window:
                    self.message_window.write_message(self.host+":"+str(PORT)+" closed")
                if self.soc is not None:
                    self.soc.close()
                    self.soc=None
                tb=sys.exec_info()[2]
                print("message:{0}".format(e.with_traceback(tb)))
                break
    def close(self):
        self.soc.close()
        self.soc=None
 
class Coms:
    def __init__(self):
        self.coms=[None,None,None,None,None,None,None,None]
    def connect_all(self):
        self.coms[0]=Com("192.168.13.128")
        self.coms[1]=Com("192.168.13.129")
        self.coms[2]=Com("192.168.13.130")
        self.coms[3]=Com("192.168.13.131")
        self.coms[4]=Com("192.168.13.132")
        self.coms[5]=Com("192.168.13.133")
        self.coms[6]=Com("192.168.13.134")
        #self.coms[7]=Com("192.168.13.16")
        self.coms[7]=Com("192.168.13.135")
        #self.coms[7]=Com("192.168.1.22")
 
        for i in range(8):
            self.coms[i].connect()
    def connect_com_to_address(self,i,address):
        print("connect_com_to_address("+str(i)+","+address+")")
        if i<0 :
            print("wrong plane number")
            return
        if i>7 :
            print("wrong plane number")
        try:
            if self.coms[i] is not None:
                print("coms["+str(i)+"] is not None, connect, ")
                self.coms[i].connect_address(address)
            else:
                print("coms["+str(i)+"] is None, connect, ")
                self.coms[i]=Com(address)
                self.coms[i].connect()
        except Exception as e:
            print("coms["+str(i)+"] connect error")
            tb=sys.exec_info()[2]
            print("message:{0}".format(e.with_traceback(tb)))
 
    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+"\n")
    def send_command_to_all(self,command):
        for i in range(8):
            if self.coms[i]!=None:
                self.coms[i].send_command(command+"\n")
    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("850x1170")
        self.notebook =ttk.Notebook(root)
        self.main_tab=tk.Frame(self.notebook, bg='white')
        self.script_tab=tk.Frame(self.notebook, bg='white')
        self.notebook.add(self.main_tab, text="main", underline=0)
        self.notebook.add(self.script_tab, text="script")
        self.notebook.pack(expand=True, fill='both', padx=10, pady=10)
 
        yy=10
        self.connect_label=Label(self.main_tab,text="Servers",width=10)
        self.connect_label.place(x=30,y=yy)
        self.connect_button=Button(self.main_tab, text="connect",bd=2,bg='white',command=self.click_connect_button, width=8, relief=RIDGE)
        self.connect_button.place(x=100,y=yy)
        self.set_depth_label=Label(self.main_tab,text="set depth",width=10)
        self.set_depth_label.place(x=200,y=yy)
        self.set_depth_button=Button(self.main_tab, text="set depth",bd=2,bg='white',command=self.click_set_depth_button, width=8, relief=RIDGE)
        self.set_depth_button.place(x=270,y=yy)
 
        yy=40
        self.command_label=Label(self.main_tab,text="command", width=8)
        self.command_label.place(x=30,y=yy)
        self.command_field=Entry(self.main_tab,width=50)
        self.command_field.place(x=150,y=yy)
        self.enter_command_button=Button(self.main_tab,text="enter",bd=2,bg='white',command=self.enter_command,width=8,relief=RIDGE)
        self.enter_command_button.place(x=550,y=yy)
        self.run_script_button=Button(self.main_tab,text="run script",bd=2,bg='white',command=self.run_script,width=8,relief=RIDGE)
        self.run_script_button.place(x=650,y=yy)
 
        yy=70
        self.command_field_label_1=Label(self.main_tab,text="command for ith", width=15)
        self.command_field_label_1.place(x=30,y=yy)
        self.command_field_for_ith_server=Entry(self.main_tab,width=40)
        self.command_field_for_ith_server.place(x=150,y=yy)
        self.ith_server_field=Entry(self.main_tab,width=5)
        self.ith_server_field.place(x=450,y=yy)
        self.ith_enter_button=Button(self.main_tab,text="enter",bd=2,bg='white',command=self.enter_ith_command,width=8,relief=RIDGE)
        self.ith_enter_button.place(x=550,y=yy)
 
        yy=100
        self.command_field_label_2=Label(self.main_tab,text="command for all", width=15)
        self.command_field_label_2.place(x=30,y=yy)
        self.command_field_for_all_server=Entry(self.main_tab,width=50)
        self.command_field_for_all_server.place(x=150,y=yy)
        self.all_enter_button=Button(self.main_tab,text="enter",bd=2,bg='white',command=self.enter_all_command,width=8,relief=RIDGE)
        self.all_enter_button.place(x=550,y=yy)
        self.clear_message_button=Button(self.main_tab,text="clear_message",bd=2,bg='white',command=self.clear_message,width=8,relief=RIDGE)
        self.clear_message_button.place(x=650,y=yy)
 
        yy=130
        self.message_frame=tk.Frame(self.main_tab,width=50,height=30)
        self.message_frame.place(x=30,y=yy)
        self.message_area=scrolledtext.ScrolledText(self.message_frame)
        self.message_area.pack()
 
        yy=560
        self.realsense_label=Label(self.main_tab,text="Real Sense",width=12)
        self.realsense_label.place(x=30,y=yy)
        self.near_depth_field=Entry(self.main_tab,width=5)
        self.near_depth_field.place(x=200,y=yy)
        self.near_depth_field.insert(tk.END,"0.1")
        self.far_depth_field=Entry(self.main_tab,width=5)
        self.far_depth_field.place(x=260,y=yy)
        self.far_depth_field.insert(tk.END,"1.0")
        self.sending=BooleanVar()
        self.real_sense_sending_check=Checkbutton(self.main_tab,variable=self.sending,text="real sense sending")
        self.real_sense_sending_check.place(x=400,y=yy)
        self.sending.set(False)
        self.pausing=BooleanVar()
        self.real_sense_pausing_check=Checkbutton(self.main_tab, variable=self.pausing, text="real sense pausing")
        self.real_sense_pausing_check.place(x=500,y=yy)
        self.pausing.set(False)
 
        yy=590
        self.bg_removed_canvas=tk.Canvas(self.main_tab,width=320,height=240)
        #self.bg_removed_canvas.bind('<Button-1>',self.bg_removed_canvas_click)
        self.bg_removed_canvas.place(x=30,y=yy)
        self.depth_colormap_canvas=tk.Canvas(self.main_tab,width=320,height=240)
        #self.depth_colormap_canvas.bind('<Button-1>',self.bg_removed_canvas_click)
        self.depth_colormap_canvas.place(x=350,y=yy)
 
        yy=840
        self.pic_panel_0_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_0_canvas.place(x=30,y=yy)
        self.pic_panel_1_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_1_canvas.place(x=190,y=yy)
        self.pic_panel_2_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_2_canvas.place(x=350,y=yy)
        self.pic_panel_3_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_3_canvas.place(x=510,y=yy)
        yy=960
        self.pic_panel_4_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_4_canvas.place(x=30,y=yy)
        self.pic_panel_5_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_5_canvas.place(x=190,y=yy)
        self.pic_panel_6_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_6_canvas.place(x=350,y=yy)
        self.pic_panel_7_canvas=tk.Canvas(self.main_tab,width=160,height=120)
        self.pic_panel_7_canvas.place(x=510,y=yy)
 
        yy=10
        self.dir_name_label=Label(self.script_tab,text="dir name",width=10)
        self.dir_name_label.place(x=30,y=yy)
        self.dir_name_field=Entry(self.script_tab,width=50)
        self.dir_name_field.place(x=100,y=yy)
        #self.read_button=Button(self.script_tab, text="read",bd=2,bg='white',command=self.click_read_button, width=8, relief=RIDGE)
        #self.read_button.place(x=500,y=yy)
        self.dir_button=Button(self.script_tab, text="directory",bd=2,bg='white',command=self.click_dir_button, width=10, relief=RIDGE)
        self.dir_button.place(x=570,y=yy)
 
        yy=50
        self.file_name_label=Label(self.script_tab,text="file name",width=10)
        self.file_name_label.place(x=30,y=yy)
        self.file_name_field=Entry(self.script_tab,width=50)
        self.file_name_field.place(x=100,y=yy)
        self.read_button=Button(self.script_tab, text="read",bd=2,bg='white',command=self.click_read_button, width=8, relief=RIDGE)
        self.read_button.place(x=500,y=yy)
        self.dir_button=Button(self.script_tab, text="select",bd=2,bg='white',command=self.click_select_file_button, width=10, relief=RIDGE)
        self.dir_button.place(x=570,y=yy)
 
        yy=100
        self.script_frame=tk.Frame(self.script_tab,width=50,height=100)
        self.script_frame.place(x=50,y=yy)
        self.script_area=scrolledtext.ScrolledText(self.script_frame)
        self.script_area.pack()
        
        #self.run_script()
        self.set_script()
 
    def set_script(self):
        #self.script_area.insert(tk.END,"self.ex(\"connect 0 to \\\"192.168.1.21\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"connect 1 to \\\"192.168.1.19\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"connect 2 to \\\"192.168.1.20\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"connect 3 to \\\"192.168.1.22\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"connect 4 to \\\"192.168.1.12\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"connect 5 to \\\"192.168.1.14\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"connect 6 to \\\"192.168.1.15\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"connect 7 to \\\"192.168.1.7\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"#connect 7 to \\\"192.168.1.22\\\"\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 0\\\" to 0\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 30\\\" to 1\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 60\\\" to 2\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 90\\\" to 3\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 120\\\" to 4\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 150\\\" to 5\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 180\\\" to 6\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"set depth 210\\\" to 7\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"clear canvas\\\" to all\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"add ball 170 170 100 120 red\\\" to all\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"add ball 170 170 100 100 black\\\" to all\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"add ball 250 350 80 90 green\\\" to all\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"add ball 250 350 80 70 black\\\" to all\")\n")
        #self.script_area.insert(tk.END,"self.ex(\"send \\\"show canvas\\\" to all\")\n")
        self.script_area.insert(tk.END,"for i in range(20):\n")
        self.script_area.insert(tk.END,"\tself.ex(\"send \\\"clear canvas\\\" to all\")\n")
        self.script_area.insert(tk.END,"\tself.ex(\"send \\\"add ball 170 \"+str(70+20*i)+\" \"+str(70+10*i)+\" 120 red\\\" to all\")\n")
        self.script_area.insert(tk.END,"\tself.ex(\"send \\\"add ball 170 \"+str(70+20*i)+\" \"+str(70+10*i)+\" 100 black\\\" to all\")\n")
        self.script_area.insert(tk.END,"\tself.ex(\"send \\\"add ball 250 350 80 90 green\\\" to all\")\n")
        self.script_area.insert(tk.END,"\tself.ex(\"send \\\"add ball 250 350 80 70 black\\\" to all\")\n")
        self.script_area.insert(tk.END,"\tself.ex(\"send \\\"show canvas\\\" to all\")\n")
        self.script_area.insert(tk.END,"\ttime.sleep(0.5)\n")
 
    def click_read_button(self):
        fn=self.file_name_field.get()
        print("fn="+fn)
        f=None
        try:
            f=open(fn,"r")
        except Exception as e:
            print("file open error.")
            tb=sys.exec_info()[2]
            print("message:{0}".format(e.with_traceback(tb)))
            return
        try:
            self.script_area.delete(1.0,END)
            line=f.readline()
            while line:
                print(line)
                self.script_area.insert(END,line)
                line=f.readline()
            f.close()
        except Exception as e:
            print("file "+fn+" read error")
            tb=sys.exec_info()[2]
            print("message:{0}".format(e.with_traceback(tb)))
    def click_dir_button(self):
        iDir = os.path.abspath(os.path.dirname(__file__))
        folder_name=tk.filedialog.askdirectory(initialdir=iDir)
        if len(folder_name)==0:
            print("cancel selcting folder")
        else:
            self.dir_name_field.insert(END,folder_name)
    def click_select_file_button(self):
        fType = [("python", "*.py")]
        iDir=""
        xdir=self.dir_name_field.get()
        if xdir==None or xdir=="":
            iDir=os.path.abspath(os.path.dirname(__file__))
        else:
            iDir=xdir
        iFilePath=filedialog.askopenfilename(filetypes=fType, initialdir=iDir)
        if iFilePath==None or iFilePath=="":
            print("cancel")
        else:
            self.file_name_field.delete(0,END)
            self.file_name_field.insert(END,iFilePath)
 
    def enter_command(self):
        print("enter_command")
        self.ex()
        
    def run_script(self):
        self.script=self.script_area.get(0.,END)
        print("run_script:"+self.script)
        exec(self.script)
 
    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 ex(self,command):
        print("parse "+command)
        command=self.r_b(command)
        if self.parse_command(command):
            self.write_message("OK")
            print("OK")
        else:
            self.write_message("ERROR")
            print("ERROR")
 
    def write_message(self, message):
        self.message_area.insert(tk.END,message)
        self.message_area.insert(tk.END,'\n')
    def clear_message(self):
        self.message_area.delete("1.0",END)
 
    def click_connect_button(self):
        self.coms.connect_all()
    def click_set_depth_button(self):
        self.ex("send \"set depth 0 \" to 0")
        self.ex("send \"set depth 30 \" to 1")
        self.ex("send \"set depth 60 \" to 2")
        self.ex("send \"set depth 90 \" to 3")
        self.ex("send \"set depth 120 \" to 4")
        self.ex("send \"set depth 150 \" to 5")
        self.ex("send \"set depth 180 \" to 6")
        self.ex("send \"set depth 210 \" to 7")
 
    #
    # command
    #    send "<command>" to <i>
    #    send "<command>" to all
    #    connect <i> to "<host ip>"
    #
    def parse_command(self,command):
        rest=[""]
        strc=[""]
        ix=[""]
        command=self.r_b(command)
        print("parse_command("+command+")")
        if self.parse_key("send ",command,rest):
            print("parse_key(send "+command+","+rest[0]+")")
            command=self.r_b(rest[0])
            if self.parse_String_Constant(command,strc,rest):
                print("strc="+strc[0]+",rest="+rest[0])
                command=self.r_b(rest[0])
                if self.parse_key("to ",command,rest):
                    command=self.r_b(rest[0])
                    if self.parse_key("all",command,rest):
                        self.write_message("send \""+strc[0]+"\" to all")
                        self.coms.send_command_to_all(strc[0])
                        #self.command_field_for_all_server.delete(0,END)
                        return True
                    elif self.parse_p_int(command,ix,rest):
                        self.write_message("send \""+strc[0]+"\" to "+str(ix[0]))
                        self.coms.send_command_to_i(ix[0],strc[0])
                        return True
                    return False
                return False
            return False
        if self.parse_key("connect ",command,rest):
            print("parse_key(connect "+command+","+rest[0]+")")
            command=self.r_b(rest[0])
            if self.parse_p_int(command,ix,rest):
                print("parse_p_int("+command+","+str(ix[0])+","+rest[0]+")")
                command=self.r_b(rest[0])
                if self.parse_key("to ",command,rest):
                    print("parse_key(to "+command+","+rest[0]+")")
                    command=self.r_b(rest[0])
                    if self.parse_String_Constant(command,strc,rest):
                        self.write_message("connect "+str(ix[0])+" to "+strc[0])
                        print("connect "+str(ix[0])+" to "+strc[0])
                        self.coms.connect_com_to_address(ix[0],strc[0])
                        return True
                    return False
                return False
            return False
        return False
    def parse_key(self,key,inx,rest):
        keylen=len(key)
        inlen=len(inx)
        if inx.startswith(key):
            rest[0]=inx[keylen:inlen]
            return True
        rest[0]=inx
        return False
 
    def parse_p_int(self,command,ix,rest):
        print("parse_p_int("+command+",ix,rest)")
        i=0
        c=command[0]
        if c in ['0','1','2','3','4','5','6','7','8','9']:
            clen=len(command)
            while True:
                i=i*10+int(c)
                command=command[1:clen]
                clen=len(command)
                if clen==0:
                    ix[0]=i
                    rest[0]=""
                    return True
                print("parse_p_int command="+command)
                c=command[0]
                if not (c in ['0','1','2','3','4','5','6','7','8','9']):
                    ix[0]=i
                    rest[0]=command
                    return True
        rest[0]=command
        ix[0]=0
        return False
 
    def r_b(self,inx):
        rx=[""]
        while self.parse_key(" ",inx,rx):
            inx=rx[0]
        return rx[0]
 
    def parse_String_Constant(self,inx,strc,rest):
        rx=[""]
        xstr=""
        if self.parse_key('"',inx,rx):
            inx=rx[0]
            fx=inx[0]
            xlen=len(inx)
            while fx!='"':
                if xlen==0:
                    return False
                if fx=='\\':
                   inx=inx[1:xlen]
                else:
                   xstr=xstr+fx
                xlen=len(inx)
                inx=inx[1:xlen]
                fx=inx[0]
            if self.parse_key('"',inx,rx):
                strc[0]=xstr
                rest[0]=rx[0]
                return True
        return False
        
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 send_realsense_3d(self):
        #
        # 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=self.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)
                    #else:
                    #self.panel.write_message("not send "+sending+" to "+str(ii))                                
        self.coms.send_command_to_all("show plane")
 
    def get_depth_image(self):
        #print("get_depth_image")
        # 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
 
        self.depth_image = np.asanyarray(aligned_depth_frame.get_data())
        self.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((self.depth_image,self.depth_image,self.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, self.color_image)
        #print("bg_removed[100,100]=",end="")
        #print(self.bg_removed[100,100])
 
    def streaming(self):
        #print("streaming")
        self.depth_image=None
        self.color_image=None
 
        if not self.panel.pausing.get():
            self.get_depth_image()
        if self.depth_image is not None:
            #print("depth_image is not None")
            # Render images:
            #   depth align to color on left
            #   depth on right
            self.depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(self.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=self.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():
                self.send_realsense_3d()
            #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
        print("streaming_loop start")
        try:
            while True:
                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()

Counter: 159, today: 1, yesterday: 0

最新の20件