3D_source_ex03.py
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
単語検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
[[TeleportDresser]]
#code(Python){{
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2017 Intel Corporation. All Rights Reserved.
#20230623
#####################################################
## 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,mw):
self.host=host
self.connected=False
self.soc=None
self.handle_thread=None
self.message_window=mw
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.exc_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,message_window):
self.message_window=message_window
self.coms[0]=Com("192.168.13.128",message_window)
self.coms[1]=Com("192.168.13.129",message_window)
self.coms[2]=Com("192.168.13.130",message_window)
self.coms[3]=Com("192.168.13.131",message_window)
self.coms[4]=Com("192.168.13.132",message_window)
self.coms[5]=Com("192.168.13.133",message_window)
self.coms[6]=Com("192.168.13.134",message_window)
#self.coms[7]=Com("192.168.13.16")
self.coms[7]=Com("192.168.13.135",message_window)
#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.exc_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("850x1220")
self.notebook =ttk.Notebook(root)
self.main_tab=tk.Frame(self.notebook, bg='white')
self.script_tab=tk.Frame(self.notebook, bg='white')
self.autorun_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.add(self.autorun_tab, text="autorun")
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=580
self.dpt_var_1=tk.IntVar()
self.dpt_var_1.set(10)
self.dpt_slider_1 = Scale(self.main_tab, variable=self.dpt_var_1, orient='horizontal', length=300, from_=0, to=200)
self.dpt_slider_1.place(x=60,y=yy)
self.dpt_slider_1_l=Label(self.main_tab, textvariable=str(self.dpt_var_1), relief="flat")
self.dpt_slider_1_l.place(x=370,y=yy)
yy=620
self.dpt_var_2=tk.IntVar()
self.dpt_var_2.set(100)
self.dpt_slider_2 = Scale(self.main_tab, variable=self.dpt_var_2, orient='horizontal', length=300, from_=0, to=200)
self.dpt_slider_2.place(x=60,y=yy)
self.dpt_slider_2_l=Label(self.main_tab, textvariable=str(self.dpt_var_2), relief="flat")
self.dpt_slider_2_l.place(x=370,y=yy)
yy=660
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=910
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=1030
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)
print("end main_tab")
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)
self.run_script_button2=Button(self.script_tab,text="run script",bd=2,bg='white',command=self.run_script,width=8,relief=RIDGE)
self.run_script_button2.place(x=680,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=80,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()
print("end script_tab")
yy=10
self.dir_name_label2=Label(self.autorun_tab,text="dir name",width=10)
self.dir_name_label2.place(x=30,y=yy)
self.dir_name_field2=Entry(self.autorun_tab,width=50)
self.dir_name_field2.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_button2=Button(self.autorun_tab, text="directory",bd=2,bg='white',command=self.click_dir_button2, width=10, relief=RIDGE)
self.dir_button2.place(x=570,y=yy)
self.run_autorun_button2=Button(self.autorun_tab,text="run autorun",bd=2,bg='white',command=self.run_autorun,width=8,relief=RIDGE)
self.run_autorun_button2.place(x=680,y=yy)
yy=50
self.file_name_label2=Label(self.autorun_tab,text="file name",width=10)
self.file_name_label2.place(x=30,y=yy)
self.file_name_field2=Entry(self.autorun_tab,width=50)
self.file_name_field2.place(x=100,y=yy)
self.read_button2=Button(self.autorun_tab, text="read",bd=2,bg='white',command=self.click_read_button2, width=8, relief=RIDGE)
self.read_button2.place(x=500,y=yy)
self.dir_button2=Button(self.autorun_tab, text="select",bd=2,bg='white',command=self.click_select_file_button2, width=10, relief=RIDGE)
self.dir_button2.place(x=570,y=yy)
yy=100
self.rpt_var=tk.BooleanVar()
self.rpt_var.set(False)
self.repeat_check=Checkbutton(self.autorun_tab,variable=self.rpt_var,text="repeat autorun")
self.repeat_check.place(x=30,y=yy)
self.rpt_var.set(False)
yy=150
self.autorun_frame=tk.Frame(self.autorun_tab,width=80,height=100)
self.autorun_frame.place(x=30,y=yy)
self.autorun_area=scrolledtext.ScrolledText(self.autorun_frame)
self.autorun_area.pack()
#self.run_script()
self.set_autorun()
print("end autorun_tab")
def set_script(self):
print("start set_script")
#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 set_autorun(self):
print("start set_autorun")
self.autorun_area.insert(tk.END, "load 3d_dragon_02.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "load 3d_ex01.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "load 3d_ex02.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "load 3d_ex03.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "start_cam\n")
self.autorun_area.insert(tk.END, "wait 60.0\n")
self.autorun_area.insert(tk.END, "stop_cam\n")
self.autorun_area.insert(tk.END, "wait 10.0\n")
def click_read_button(self):
print("click_read_button")
fn=self.file_name_field.get()
print("fn="+fn)
self.read_file_into_script(fn)
def click_read_button2(self):
print("click_read_button2")
fn=self.file_name_field2.get()
print("fn="+fn)
f=None
try:
f=open(fn,"r")
except Exception as e:
print("file open error.")
tb=sys.exc_info()[2]
print("message:{0}".format(e.with_traceback(tb)))
return
try:
self.autorun_area.delete(1.0,END)
line=f.readline()
while line:
print(line)
self.autorun_area.insert(END,line)
line=f.readline()
f.close()
except Exception as e:
print("file "+fn+" read error")
tb=sys.exc_info()[2]
print("message:{0}".format(e.with_traceback(tb)))
def click_dir_button(self):
print("click_dir_button")
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_dir_button2(self):
print("click_dir_button2")
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_field2.insert(END,folder_name)
def click_select_file_button(self):
print("click_select_file_button")
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 click_select_file_button2(self):
print("click_select_file_button2")
fType = [("python", "*.py")]
iDir=""
xdir=self.dir_name_field2.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_field2.delete(0,END)
self.file_name_field2.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 run_autorun(self):
self.autorun=self.autorun_area.get(0.,END)
print("run_script:"+self.autorun)
self.autorun_thread=threading.Thread(target=self.autorun_handler)
#autorun_threadをstart
self.autorun_thread.start()
def autorun_handler(self):
print("autorun_handler")
autorun_list=(self.autorun).splitlines()
max_line=len(autorun_list)
line_no=0
while True:
while line_no<max_line:
cx=autorun_list[line_no]
print("run command:"+cx)
self.interpret_command(cx)
line_no=line_no+1
if self.rpt_var.get():
line_no=0
print("continue autorun")
else:
print("end autorun")
return
def parse_key(self,key,inx,rest):
#print("parse_key-"+key)
keylen=len(key)
inlen=len(inx)
if inx.startswith(key):
rest[0]=inx[keylen:inlen]
return True
rest[0]=inx
return False
def parse_String_Constant(self,inx,strc,rest):
rx=[""]
xstr=""
if self.parse_key('"',inx,rx):
#print("parse_String_Constant")
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
def parse_b(self,inx):
#print("parse_b")
#print("p="+str(p))
xlen=len(inx)
if 1>xlen:
return ""
while inx[0]==' ':
inx=inx[1:]
return inx
def parse_tag(self,inx,tag,rest):
rx=[""]
xstr=""
wstr=[""]
if self.parse_key('<',inx,rx):
#print("parse_tag")
inx=rx[0]
fx=inx[0]
xlen=len(inx)
while fx!='>':
if xlen<=0:
return False
if fx=='\\':
inx=inx[1:xlen]
if self.parse_String_Constant(inx,wstr,rx):
inx=rx[0]
xstr=xstr+'"'+wstr[0]+'"'
else:
xstr=xstr+fx
inx=inx[1:xlen]
fx=inx[0]
xlen=len(inx)
if self.parse_key('>',inx,rx):
tag[0]=xstr
rest[0]=rx[0]
return True
return False
def get_tokens(self):
return self.tokens
def next_token(self):
if self.token_ith<len(self.tokens):
self.token_ith=self.token_ith+1
return self.tokens[self.token_ith-1]
return ""
def read_file_into_script(self,fn):
print("read_file_into_script:"+fn)
f=None
try:
f=open(fn,"r")
except Exception as e:
print("file open error.")
tb=sys.exc_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.exc_info()[2]
print("message.{0}".format(e.with_traceback(tb)))
def start_cam(self):
self.sending.set(True)
def stop_cam(self):
self.sending.set(False)
def interpret_command(self,cx):
print("interpret_command:"+cx)
cx=self.parse_b(cx)
rest=[' ']
if self.parse_key("load ",cx,rest):
fn=self.parse_b(rest[0])
print("fn="+fn)
self.read_file_into_script(fn)
self.run_script()
return
elif self.parse_key("wait ",cx,rest):
tx=self.parse_b(rest[0])
tf=float(tx)
time.sleep(tf)
return
elif self.parse_key("start_cam",cx,rest):
print("start_cam")
self.start_cam()
return
elif self.parse_key("stop_cam",cx,rest):
print("stop_cam")
self.stop_cam()
return
else:
print("no such command:"+cx)
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(self)
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.dpt_var_1.get())/100.0
#print("depth range far(m):")
#self.d_far=input()
self.panel.near_depth_field.delete(0,END)
self.panel.near_depth_field.insert(END,str(self.d_near))
self.d_far=float(self.panel.dpt_var_2.get())/100.0
self.panel.far_depth_field.delete(0,END)
self.panel.far_depth_field.insert(END,str(self.d_far))
#print("d_far="+str(self.d_far))
self.clipping_near = float(self.d_near) / self.depth_scale
#print("depth range: ("+str(self.d_near)+" - "+str(self.d_far)+")")
self.clipping_distance_in_meters = float(self.d_far) #1 meter
self.clipping_distance = self.clipping_distance_in_meters / self.depth_scale
#print("ping_distance_in_meters="+str(self.clipping_distance_in_meters))
#print("clipping_distance ="+str(self.clipping_distance))
# 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()
}}
終了行:
[[TeleportDresser]]
#code(Python){{
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2017 Intel Corporation. All Rights Reserved.
#20230623
#####################################################
## 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,mw):
self.host=host
self.connected=False
self.soc=None
self.handle_thread=None
self.message_window=mw
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.exc_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,message_window):
self.message_window=message_window
self.coms[0]=Com("192.168.13.128",message_window)
self.coms[1]=Com("192.168.13.129",message_window)
self.coms[2]=Com("192.168.13.130",message_window)
self.coms[3]=Com("192.168.13.131",message_window)
self.coms[4]=Com("192.168.13.132",message_window)
self.coms[5]=Com("192.168.13.133",message_window)
self.coms[6]=Com("192.168.13.134",message_window)
#self.coms[7]=Com("192.168.13.16")
self.coms[7]=Com("192.168.13.135",message_window)
#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.exc_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("850x1220")
self.notebook =ttk.Notebook(root)
self.main_tab=tk.Frame(self.notebook, bg='white')
self.script_tab=tk.Frame(self.notebook, bg='white')
self.autorun_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.add(self.autorun_tab, text="autorun")
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=580
self.dpt_var_1=tk.IntVar()
self.dpt_var_1.set(10)
self.dpt_slider_1 = Scale(self.main_tab, variable=self.dpt_var_1, orient='horizontal', length=300, from_=0, to=200)
self.dpt_slider_1.place(x=60,y=yy)
self.dpt_slider_1_l=Label(self.main_tab, textvariable=str(self.dpt_var_1), relief="flat")
self.dpt_slider_1_l.place(x=370,y=yy)
yy=620
self.dpt_var_2=tk.IntVar()
self.dpt_var_2.set(100)
self.dpt_slider_2 = Scale(self.main_tab, variable=self.dpt_var_2, orient='horizontal', length=300, from_=0, to=200)
self.dpt_slider_2.place(x=60,y=yy)
self.dpt_slider_2_l=Label(self.main_tab, textvariable=str(self.dpt_var_2), relief="flat")
self.dpt_slider_2_l.place(x=370,y=yy)
yy=660
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=910
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=1030
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)
print("end main_tab")
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)
self.run_script_button2=Button(self.script_tab,text="run script",bd=2,bg='white',command=self.run_script,width=8,relief=RIDGE)
self.run_script_button2.place(x=680,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=80,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()
print("end script_tab")
yy=10
self.dir_name_label2=Label(self.autorun_tab,text="dir name",width=10)
self.dir_name_label2.place(x=30,y=yy)
self.dir_name_field2=Entry(self.autorun_tab,width=50)
self.dir_name_field2.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_button2=Button(self.autorun_tab, text="directory",bd=2,bg='white',command=self.click_dir_button2, width=10, relief=RIDGE)
self.dir_button2.place(x=570,y=yy)
self.run_autorun_button2=Button(self.autorun_tab,text="run autorun",bd=2,bg='white',command=self.run_autorun,width=8,relief=RIDGE)
self.run_autorun_button2.place(x=680,y=yy)
yy=50
self.file_name_label2=Label(self.autorun_tab,text="file name",width=10)
self.file_name_label2.place(x=30,y=yy)
self.file_name_field2=Entry(self.autorun_tab,width=50)
self.file_name_field2.place(x=100,y=yy)
self.read_button2=Button(self.autorun_tab, text="read",bd=2,bg='white',command=self.click_read_button2, width=8, relief=RIDGE)
self.read_button2.place(x=500,y=yy)
self.dir_button2=Button(self.autorun_tab, text="select",bd=2,bg='white',command=self.click_select_file_button2, width=10, relief=RIDGE)
self.dir_button2.place(x=570,y=yy)
yy=100
self.rpt_var=tk.BooleanVar()
self.rpt_var.set(False)
self.repeat_check=Checkbutton(self.autorun_tab,variable=self.rpt_var,text="repeat autorun")
self.repeat_check.place(x=30,y=yy)
self.rpt_var.set(False)
yy=150
self.autorun_frame=tk.Frame(self.autorun_tab,width=80,height=100)
self.autorun_frame.place(x=30,y=yy)
self.autorun_area=scrolledtext.ScrolledText(self.autorun_frame)
self.autorun_area.pack()
#self.run_script()
self.set_autorun()
print("end autorun_tab")
def set_script(self):
print("start set_script")
#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 set_autorun(self):
print("start set_autorun")
self.autorun_area.insert(tk.END, "load 3d_dragon_02.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "load 3d_ex01.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "load 3d_ex02.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "load 3d_ex03.py\n")
self.autorun_area.insert(tk.END, "wait 30.0\n")
self.autorun_area.insert(tk.END, "start_cam\n")
self.autorun_area.insert(tk.END, "wait 60.0\n")
self.autorun_area.insert(tk.END, "stop_cam\n")
self.autorun_area.insert(tk.END, "wait 10.0\n")
def click_read_button(self):
print("click_read_button")
fn=self.file_name_field.get()
print("fn="+fn)
self.read_file_into_script(fn)
def click_read_button2(self):
print("click_read_button2")
fn=self.file_name_field2.get()
print("fn="+fn)
f=None
try:
f=open(fn,"r")
except Exception as e:
print("file open error.")
tb=sys.exc_info()[2]
print("message:{0}".format(e.with_traceback(tb)))
return
try:
self.autorun_area.delete(1.0,END)
line=f.readline()
while line:
print(line)
self.autorun_area.insert(END,line)
line=f.readline()
f.close()
except Exception as e:
print("file "+fn+" read error")
tb=sys.exc_info()[2]
print("message:{0}".format(e.with_traceback(tb)))
def click_dir_button(self):
print("click_dir_button")
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_dir_button2(self):
print("click_dir_button2")
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_field2.insert(END,folder_name)
def click_select_file_button(self):
print("click_select_file_button")
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 click_select_file_button2(self):
print("click_select_file_button2")
fType = [("python", "*.py")]
iDir=""
xdir=self.dir_name_field2.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_field2.delete(0,END)
self.file_name_field2.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 run_autorun(self):
self.autorun=self.autorun_area.get(0.,END)
print("run_script:"+self.autorun)
self.autorun_thread=threading.Thread(target=self.autorun_handler)
#autorun_threadをstart
self.autorun_thread.start()
def autorun_handler(self):
print("autorun_handler")
autorun_list=(self.autorun).splitlines()
max_line=len(autorun_list)
line_no=0
while True:
while line_no<max_line:
cx=autorun_list[line_no]
print("run command:"+cx)
self.interpret_command(cx)
line_no=line_no+1
if self.rpt_var.get():
line_no=0
print("continue autorun")
else:
print("end autorun")
return
def parse_key(self,key,inx,rest):
#print("parse_key-"+key)
keylen=len(key)
inlen=len(inx)
if inx.startswith(key):
rest[0]=inx[keylen:inlen]
return True
rest[0]=inx
return False
def parse_String_Constant(self,inx,strc,rest):
rx=[""]
xstr=""
if self.parse_key('"',inx,rx):
#print("parse_String_Constant")
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
def parse_b(self,inx):
#print("parse_b")
#print("p="+str(p))
xlen=len(inx)
if 1>xlen:
return ""
while inx[0]==' ':
inx=inx[1:]
return inx
def parse_tag(self,inx,tag,rest):
rx=[""]
xstr=""
wstr=[""]
if self.parse_key('<',inx,rx):
#print("parse_tag")
inx=rx[0]
fx=inx[0]
xlen=len(inx)
while fx!='>':
if xlen<=0:
return False
if fx=='\\':
inx=inx[1:xlen]
if self.parse_String_Constant(inx,wstr,rx):
inx=rx[0]
xstr=xstr+'"'+wstr[0]+'"'
else:
xstr=xstr+fx
inx=inx[1:xlen]
fx=inx[0]
xlen=len(inx)
if self.parse_key('>',inx,rx):
tag[0]=xstr
rest[0]=rx[0]
return True
return False
def get_tokens(self):
return self.tokens
def next_token(self):
if self.token_ith<len(self.tokens):
self.token_ith=self.token_ith+1
return self.tokens[self.token_ith-1]
return ""
def read_file_into_script(self,fn):
print("read_file_into_script:"+fn)
f=None
try:
f=open(fn,"r")
except Exception as e:
print("file open error.")
tb=sys.exc_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.exc_info()[2]
print("message.{0}".format(e.with_traceback(tb)))
def start_cam(self):
self.sending.set(True)
def stop_cam(self):
self.sending.set(False)
def interpret_command(self,cx):
print("interpret_command:"+cx)
cx=self.parse_b(cx)
rest=[' ']
if self.parse_key("load ",cx,rest):
fn=self.parse_b(rest[0])
print("fn="+fn)
self.read_file_into_script(fn)
self.run_script()
return
elif self.parse_key("wait ",cx,rest):
tx=self.parse_b(rest[0])
tf=float(tx)
time.sleep(tf)
return
elif self.parse_key("start_cam",cx,rest):
print("start_cam")
self.start_cam()
return
elif self.parse_key("stop_cam",cx,rest):
print("stop_cam")
self.stop_cam()
return
else:
print("no such command:"+cx)
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(self)
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.dpt_var_1.get())/100.0
#print("depth range far(m):")
#self.d_far=input()
self.panel.near_depth_field.delete(0,END)
self.panel.near_depth_field.insert(END,str(self.d_near))
self.d_far=float(self.panel.dpt_var_2.get())/100.0
self.panel.far_depth_field.delete(0,END)
self.panel.far_depth_field.insert(END,str(self.d_far))
#print("d_far="+str(self.d_far))
self.clipping_near = float(self.d_near) / self.depth_scale
#print("depth range: ("+str(self.d_near)+" - "+str(self.d_far)+")")
self.clipping_distance_in_meters = float(self.d_far) #1 meter
self.clipping_distance = self.clipping_distance_in_meters / self.depth_scale
#print("ping_distance_in_meters="+str(self.clipping_distance_in_meters))
#print("clipping_distance ="+str(self.clipping_distance))
# 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()
}}
ページ名:
![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
