diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index bc9ef8a..daec804 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -1,9 +1,12 @@ #import rospy +from textwrap import fill import tkinter as tk +import tkinter.filedialog import numpy as np import ruamel.yaml from PIL import Image, ImageTk from pathlib import Path +from ruamel.yaml.comments import CommentedMap, CommentedSeq #===== Applicationクラスの定義　tk.Frameクラスを継承 =====# @@ -35,16 +38,16 @@ ## 右クリックしたときに表示するポップアップメニューを作成 self.popup_menu = tk.Menu(self, tearoff=tk.OFF) - self.popup_menu.add_command(label="add waypoint", command=self.add_waypoint) + self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint) self.right_click_coord = None # 右クリックしたときの座標を保持する変数 ## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止 self.__map_img_pil, self.__map_yaml = self.get_map_info() - self.__waypoints = self.get_waypoints() + self.__waypoints, self.waypoint_filepath = self.get_waypoints() self.current_waypoints = self.__waypoints # 編集中のウェイポイント情報を保持する ## canvasを配置 - self.canvas = tk.Canvas(self.master, background="#a0a0a0") # 画像を描画するcanvas + self.canvas = tk.Canvas(self.master, background="#AAA") # 画像を描画するcanvas self.canvas.pack(expand=True, fill=tk.BOTH) # canvasを配置 self.update() # 情報の更新をする(canvasのサイズなどの情報が更新される) @@ -90,6 +93,11 @@ self.master.bind("", self.window_resize_callback) self.old_click_point = None + self.wp_info_win = None + self.editing_waypoint_id = None + self.moving_waypoint = False + self.add_wp_win = None + self.adding_waypoint = False return @@ -99,7 +107,7 @@ --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- """ def get_map_info(self): - map_path = Path('..','..','waypoint_nav','maps','map_gakunai') # .pgmと.yamlの手前までのパス + map_path = Path('..','..','waypoint_nav','maps','map') # .pgmと.yamlの手前までのパス map_img_pil = Image.open(Path(str(map_path)+'.pgm')) # .pgmをplillowで読み込む with open(str(map_path)+'.yaml') as file: # .yamlを読み込む map_yaml = ruamel.yaml.YAML().load(file) @@ -113,10 +121,10 @@ --- これもget_param()でパスを受け取り、読み込めるようにする --- """ def get_waypoints(self): - file_path = Path('..','..','waypoint_nav','param','waypoints_gakunai.yaml') + file_path = Path('..','..','waypoint_nav','param','test.yaml') with open(file_path) as file: waypoints = ruamel.yaml.YAML().load(file) - return waypoints + return waypoints, file_path @@ -144,17 +152,23 @@ def plot_waypoints(self): points = self.current_waypoints['waypoints'] # ウェイポイントのリスト r = self.point_rad + # ウェイポイントの追加または削除が行なわれている場合、初期化 + if len(self.waypoints_id) != len(points): + for id in self.waypoints_id: + self.canvas.delete(id) + self.waypoints_id = np.array([], np.uint8) + # 全てのウェイポイントを描画 for i in range(len(points)): - x = points[i]['point']['x'] / self.map_resolution + self.img_origin[0] - y = -points[i]['point']['y'] / self.map_resolution + self.img_origin[1] - xy_affine = np.dot(self.mat_affine, [x, y, 1]) + img_x = float(points[i]['point']['x']) / self.map_resolution + self.img_origin[0] + img_y = -float(points[i]['point']['y']) / self.map_resolution + self.img_origin[1] + xy_affine = np.dot(self.mat_affine, [img_x, img_y, 1]) x1 = xy_affine[0] - r y1 = xy_affine[1] - r x2 = xy_affine[0] + r + 1 y2 = xy_affine[1] + r + 1 if len(self.waypoints_id) < len(points): - id = self.canvas.create_oval(x1, y1, x2, y2, fill='magenta', outline='red', - activefill='red') + id = self.canvas.create_oval(x1, y1, x2, y2, + fill='#FDD', outline='red', activefill='red') self.waypoints_id = np.append(self.waypoints_id, id) else: id = self.waypoints_id[i] @@ -163,7 +177,6 @@ - """ +++++ 左クリックされたときに実行されるコールバック関数 +++++ """ @@ -171,9 +184,121 @@ 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: # オブジェクトがクリックされた場合 - print("id=",clicked_obj[0]) - print("tag=", self.canvas.gettags(clicked_obj[0])) + if not clicked_obj: return # オブジェクト以外の領域 + id = clicked_obj[0] + tag = self.canvas.gettags(id) + if (len(tag) == 0): return + if (tag[0] == "origin"): return + # ↓ウェイポイントがクリックされた場合、情報を表示 + if id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え + self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') + self.editing_waypoint_id = id + self.canvas.itemconfig(id, fill='red') + self.disp_waypoint_info(id) + return + + + + """ + +++++ ウェイポイントが左クリックされたとき、別窓で情報を表示する関数 +++++ + """ + def disp_waypoint_info(self, id): + wp_num = np.where(self.waypoints_id == id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 + point = self.current_waypoints['waypoints'][wp_num-1]['point'] + if (self.wp_info_win is None) or (not self.wp_info_win.winfo_exists()): + # ウィンドウが表示されてない場合、初期化 + self.wp_info_win = tk.Toplevel() + self.wp_info_win.protocol("WM_DELETE_WINDOW", self.close_wp_info) + self.wp_info_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 + # ウェイポイントファイルのキーを取得し、ラベルを配置 + label_w = 4 + for i, key in enumerate(point): + key_label = tk.Label(self.wp_info_win, text=key+":", width=label_w, font=("Consolas",15), anchor=tk.E) + key_label.grid(column=0, row=i, padx=2, pady=5) + txt_box = tk.Entry(self.wp_info_win, width=20, font=("Consolas", 15)) + txt_box.insert(tk.END, point[key]) + txt_box.grid(column=1, row=i, padx=2, pady=2, ipady=3, sticky=tk.EW) + # ボタンを配置 + canv = tk.Canvas(self.wp_info_win) + canv.grid(column=0, columnspan=2, row=self.wp_info_win.grid_size()[1], sticky=tk.EW) + apply_btn = tk.Button(canv, text="Apply", width=5, height=1, background="#FDD", + command=self.apply_btn_callback) + apply_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) + dnd_btn = tk.Button(canv, text="DnD", width=5, height=1, background="#EEE", + command=self.dnd_btn_callback) + dnd_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) + remove_btn = tk.Button(canv, text="Remove", width=7, height=1, background="#F00", + command=self.remove_btn_callback) + remove_btn.pack(side=tk.LEFT, anchor=tk.SE, padx=5, pady=5) + # 位置とサイズを設定 + self.wp_info_win.update() + w = self.wp_info_win.winfo_width() + h = self.wp_info_win.winfo_height() + x = self.master.winfo_x() + self.canv_w - w + y = self.master.winfo_y() + self.canv_h - h + geometry = "{}x{}+{}+{}".format(w, h, x, y) + self.wp_info_win.geometry(geometry) + else: + # 既にウィンドウが表示されている場合、テキストボックスの中身を変える + for i, key in enumerate(point): + txt_box = self.wp_info_win.grid_slaves(column=1, row=i)[0] + txt_box.delete(0, tk.END) + txt_box.insert(tk.END, point[key]) + self.wp_info_win.title("Waypoint " + str(wp_num)) # タイトルを設定 + return + + + + """ + +++++ Applyボタンを押したときのコールバック +++++ + """ + def apply_btn_callback(self): + wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 + point = self.current_waypoints['waypoints'][wp_num-1]['point'] + for i, key in enumerate(point): + txt_box = self.wp_info_win.grid_slaves(column=1, row=i)[0] + self.current_waypoints['waypoints'][wp_num-1]['point'][key] = float(txt_box.get()) + self.plot_waypoints() + return + + + + """ + +++++ ドラッグ&ドロップボタン(DnD)を押したときのコールバック +++++ + """ + def dnd_btn_callback(self): + c = self.wp_info_win.grid_slaves(row=self.wp_info_win.grid_size()[1]-1, column=0)[0] + btn = c.pack_slaves()[1] + # 押された状態とそうでない状態を切り替える + if btn["relief"] == tk.RAISED: + btn["relief"] = tk.SUNKEN + btn["background"] = "#AAA" + elif btn["relief"] == tk.SUNKEN: + btn["relief"] = tk.RAISED + btn["background"] = "#EEE" + return + + + + """ + +++++ removeボタンを押したときのコールバック +++++ + """ + def remove_btn_callback(self): + wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 + self.current_waypoints['waypoints'].pop(wp_num-1) # ウェイポイントを削除 + self.plot_waypoints() # ウェイポイントを再描画、waypoints_idを更新 + self.close_wp_info() + return + + + + """ + +++++ ウェイポイント情報を表示するサブウィンドウを閉じたときのコールバック +++++ + """ + def close_wp_info(self): + self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') + self.editing_waypoint_id = None + self.wp_info_win.destroy() return @@ -182,14 +307,53 @@ +++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++ """ def left_click_move(self, event): - if not self.old_click_point is None: - delta_x = event.x-self.old_click_point[0] - delta_y = event.y-self.old_click_point[1] - if (np.linalg.norm([delta_x, delta_y]) < 300): - self.translate_mat(event.x-self.old_click_point[0], event.y-self.old_click_point[1]) - self.draw_image() - self.canvas.move("origin", delta_x, delta_y) # self.plot_origin() - self.plot_waypoints() + if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()) and (not self.moving_waypoint): + # ウェイポイント情報が表示されていて、ウェイポイントを動かしていないとき + c = self.wp_info_win.grid_slaves(row=self.wp_info_win.grid_size()[1]-1, column=0)[0] + dnd_btn = c.pack_slaves()[1] # dndボタンの状態を取得 + clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) + if (clicked_obj) and (clicked_obj[0] == self.editing_waypoint_id) and (dnd_btn["relief"] == tk.SUNKEN): + # dndボタンが押された状態で、編集中のウェイポイントがクリックされたとき + self.moving_waypoint = True # ウェイポイントをドラッグで移動させるモードへ + self.old_click_point = [event.x, event.y] + return + if self.old_click_point is None: + self.old_click_point = [event.x, event.y] + return + # カーソルの移動量を計算 + delta_x = event.x-self.old_click_point[0] + delta_y = event.y-self.old_click_point[1] + #ウェイポイントをドラッグで動かしているとき + if self.moving_waypoint: + self.canvas.move(self.editing_waypoint_id, delta_x, delta_y) + box = self.canvas.bbox(self.editing_waypoint_id) + px = (box[2] + box[0]) / 2 # ウィンドウ上の座標 + py = (box[3] + box[1]) / 2 + mat_inv = np.linalg.inv(self.mat_affine) + img_xy = np.dot(mat_inv, [px, py, 1]) # マップ画像上の座標 + # マップ画像上の座標を、実際の座標に変換 + x = (img_xy[0] - self.img_origin[0]) * self.map_resolution + y = (-img_xy[1] + self.img_origin[1]) * self.map_resolution + x = round(x, 6) + y = round(y, 6) + wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + # 編集中のウェイポイント情報を更新 + self.current_waypoints['waypoints'][wp_num]['point']['x'] = x + self.current_waypoints['waypoints'][wp_num]['point']['y'] = y + # 表示中のウェイポイント情報を更新 + txt_box = self.wp_info_win.grid_slaves(column=1, row=0)[0] + txt_box.delete(0, tk.END) + txt_box.insert(tk.END, x) + txt_box = self.wp_info_win.grid_slaves(column=1, row=1)[0] + txt_box.delete(0, tk.END) + txt_box.insert(tk.END, y) + self.old_click_point = [event.x, event.y] + return + # ウェイポイント移動モードでないとき、地図を平行移動 + self.translate_mat(delta_x, delta_y) + self.draw_image() + self.canvas.move("origin", delta_x, delta_y) #self.plot_origin() + self.plot_waypoints() self.old_click_point = [event.x, event.y] return @@ -200,6 +364,7 @@ """ def left_click_release(self, event): self.old_click_point = None + self.moving_waypoint = False return @@ -229,9 +394,65 @@ +++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++ """ def add_waypoint(self): - print("Clicked \"add waypoint\"", self.right_click_coord) - pix_xy = np.array(self.right_click_coord, np.uint32) - print("value=",self.__map_img_pil.getpixel((pix_xy[0], pix_xy[1]))) + if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): + self.close_wp_info() + img_xy = self.right_click_coord + if self.__map_img_pil.getpixel((img_xy[0], img_xy[1])) == 0: + print("There is obstacles") + return + # 何番目のウェイポイントの次に追加するか入力させる + self.add_wp_win = tk.Toplevel() + self.add_wp_win.title("Add waypoint") + self.add_wp_win.protocol("WM_DELETE_WINDOW") + self.add_wp_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 + msg = tk.Label(self.add_wp_win, text="Add waypoint after no. ", font=("Consolas",15), anchor=tk.E) + msg.grid(column=0, row=0, padx=10, pady=10, sticky=tk.EW) + txt_box = tk.Entry(self.add_wp_win, width=4, font=("Consolas",15)) + txt_box.grid(column=1, row=0, pady=10, sticky=tk.W) + add_btn = tk.Button(self.add_wp_win, text="Add", width=5, height=1, command=self.add_btn_callback) + add_btn.grid(column=0, columnspan=2, row=1, padx=10, pady=10) + self.add_wp_win.update() + w = self.add_wp_win.winfo_width() + 10 + h = self.add_wp_win.winfo_height() + x = int((self.canv_w - w) / 2) + y = int((self.canv_h - h) / 2) + geometry = "{}x{}+{}+{}".format(w, h, x, y) + self.add_wp_win.geometry(geometry) + return + + + + """ + +++++ add waypoint hereをクリックして開いた別窓のボタンのコールバック +++++ + """ + def add_btn_callback(self): + num_box = self.add_wp_win.grid_slaves(row=0, column=1)[0] + num = num_box.get() + if (num == ""): + print("Please enter number") + return + self.add_wp_win.destroy() + num = int(num) + img_xy = self.right_click_coord + # ウェイポイント座標を計算 + x = (img_xy[0] - self.img_origin[0]) * self.map_resolution + y = (-img_xy[1] + self.img_origin[1]) * self.map_resolution + x = round(x, 6) + y = round(y, 6) + # ウェイポイントを追加 + data = CommentedMap() + data['point'] = CommentedMap() + for key in self.current_waypoints['waypoints'][0]['point']: + if (key == 'x'): val = x + elif (key == 'y'): val = y + else: val = 0.0 + data['point'][key] = val + self.current_waypoints['waypoints'].insert(num, data) + self.plot_waypoints() # ウェイポイントを再描画、waypoints_idも初期化 + id = self.waypoints_id[num] + self.editing_waypoint_id = id + self.canvas.itemconfig(id, fill='red') + self.disp_waypoint_info(id) return @@ -260,7 +481,8 @@ +++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++ """ def menu_save_clicked(self, event=None): - print("Clicked \"Save\"") + self.save_waypoints(self.waypoint_filepath) + print("Saved changes!") return @@ -269,11 +491,50 @@ +++++ Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 +++++ """ def menu_saveas_clicked(self, event=None): - print("Clicked \"Save As\"") + new_filepath = tkinter.filedialog.asksaveasfilename( + parent=self.master, + title="Save As", + initialdir=str(Path('..','..','waypoint_nav','param')), + filetypes=[("YAML", ".yaml")], + defaultextension="yaml" + ) + if len(new_filepath) == 0: return # cancel + self.save_waypoints(new_filepath) + self.waypoint_filepath = new_filepath + print("Save As", "\"", new_filepath, "\"") return + """ + +++++ 現在のウェイポイント情報を指定のファイルに書き込む +++++ + """ + def save_waypoints(self, path): + points = self.current_waypoints['waypoints'] + fin_pose = self.current_waypoints['finish_pose'] + with open(path, 'w') as f: + f.write("waypoints:\n") + for point in points: + f.write("- point: {") + for i, key in enumerate(point['point']): + if i == 0: + f.write(key + ": " + str(point['point'][key])) + else: + f.write(", " + key + ": " + str(point['point'][key])) + f.write("}\n") + f.write("finish_pose:\n") + seq = fin_pose['header']['seq'] + stamp = fin_pose['header']['stamp'] + fid = fin_pose['header']['frame_id'] + f.write(" header: {" + "seq: {}, stamp: {}, frame_id: {}".format(seq, stamp, fid) + "}\n") + f.write(" pose:\n") + p = fin_pose['pose']['position'] + o = fin_pose['pose']['orientation'] + f.write(" position: {" + "x: {}, y: {}, z: {}".format(p["x"], p["y"], p["z"]) + "}\n") + f.write(" orientation: {" + "x: {}, y: {}, z: {}, w: {}".format(o["x"], o["y"], o["z"], o["w"]) + "}") + return + + """ +++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++ @@ -337,7 +598,7 @@ if __name__ == "__main__": #rospy.init_node("manager_GUI") root = tk.Tk() # 大元になるウィンドウ - w, h = root.winfo_screenwidth(), root.winfo_screenheight() + w, h = root.winfo_screenwidth()-100, root.winfo_screenheight()-300 root.geometry("%dx%d+0+0" % (w, h)) app = Application(master=root) # tk.Frameを継承したApplicationクラスのインスタンス app.mainloop()