![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
#author("2023-05-23T16:44:31+09:00","default:TeleportDresser","TeleportDresser") #author("2023-05-23T16:50:09+09:00","default:TeleportDresser","TeleportDresser") [[3D_source_ex02.py]] #code(python){{ ## 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 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)) 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 #受信担当の関数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)) 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: 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) 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 self.soc is not None: 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 Exception as e: except socket.error: 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))) 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] def connect_all(self): 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): 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: 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))) 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+"\n") 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+"\n") 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("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.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() 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.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.main_tab,width=5) self.far_depth_field.place(x=260,y=yy) 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.main_tab,variable=self.sending,text="real sense sending") self.real_sense_sending_check.place(x=400,y=yy) 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.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=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=30,y=yy) self.depth_colormap_canvas=tk.Canvas(self.main_tab,width=320,height=240) 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=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) 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 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") 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):") 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()) 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((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 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 print("streaming_loop start") 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() }}
