diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index ea1a731..a1224fd 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -9,8 +9,10 @@ #===== Applicationクラスの定義　tk.Frameクラスを継承 =====# class Application(tk.Frame): - #--- コンストラクタ --- - # プログラムを実行したときに最初にすべき処理を全て記述する + """ + +++++ コンストラクタ +++++ + プログラムを実行したときに最初にすべき処理を全て記述する + """ def __init__(self, master): super().__init__(master) # スーパークラスのコンストラクタを実行 self.master.title("Waypoints Manager") @@ -20,13 +22,13 @@ self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) # バーに追加するメニューを作成 self.menu_bar.add_cascade(label="File", menu=self.file_menu) # Fileメニューとしてバーに追加 self.file_menu.add_command(label="Save", # FileメニューにSaveコマンドを追加 - command=self.menu_save_clicked, #コールバック関数を設定 - accelerator="Ctrl+S"# 右側に表示するキーボードショートカット - ) + command=self.menu_save_clicked, #コールバック関数を設定 + accelerator="Ctrl+S"# 右側に表示するキーボードショートカット + ) self.file_menu.add_command(label="Save As", # 同様にSave Asコマンドを追加 - command=self.menu_saveas_clicked, - accelerator="Ctrl+Shift+S" - ) + command=self.menu_saveas_clicked, + accelerator="Ctrl+Shift+S" + ) self.bind_all("", self.menu_save_clicked) #キーボードショートカットを設定 self.bind_all("", self.menu_saveas_clicked) self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定 @@ -72,21 +74,20 @@ self.master.bind("", self.right_click) ## ウィンドウに関するコールバック - # self.master.bind("", self.window_resize_callback) + self.master.bind("", self.window_resize_callback) + origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m) + resol = self.__map_yaml['resolution'] + self.img_origin = [-origin[0]/resol, self.__map_img_pil.height+origin[1]/resol, 1] self.plot_origin() return - #--- 地図上の原点に円を描画する --- - def plot_origin(self): - origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの - return - - - #--- mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む --- - #--- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- + """ + +++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++ + --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- + """ def get_map_info(self): map_path = Path('..','..','waypoint_nav','maps','map') # .pgmと.yamlの手前までのパス map_img_pil = Image.open(Path(str(map_path)+'.pgm')) # .pgmをplillowで読み込む @@ -97,7 +98,10 @@ - #--- これもget_param()でパスを受け取り、読み込めるようにする --- + """ + +++++ waypointsのパスを受け取り読み込んだデータを返す +++++ + --- これもget_param()でパスを受け取り、読み込めるようにする --- + """ def get_waypoints(self): file_path = Path('..','..','waypoint_nav','param','waypoints.yaml') with open(file_path) as file: @@ -106,23 +110,73 @@ - #--- マウスを左クリックしながらドラッグしたときのコールバック関数 --- + """ + +++++ 地図上の原点に円を描画する +++++ + """ + def plot_origin(self): + origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換 + origin_affine = origin_affine.astype(np.int32) # 符号あり整数に変換 + r = 10 # 円の半径（ピクセル） + x0 = origin_affine[0] - r + y0 = origin_affine[1] - r + x1 = origin_affine[0] + r + 1 + y1 = origin_affine[1] + r + 1 + # 円を描画できる位置かどうか判別 + if (x0 < 0) or (y0 < 0) or (x1 > self.canv_w) or (y1 > self.canv_h): + return + elif self.canvas.find_withtag("origin"): # 既に円を描画済み + self.canvas.itemconfig("origin", x0=x0, y0=y0, x1=x1, y1=y1) + else: # 初めて円を描画する + self.canvas.create_oval(x0, y0, x1, y1, tags="origin", fill='cyan', outline='blue') + return + + + + """ + +++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++ + """ def left_click_move(self, event): print("x=" + str(event.x) + " y=" + str(event.y)) return - #--- 右クリックしたときのコールバック関数 --- + """ + +++++ 右クリックしたときのコールバック関数 +++++ + """ def right_click(self, event): + # クリックした座標の近くにあるオブジェクトを取得 + clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) + if clicked_obj: # 何かオブジェクトがクリックされていた場合、何もしない + return + # クリックされた座標 => 元画像の座標 の変換 + mat_inv = np.linalg.inv(self.mat_affine) + img_xy = np.dot(mat_inv, np.array([[event.x], [event.y], [1]])) + # 変換後の元画像の座標がサイズから外れている場合何もしない（地図画像の外をクリックしている） + if (img_xy[0] < 0) or (img_xy[1] < 0) or \ + (img_xy[0] > self.__map_img_pil.width) or (img_xy[1] > self.__map_img_pil.height): + return self.popup_menu.post(event.x_root, event.y_root) # メニューをポップアップ - self.right_click_coord = [event.x, event.y] # クリックされた座標を変数に格納 + self.right_click_coord = img_xy # クリックされた元画像上の座標を変数に格納 return - #--- マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 --- - #--- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも --- + """ + +++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++ + """ + def add_waypoint(self): + print("Clicked \"add waypoint\"", self.right_click_coord[0], self.right_click_coord[1]) + pix_xy = np.array(self.right_click_coord, np.uint32) + print("value=",self.__map_img_pil.getpixel((pix_xy[0], pix_xy[1]))) + return + + + + """ + +++++ マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 +++++ + --- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも --- + """ def mouse_wheel(self, event): if event.delta > 0: # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 @@ -135,41 +189,52 @@ - #--- 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 --- - def add_waypoint(self): - print("Clicked \"add waypoint\"") - return - - - - #--- Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 --- + """ + +++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++ + """ def menu_save_clicked(self, event=None): print("Clicked \"Save\"") return - #--- Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 --- + """ + +++++ Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 +++++ + """ def menu_saveas_clicked(self, event=None): print("Clicked \"Save As\"") return - #--- 左クリックされたときに実行されるコールバック関数 --- + """ + +++++ 左クリックされたときに実行されるコールバック関数 +++++ + """ def left_click(self, event): - self.popup_menu.unpost() + self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 + # クリックした座標の近くにあるオブジェクトを取得 + clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) + if clicked_obj: # オブジェクトがクリックされた場合 + tag = self.canvas.gettags(clicked_obj[0])[0] + print("Clicked " + tag) + return - #--- mat_affineにx,yの平行移動を加えた同次変換行列を返す --- + """ + +++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++ + """ def translate_mat(self, x, y, mat_affine): mat = np.eye(3) mat[0, 2] = float(x) mat[1, 2] = float(y) return np.dot(mat, mat_affine) - #--- mat_affineにscale倍のリサイズを加えた同次変換行列を返す --- + + + """ + +++++ 引数のmat_affineにscale倍のリサイズを加えた同次変換行列を返す +++++ + """ def scale_mat(self, scale, mat_affine): mat = np.eye(3) mat[0, 0] = scale @@ -177,6 +242,10 @@ return np.dot(mat, mat_affine) + + """ + +++++ 元画像をaffne変換して描画、その画像を返す +++++ + """ def draw_image(self, mat_affine): mat_inv = np.linalg.inv(mat_affine) img = self.__map_img_pil.transform( @@ -186,19 +255,23 @@ fillcolor = 160 ) tk_img = ImageTk.PhotoImage(image=img) - self.canvas.create_image(0, 0, anchor='nw', image=tk_img) # 画像の描画 + if not self.canvas.find_withtag("map_image"): # 初めて画像を描画するとき + self.canvas.create_image(0, 0, anchor='nw', image=tk_img, tags="map_image") # 画像の描画 + else: + self.canvas.itemconfig("map_image", image=tk_img) # 既に描画された画像を差し替える return tk_img - #--- + + """ + +++++ ウィンドウサイズが変更されたとき、情報を更新する +++++ + """ def window_resize_callback(self, event): cw = self.canvas.winfo_width() ch = self.canvas.winfo_height() if (self.canv_w != cw) or (self.canv_h != ch): self.canv_w = cw self.canv_h = ch - self.canvas.delete("all") - self.canvas.create_image(self.canv_w/2, self.canv_h/2, image=self.draw_img_tk) return