| |
|---|
| | from textwrap import fill |
|---|
| | 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クラスを継承 =====# |
|---|
| | class Application(tk.Frame): |
|---|
| |
|---|
| | self.file_menu.add_command(label="Save As", # 同様にSave Asコマンドを追加 |
|---|
| | 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("<Control-s>", self.menu_save_clicked) #キーボードショートカットを設定 |
|---|
| | self.bind_all("<Control-Shift-S>", self.menu_saveas_clicked) |
|---|
| | self.bind_all("<Control-q>", 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) |
|---|
| |
|---|
| | self.plot_origin() |
|---|
| | |
|---|
| | ## 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("<Motion>", self.mouse_move) |
|---|
| | self.master.bind("<MouseWheel>", self.mouse_wheel) |
|---|
| | self.master.bind("<B1-Motion>", self.left_click_move) |
|---|
| | self.master.bind("<Button-1>", self.left_click) |
|---|
| | self.master.bind("<ButtonRelease-1>", self.left_click_release) |
|---|
| | self.master.bind("<Button-3>", self.right_click) |
|---|
| | self.master.bind("<Control-Button-1>", self.ctrl_left_click) |
|---|
| | self.master.bind("<Control-Button-3>", self.ctrl_right_click) |
|---|
| | ## ウィンドウに関するコールバック |
|---|
| | self.master.bind("<Configure>", 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 |
|---|
| | ## その他必要になる変数の初期化 |
|---|
| | 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) |
|---|
| | self.master.title(map_path.name + " - " + self.master.title()) |
|---|
| |
|---|
| | +++++ waypointsのパスを受け取り読み込んだデータを返す +++++ |
|---|
| | --- これも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()) |
|---|
| | return waypoints, file_path |
|---|
| |
|---|
| | """ |
|---|
| | 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 |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ 地図上にウェイポイントを示す円を描画する +++++ |
|---|
| | """ |
|---|
| | def plot_waypoints(self): |
|---|
| | print(self.canvas.find_all()) |
|---|
| | points = self.current_waypoints['waypoints'] # ウェイポイントのリスト |
|---|
| | r = self.point_rad |
|---|
| | # ウェイポイントの追加または削除が行なわれている場合、初期化 |
|---|
| | if len(self.waypoints_id) != len(points): |
|---|
| |
|---|
| | for i in range(len(points)): |
|---|
| | 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 |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| |
|---|
| | def left_click(self, event): |
|---|
| | 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 |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| |
|---|
| | 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() |
|---|
| | self.message("Apply changes of waypoint parameters") |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| |
|---|
| | btn["background"] = "#AAA" |
|---|
| | elif btn["relief"] == tk.SUNKEN: |
|---|
| | btn["relief"] = tk.RAISED |
|---|
| | btn["background"] = "#EEE" |
|---|
| | return |
|---|
| | self.message("Drag & Drop to move waypoint") |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| |
|---|
| | 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 |
|---|
| | self.message("Removed waypoint" + str(wp_num)) |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++ |
|---|
| | +++++ 右クリックしてポップアップメニューのadd waypoint hereをクリックしたときのコールバック関数 +++++ |
|---|
| | """ |
|---|
| | def add_waypoint(self): |
|---|
| | if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): |
|---|
| | self.close_wp_info() |
|---|
| |
|---|
| | 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をクリックして開いた別窓のボタンのコールバック +++++ |
|---|
| | self.message("Add waypoint") |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ add waypoint hereをクリックして開いた別窓のAddボタンのコールバック +++++ |
|---|
| | """ |
|---|
| | def add_btn_callback(self): |
|---|
| | num_box = self.add_wp_win.grid_slaves(row=0, column=1)[0] |
|---|
| | num = num_box.get() |
|---|
| |
|---|
| | self.plot_origin() |
|---|
| | self.plot_waypoints() |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ 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!") |
|---|
| | return |
|---|
| | self.message("Saved changes!") |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| |
|---|
| | ) |
|---|
| | if len(new_filepath) == 0: return # cancel |
|---|
| | self.save_waypoints(new_filepath) |
|---|
| | self.waypoint_filepath = new_filepath |
|---|
| | print("Save As", "\"", new_filepath, "\"") |
|---|
| | return |
|---|
| | self.message("Save As", "\"", new_filepath, "\"") |
|---|
| | return |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ Fileメニューの"Exit"がクリックされたときに実行されるコールバック関数 +++++ |
|---|
| | """ |
|---|
| | def menu_exit_clicked(self, event=None): |
|---|
| | self.master.destroy() |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++ |
|---|
| | +++++ 同次変換行列(mat_affine)にx,yの平行移動を加える +++++ |
|---|
| | """ |
|---|
| | def translate_mat(self, x, y): |
|---|
| | mat = np.eye(3) |
|---|
| | mat[0, 2] = float(x) |
|---|
| |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ 引数のmat_affineにscale倍のリサイズを加えた同次変換行列を返す +++++ |
|---|
| | +++++ mat_affineにscale倍のリサイズを加える +++++ |
|---|
| | """ |
|---|
| | def scale_mat(self, scale): |
|---|
| | mat = np.eye(3) |
|---|
| | mat[0, 0] = scale |
|---|
| |
|---|
| | |
|---|
| | |
|---|
| | |
|---|
| | """ |
|---|
| | +++++ 元画像をaffne変換して描画、その画像を返す +++++ |
|---|
| | +++++ 元画像をaffne変換して描画 +++++ |
|---|
| | """ |
|---|
| | def draw_image(self): |
|---|
| | mat_inv = np.linalg.inv(self.mat_affine) |
|---|
| | img = self.__map_img_pil.transform( |
|---|
| |
|---|
| | |
