diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index 05b3fc2..d15b0b6 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -3,10 +3,12 @@ import tkinter as tk import tkinter.filedialog import numpy as np +import quaternion +import math import ruamel.yaml from PIL import Image, ImageTk from pathlib import Path -from ruamel.yaml.comments import CommentedMap, CommentedSeq +from ruamel.yaml.comments import CommentedMap #===== Applicationクラスの定義　tk.Frameクラスを継承 =====# @@ -32,10 +34,24 @@ command=self.menu_saveas_clicked, accelerator="Ctrl+Shift+S" ) + self.file_menu.add_separator() + self.file_menu.add_command(label="Exit", command=self.menu_exit_clicked, accelerator="Ctrl+Q") self.bind_all("", self.menu_save_clicked) #キーボードショートカットを設定 self.bind_all("", self.menu_saveas_clicked) + self.bind_all("", self.menu_exit_clicked) self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定 + ## 画面上部に、システムからのメッセージを表示するラベルを配置 + self.msg_label = tk.Label(self.master, text="Start up", anchor=tk.E) + self.msg_label.pack(fill=tk.X) + + ## 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 + self.status_bar = tk.Frame(self.master) + self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, text="(x, y, val) = ", + width=30, anchor=tk.W) + self.mouse_position.pack(side=tk.LEFT, padx=1) + self.status_bar.pack(side=tk.BOTTOM, fill=tk.X) + ## 右クリックしたときに表示するポップアップメニューを作成 self.popup_menu = tk.Menu(self, tearoff=tk.OFF) self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint) @@ -82,33 +98,49 @@ ## waypoints.yamlからウェイポイント情報を取得し、画像にポイントを描画 self.waypoints_id = np.array([], np.uint16) + self.wplabel_id = np.array([], np.uint16) + self.finish_pose = None + self.finishpose_id = None self.plot_waypoints() ## マウスイベントに対するコールバックを設定 + self.master.bind("", self.mouse_move) self.master.bind("", self.mouse_wheel) self.master.bind("", self.left_click_move) self.master.bind("", self.left_click) self.master.bind("", self.left_click_release) self.master.bind("", self.right_click) + self.master.bind("", self.ctrl_left_click) + self.master.bind("", self.ctrl_right_click) ## ウィンドウに関するコールバック 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 + ## その他必要になる変数の初期化 + self.old_click_point = None # 最後にカーソルのあった座標を保持 + self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ + self.editing_waypoint_id = None # 編集中のウェイポイントを示す図形のオブジェクトID + self.moving_waypoint = False # ウェイポイントをDnDで動かしている最中かどうか + self.add_wp_win = None # add waypoint hereをクリックしたときに表示するウィンドウ return """ + +++++ 画面上部にメッセージを表示する +++++ + """ + def message(self, msg): + if not isinstance(msg, str): + msg = str(msg) + self.msg_label["text"] = str(msg) + + + + """ +++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++ --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- """ def get_map_info(self): - map_path = Path('..','..','waypoint_nav','maps','map') # .pgmと.yamlの手前までのパス + map_path = Path('..','..','waypoint_nav','maps','map_gakunai') # .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) @@ -122,7 +154,7 @@ --- これもget_param()でパスを受け取り、読み込めるようにする --- """ def get_waypoints(self): - file_path = Path('..','..','waypoint_nav','param','waypoints.yaml') + file_path = Path('..','..','waypoint_nav','param','waypoints_gakunai.yaml') with open(file_path) as file: waypoints = ruamel.yaml.YAML().load(file) self.master.title(file_path.name + " - " + self.master.title()) @@ -136,14 +168,14 @@ def plot_origin(self): origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換 r = self.point_rad # 円の半径（ピクセル） - x1 = origin_affine[0] - r - y1 = origin_affine[1] - r - x2 = origin_affine[0] + r + 1 - y2 = origin_affine[1] + r + 1 + x0 = origin_affine[0] - r + y0 = origin_affine[1] - r + x1 = origin_affine[0] + r + 1 + y1 = origin_affine[1] + r + 1 if self.canvas.find_withtag("origin"): - self.canvas.moveto("origin", x1, y1) + self.canvas.moveto("origin", x0, y0) else: - self.canvas.create_oval(x1, y1, x2, y2, tags="origin", fill='cyan', outline='blue') + self.canvas.create_oval(x0, y0, x1, y1, tags="origin", fill='cyan', outline='blue') return @@ -152,6 +184,7 @@ +++++ 地図上にウェイポイントを示す円を描画する +++++ """ def plot_waypoints(self): + print(self.canvas.find_all()) points = self.current_waypoints['waypoints'] # ウェイポイントのリスト r = self.point_rad # ウェイポイントの追加または削除が行なわれている場合、初期化 @@ -164,17 +197,55 @@ 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 + x0 = xy_affine[0] - r + y0 = xy_affine[1] - r + x1 = xy_affine[0] + r + 1 + y1 = xy_affine[1] + r + 1 if len(self.waypoints_id) < len(points): - id = self.canvas.create_oval(x1, y1, x2, y2, + id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FDD', outline='red', activefill='red') self.waypoints_id = np.append(self.waypoints_id, id) else: id = self.waypoints_id[i] - self.canvas.moveto(id, x1, y1) + self.canvas.moveto(id, x0, y0) + # Finish poseを描画 + if self.finish_pose is None: + pos = self.current_waypoints['finish_pose']['pose']['position'] + img_x = float(pos['x']) / self.map_resolution + self.img_origin[0] + img_y = -float(pos['y']) / self.map_resolution + self.img_origin[1] + orient = self.current_waypoints['finish_pose']['pose']['orientation'] + quat = np.quaternion(orient['x'], orient['y'], orient['z'], orient['w']) + yaw = quaternion.as_euler_angles(quat)[1] + self.finish_pose = [img_x, img_y, yaw] + xy_affine = np.dot(self.mat_affine, [self.finish_pose[0], self.finish_pose[1], 1]) + x0 = xy_affine[0] + y0 = xy_affine[1] + x1 = x0 + math.cos(self.finish_pose[2]) * r * 3 + y1 = y0 - math.sin(self.finish_pose[2]) * r * 3 + if self.finishpose_id is not None: + self.canvas.delete(self.finishpose_id) + self.finishpose_id = self.canvas.create_line(x0, y0, x1, y1, width=10, + arrow=tk.LAST, arrowshape=(12,15,9), fill="#AAF") + return + + + + """ + +++++ キャンバス内でマウスを動かしたときのコールバック +++++ + """ + def mouse_move(self, event): + mat_inv = np.linalg.inv(self.mat_affine) + img_xy = np.dot(mat_inv, [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): + self.mouse_position["text"] = " Out of map" + return + 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, 3) + y = round(y, 3) + val = self.__map_img_pil.getpixel((img_xy[0], img_xy[1])) + self.mouse_position["text"] = " (x, y, val) = ({}, {}, {})".format(x,y,val) return @@ -186,17 +257,18 @@ self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 # クリックした座標の近くにあるオブジェクトを取得 clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) - if not clicked_obj: return # オブジェクト以外の領域 + if (not clicked_obj): return # オブジェクト以外の領域 id = clicked_obj[0] + if (id == self.finishpose_id): return tag = self.canvas.gettags(id) - if (len(tag) == 0): return - if (tag[0] == "origin"): return + if (len(tag) == 0) or (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) + self.message("Show selected waypoint information") return @@ -261,6 +333,7 @@ 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() + self.message("Apply changes of waypoint parameters") return @@ -278,6 +351,7 @@ elif btn["relief"] == tk.SUNKEN: btn["relief"] = tk.RAISED btn["background"] = "#EEE" + self.message("Drag & Drop to move waypoint") return @@ -290,6 +364,7 @@ self.current_waypoints['waypoints'].pop(wp_num-1) # ウェイポイントを削除 self.plot_waypoints() # ウェイポイントを再描画、waypoints_idを更新 self.close_wp_info() + self.message("Removed waypoint" + str(wp_num)) return @@ -393,7 +468,7 @@ """ - +++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++ + +++++ 右クリックしてポップアップメニューのadd waypoint hereをクリックしたときのコールバック関数 +++++ """ def add_waypoint(self): if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): @@ -420,12 +495,13 @@ y = int((self.canv_h - h) / 2) geometry = "{}x{}+{}+{}".format(w, h, x, y) self.add_wp_win.geometry(geometry) + self.message("Add waypoint") return """ - +++++ add waypoint hereをクリックして開いた別窓のボタンのコールバック +++++ + +++++ add waypoint hereをクリックして開いた別窓のAddボタンのコールバック +++++ """ def add_btn_callback(self): num_box = self.add_wp_win.grid_slaves(row=0, column=1)[0] @@ -480,11 +556,41 @@ """ + +++++ Ctrl押しながら左クリックしたときのコールバック +++++ + """ + def ctrl_left_click(self, event): + self.translate_mat(-event.x, -event.y) + self.scale_mat(1.2) + self.translate_mat(event.x, event.y) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom In") + return + + + + """ + +++++ Ctrl押しながら右クリックしたときのコールバック +++++ + """ + def ctrl_right_click(self, event): + self.translate_mat(-event.x, -event.y) + self.scale_mat(0.8) + self.translate_mat(event.x, event.y) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom Out") + return + + + + """ +++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++ """ def menu_save_clicked(self, event=None): self.save_waypoints(self.waypoint_filepath) - print("Saved changes!") + self.message("Saved changes!") return @@ -503,12 +609,21 @@ if len(new_filepath) == 0: return # cancel self.save_waypoints(new_filepath) self.waypoint_filepath = new_filepath - print("Save As", "\"", new_filepath, "\"") + self.message("Save As", "\"", new_filepath, "\"") return """ + +++++ Fileメニューの"Exit"がクリックされたときに実行されるコールバック関数 +++++ + """ + def menu_exit_clicked(self, event=None): + self.master.destroy() + + + + + """ +++++ 現在のウェイポイント情報を指定のファイルに書き込む +++++ """ def save_waypoints(self, path): @@ -539,7 +654,7 @@ """ - +++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++ + +++++ 同次変換行列(mat_affine)にx,yの平行移動を加える +++++ """ def translate_mat(self, x, y): mat = np.eye(3) @@ -551,7 +666,7 @@ """ - +++++ 引数のmat_affineにscale倍のリサイズを加えた同次変換行列を返す +++++ + +++++ mat_affineにscale倍のリサイズを加える +++++ """ def scale_mat(self, scale): mat = np.eye(3) @@ -563,7 +678,7 @@ """ - +++++ 元画像をaffne変換して描画、その画像を返す +++++ + +++++ 元画像をaffne変換して描画 +++++ """ def draw_image(self): mat_inv = np.linalg.inv(self.mat_affine) diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml index f8d6b5b..ec22d02 100644 --- a/waypoint_nav/param/waypoints.yaml +++ b/waypoint_nav/param/waypoints.yaml @@ -1,10 +1,10 @@ -waypoints: -- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.5} -- point: {x: 0.203745, y: -0.496466, z: 0.0, vel: 0.5, rad: 0.2} -- point: {x: 0.666450, y: 0.504773, z: 0.0, vel: 0.5, rad: 0.3} -- point: {x: 0.515219, y: 2.015219, z: 0.0, vel: 1.0, rad: 0.5} -finish_pose: - header: {seq: 0.0, stamp: 1.6203674303803937E9, frame_id: base_link} - pose: - position: {x: -0.550981, y: -0.550981, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} +waypoints: +- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.5} +- point: {x: 0.45187, y: -0.576395, z: 0.0, vel: 0.5, rad: 0.2} +- point: {x: 0.66645, y: 0.504773, z: 0.0, vel: 0.5, rad: 0.3} +- point: {x: 0.515219, y: 2.015219, z: 0.0, vel: 1.0, rad: 0.5} +finish_pose: + header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link} + pose: + position: {x: -0.550981, y: 2.010154, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} \ No newline at end of file