diff --git a/waypoint_manager/scripts/None b/waypoint_manager/scripts/None deleted file mode 100644 index 3463e52..0000000 --- a/waypoint_manager/scripts/None +++ /dev/null @@ -1,11 +0,0 @@ -waypoints: -- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.3} -- point: {x: -0.188321, y: -0.510442, z: 0.0, vel: 1.0, rad: 0.3} -- point: {x: 0.568284, y: -0.501916, z: 0.0, vel: 0.8, rad: 0.2} -- point: {x: 0.631463, y: 0.530001, z: 0.0, vel: 0.5, rad: 0.3} -- point: {x: 0.629175, y: 1.860692, z: 0.0, vel: 0.5, rad: 0.2} -finish_pose: - header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link} - pose: - position: {x: -0.550981, y: 1.85, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} \ No newline at end of file diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index 9912315..05ae823 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -1,79 +1,70 @@ -#import rospy 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 +from lib.mymaplib import MyMap +from lib.waypointlib import WaypointList, FinishPose, get_waypoint_yaml -#===== Applicationクラスの定義　tk.Frameクラスを継承 =====# + + class Application(tk.Frame): - """ - +++++ コンストラクタ +++++ - プログラムを実行したときに最初にすべき処理を全て記述する - """ def __init__(self, master): super().__init__(master) # スーパークラスのコンストラクタを実行 self.master.title("Waypoints Manager") - ## 画面上部のメニューを作成 + #### 画面上部のメニューを作成 #### self.menu_bar = tk.Menu(self) # メニューバーを配置 self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) # バーに追加するメニューを作成 self.menu_bar.add_cascade(label="File", menu=self.file_menu) # Fileメニューとしてバーに追加 - self.open_menu = tk.Menu(self.file_menu, tearoff=tk.OFF) + self.open_menu = tk.Menu(self.file_menu, tearoff=tk.OFF) # Openメニュー self.open_menu.add_command(label="Map", command=self.menu_open_map) - self.open_menu.add_command(label="Waypoints", command=self.menu_open_waypoints, state="disabled") + self.open_menu.add_command(label="Waypoints", command=self.menu_open_waypoints, state=tk.DISABLED) self.file_menu.add_cascade(label="Open", menu=self.open_menu) - - self.file_menu.add_command(label="Save", # FileメニューにSaveコマンドを追加 - command=self.menu_save_clicked, #コールバック関数を設定 - accelerator="Ctrl+S"# 右側に表示するキーボードショートカット + self.file_menu.add_command(label="Save", command=self.menu_save, #コールバック関数を設定 + accelerator="Ctrl+S", state=tk.DISABLED ) - self.file_menu.add_command(label="Save As", # 同様にSave Asコマンドを追加 - command=self.menu_saveas_clicked, - accelerator="Ctrl+Shift+S" + self.file_menu.add_command(label="Save As", command=self.menu_saveas, + accelerator="Ctrl+Shift+S", state=tk.DISABLED ) 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.file_menu.add_command(label="Exit", command=self.menu_exit, accelerator="Ctrl+Q") + self.bind_all("", self.menu_save) #キーボードショートカットを設定 + self.bind_all("", self.menu_saveas) + self.bind_all("", self.menu_exit) 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.msg_label = tk.Label(self.master, text="Please open map file ", anchor=tk.E) + self.msg_label.pack(fill=tk.X, padx=5) - ## 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 + #### 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 #### self.status_bar = tk.Frame(self.master) - self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, text="(x, y, val) = ", - width=40, anchor=tk.W, font=("", 15)) - self.mouse_position.pack(side=tk.LEFT, padx=1) - self.waypoint_num = tk.Label(self.status_bar, relief=tk.SUNKEN, text="Waypoint No. -----", - width=20, anchor=tk.W, font=("", 15)) - self.waypoint_num.pack(side=tk.LEFT, padx=1) + self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, + text=" (x, y) = ", anchor=tk.W, font=("", 15) + ) + self.mouse_position.pack(side=tk.LEFT, padx=3) + self.waypoint_num = tk.Label(self.status_bar, relief=tk.SUNKEN, + text=" Waypoint No. -----", anchor=tk.W, font=("", 15) + ) + self.waypoint_num.pack(side=tk.RIGHT, padx=3) 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_here) + self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint_here, state=tk.DISABLED) self.right_click_coord = None # 右クリックしたときの座標を保持する変数 - ## canvasを配置 - self.canvas = tk.Canvas(self.master, background="#AAA") # 画像を描画するcanvas + #### canvasを配置、サイズを取得 #### + self.canvas = tk.Canvas(self.master, bg="#034") # 画像を描画するcanvas self.canvas.pack(expand=True, fill=tk.BOTH) # canvasを配置 self.update() # 情報の更新をする(canvasのサイズなどの情報が更新される) - - ## 画像をcanvasのサイズにフィッティングして描画 self.canv_w = self.canvas.winfo_width() # canvasの幅を取得 self.canv_h = self.canvas.winfo_height() # canvasの高さを取得 - ## マウスイベントに対するコールバックを設定 + #### イベントに対するコールバックを設定 #### self.master.bind("", self.mouse_move) self.master.bind("", self.mouse_wheel) self.master.bind("", self.left_click_move) @@ -82,33 +73,31 @@ 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.__map_img_pil = None - self.__map_yaml = None - self.map_resolution = 0.05 - self.img_origin = [] - self.mat_affine = np.eye(3) # 同次変換行列 - self.draw_img_tk = None - self.__waypoints = None - self.waypoint_filepath = None - self.current_waypoints = None - self.waypoints_id = np.array([], np.uint16) - self.wplabel_id = np.array([], np.uint16) + #### その他必要になる変数 #### + self.mymap = None + self.waypoints = None self.finish_pose = None - self.finishpose_id = None - self.old_click_point = None # 最後にカーソルのあった座標を保持 - self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ + self.waypoints_filepath = None self.editing_waypoint_id = None # 編集中のウェイポイントを示す図形のオブジェクトID self.moving_waypoint = False # ウェイポイントをDnDで動かしている最中かどうか - self.add_wp_win = None # add waypoint hereをクリックしたときに表示するウィンドウ - self.point_rad = 10 + self.old_click_point = None # 最後にカーソルのあった座標を保持 + self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ + self.point_rad = 10 # 画像上に示すポイントの半径ピクセル return + """ + ############################### + メニューコールバック関数 + ############################### + """ + + """ + +++++ File -> Open -> Map +++++ + """ def menu_open_map(self): map_path = tkinter.filedialog.askopenfilename( parent=self.master, @@ -119,39 +108,35 @@ if not map_path: return with open(map_path) as file: # .yamlを読み込む - self.__map_yaml = ruamel.yaml.YAML().load(file) - # .pgmをplillowで読み込む - self.__map_img_pil = Image.open(Path(map_path).with_name(self.__map_yaml["image"]).resolve()) - # 画像を表示 + map_yaml = ruamel.yaml.YAML().load(file) + self.mymap = MyMap(Path(map_path), map_yaml) + ## キャンバスサイズに合わせて画像を表示 scale = 1 offset_x = 0 offset_y = 0 - if (self.canv_w / self.canv_h) > (self.__map_img_pil.width / self.__map_img_pil.height): + if (self.canv_w / self.canv_h) > (self.mymap.width() / self.mymap.height()): # canvasの方が横長　画像をcanvasの縦に合わせてリサイズ - scale = self.canv_h / self.__map_img_pil.height - offset_x = (self.canv_w - self.__map_img_pil.width*scale) / 2 + scale = self.canv_h / self.mymap.height() + offset_x = (self.canv_w - self.mymap.width()*scale) / 2 else: # canvasの方が縦長　画像をcanvasの横に合わせてリサイズ - scale = self.canv_w / self.__map_img_pil.width - offset_y = (self.canv_h - self.__map_img_pil.height*scale) / 2 + scale = self.canv_w / self.mymap.width() + offset_y = (self.canv_h - self.mymap.height()*scale) / 2 - self.scale_mat(scale) # 同次変換行列を更新 - self.translate_mat(offset_x, offset_y) # 同次変換行列を更新 - self.draw_img_tk = ImageTk.PhotoImage(image=self.__map_img_pil) # 描画する画像を保持する変数を初期化 + self.mymap.scale_at(0, 0, scale) + self.mymap.translate(offset_x, offset_y) self.draw_image() # 画像を描画 - ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 - origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m) - self.map_resolution = self.__map_yaml['resolution'] - # 画像上の原点座標を保持（左上から横x, 縦y） - self.img_origin = [-origin[0]/self.map_resolution, self.__map_img_pil.height+origin[1]/self.map_resolution, 1] - self.plot_origin() + self.plot_origin() # 原点を示す円を描画 self.master.title(Path(map_path).name + " - " + self.master.title()) self.open_menu.entryconfigure("Map", state=tk.DISABLED) self.open_menu.entryconfigure("Waypoints", state=tk.NORMAL) + self.message("Please open waypoints file ") return - + """ + +++++ File -> Open -> Waypionts +++++ + """ def menu_open_waypoints(self): filepath = tkinter.filedialog.askopenfilename( parent=self.master, @@ -161,486 +146,31 @@ ) if not filepath: return with open(filepath) as file: - waypoints = ruamel.yaml.YAML().load(file) - self.__waypoints = waypoints - self.current_waypoints = waypoints - self.waypoint_filepath = Path(filepath) + wp_yaml = ruamel.yaml.YAML().load(file) + self.waypoints = WaypointList(wp_yaml) + self.finish_pose = FinishPose(wp_yaml) + self.waypoints_filepath = Path(filepath) self.plot_waypoints() self.master.title(Path(filepath).name + " - " + self.master.title()) - return - - - - """ - +++++ 画面上部にメッセージを表示する +++++ - """ - def message(self, msg): - if not isinstance(msg, str): - msg = str(msg) - self.msg_label["text"] = str(msg) - - - - """ - +++++ 地図上の原点に円を描画する +++++ - """ - def plot_origin(self): - origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換 - r = self.point_rad # 円の半径（ピクセル） - 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", x0, y0) - else: - self.canvas.create_oval(x0, y0, x1, y1, tags="origin", fill='cyan', outline='blue') - return - - - - """ - +++++ 地図上にウェイポイントを示す円を描画する +++++ - """ - def plot_waypoints(self, id=None): - if not self.__waypoints: return - points = self.current_waypoints['waypoints'] # ウェイポイントのリスト - r = self.point_rad - # 引数にidが指定された場合、そのポイントのみを再描画して終了 - if id: - wp_num = np.where(self.waypoints_id == id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 - img_x = float(points[wp_num-1]['point']['x']) / self.map_resolution + self.img_origin[0] - img_y = -float(points[wp_num-1]['point']['y']) / self.map_resolution + self.img_origin[1] - xy_affine = np.dot(self.mat_affine, [img_x, img_y, 1]) - x0 = xy_affine[0] - r - y0 = xy_affine[1] - r - self.canvas.moveto(id, x0, y0) - return - # ウェイポイントの追加または削除が行なわれている場合、初期化 - if len(self.waypoints_id) != len(points): - for id in self.waypoints_id: - self.canvas.delete(id) - self.waypoints_id = np.array([], np.uint16) - # 全てのウェイポイントを描画 - 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]) - 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(x0, y0, x1, y1, fill='#FDD', outline='red', activefill='red') - self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_clicked(event, wp_id)) - self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_enter(event, wp_id)) - self.canvas.tag_bind(id, "", self.waypoint_leave) - self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_click_move(event, wp_id)) - self.waypoints_id = np.append(self.waypoints_id, id) - else: - id = self.waypoints_id[i] - 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, tags="finish_pose", - arrow=tk.LAST, arrowshape=(12,15,9), fill="#AAF") - return - - - - """ - +++++ ウェイポイントが左クリックされたときのコールバック +++++ - """ - def waypoint_clicked(self, event, wp_id): - if wp_id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え - self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') - self.editing_waypoint_id = wp_id - self.canvas.itemconfig(wp_id, fill='red') - self.disp_waypoint_info(wp_id) - self.message("Show selected waypoint information") + self.file_menu.entryconfigure("Save", state=tk.NORMAL) + self.file_menu.entryconfigure("Save As", state=tk.NORMAL) + self.popup_menu.entryconfigure("Add waypoint here", state=tk.NORMAL) return - """ - +++++ ウェイポイントを左クリックしながら動かしたときのコールバック +++++ + +++++ File -> Save +++++ """ - def waypoint_click_move(self, event, wp_id): - if not self.moving_waypoint: 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] - 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 - img_x, img_y = self.canvas2image(px, py) - # マップ画像上の座標を、実際の座標に変換 - x, y = self.image2real(img_x, img_y) - # 編集中のウェイポイント情報を更新 - 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 - - - """ - +++++ ウェイポイントを示す円にカーソルが入ったときと出たときのコールバック +++++ - """ - def waypoint_enter(self, event, wp_id): - wp_num = np.where(self.waypoints_id == wp_id)[0][0] + 1 # ウェイポイントの番号を取得 - self.waypoint_num["text"] = "Waypoint No. " + str(wp_num) - - def waypoint_leave(self, event): - self.waypoint_num["text"] = "Waypoint No. -----" - - - - """ - +++++ キャンバス内でマウスを動かしたときのコールバック +++++ - """ - def mouse_move(self, event): - if not self.__map_img_pil: return - img_x, img_y = self.canvas2image(event.x, event.y) - if (img_x < 0) or (img_y < 0) or (img_x > self.__map_img_pil.width) or (img_y > self.__map_img_pil.height): - self.mouse_position["text"] = " Out of map" - return - x, y = self.image2real(img_x, img_y) - val = self.__map_img_pil.getpixel((img_x, img_y)) - self.mouse_position["text"] = " (x, y, val) = ({}, {}, {})".format(x, y, val) - return - - - - """ - +++++ 左クリックされたときに実行されるコールバック関数 +++++ - """ - def left_click(self, event): - self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 - 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) # サブウィンドウを最前面で固定 - # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置 - for i, key in enumerate(point): - key_label = tk.Label(self.wp_info_win, text=key+":", width=6, 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") - dnd_btn["command"] = lambda obj=dnd_btn: self.dnd_btn_callback(dnd_btn) - 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(id=self.editing_waypoint_id) - self.message("Apply changes of waypoint parameters") - return - - - - """ - +++++ ドラッグ&ドロップボタン(DnD)を押したときのコールバック +++++ - """ - def dnd_btn_callback(self, obj=None): - if obj is None: return - btn = obj - # 押された状態とそうでない状態を切り替える - if btn["relief"] == tk.RAISED: - btn["relief"] = tk.SUNKEN - btn["background"] = "#AAA" - self.moving_waypoint = True - self.message("Drag & Drop to move waypoint") - elif btn["relief"] == tk.SUNKEN: - btn["relief"] = tk.RAISED - btn["background"] = "#EEE" - self.moving_waypoint = False - self.message("Show selected waypoint information") - 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.canvas.delete(self.editing_waypoint_id) # ウェイポイントを示す円を削除 - self.waypoints_id = np.delete(self.waypoints_id, wp_num-1) # waypoints_idから削除 - self.close_wp_info() - self.message("Removed waypoint" + str(wp_num)) - 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 - - - - """ - +++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++ - """ - def left_click_move(self, event): - if not self.__map_img_pil: return - if self.moving_waypoint: 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] - # ウェイポイント移動モードでないとき、地図を平行移動 - self.translate_mat(delta_x, delta_y) - self.draw_image() - # origin, waypoints finish_pose を平行移動 - self.canvas.move("origin", delta_x, delta_y) - for id in self.waypoints_id: - self.canvas.move(id, delta_x, delta_y) - self.canvas.move("finish_pose", delta_x, delta_y) - self.old_click_point = [event.x, event.y] - return - - - - """ - +++++ マウスの左クリックを離したときのコールバック関数 +++++ - """ - def left_click_release(self, event): - self.old_click_point = None - return - - - - """ - +++++ 右クリックしたときのコールバック関数 +++++ - """ - def right_click(self, event): - if not self.__map_img_pil: return - # クリックした座標の近くにあるオブジェクトを取得 - clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) - if clicked_obj: # 何かオブジェクトがクリックされていた場合 - return - # クリックされた座標 => 元画像の座標 の変換 - img_x, img_y = self.canvas2image(event.x, event.y) - # 変換後の元画像の座標がサイズから外れている場合（地図画像の外をクリックしている） - if (img_x < 0) or (img_y < 0) or (img_x > self.__map_img_pil.width) or (img_y > self.__map_img_pil.height): - return - self.popup_menu.post(event.x_root, event.y_root) # メニューをポップアップ - self.right_click_coord = [img_x, img_y] # クリックされた元画像上の座標を変数に格納 - return - - - - """ - +++++ 右クリックしてポップアップメニューのadd waypoint hereをクリックしたときのコールバック関数 +++++ - """ - def add_waypoint_here(self): - 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: - self.message("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) - 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() - if (num == ""): - self.message("Please enter number") - return - self.add_wp_win.destroy() - num = int(num) - img_xy = self.right_click_coord - # ウェイポイント座標を計算 - x, y = self.image2real(img_xy[0], img_xy[1]) - # ウェイポイントを追加 - data = CommentedMap() - data['point'] = CommentedMap() - for key in self.current_waypoints['waypoints'][0]['point']: - if (key == 'x'): v = x - elif (key == 'y'): v = y - else: v = 0.0 - data['point'][key] = v - 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 - - - - """ - +++++ マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 +++++ - --- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも --- - """ - def mouse_wheel(self, event): - if not self.__map_img_pil: return - self.translate_mat(-event.x, -event.y) - if event.delta > 0: - # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 - self.scale_mat(1.1) - else: - # 下に回転（タッチパッドなら上にドラッグ）=> 縮小 - self.scale_mat(0.9) - self.translate_mat(event.x, event.y) - self.draw_image() - self.plot_origin() - self.plot_waypoints() - return - - - - """ - +++++ Ctrl押しながら左クリックしたときのコールバック +++++ - """ - def ctrl_left_click(self, event): - if not self.__map_img_pil: return - 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): - if not self.__map_img_pil: return - 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(str(self.waypoint_filepath)) + def menu_save(self, event=None): + self.save_waypoints(str(self.waypoints_filepath)) self.message("Saved changes!") return - """ - +++++ Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 +++++ + +++++ File -> Save As +++++ """ - def menu_saveas_clicked(self, event=None): + def menu_saveas(self, event=None): new_filepath = tkinter.filedialog.asksaveasfilename( parent=self.master, title="Save As", @@ -651,96 +181,151 @@ if len(new_filepath) == 0: return # cancel self.save_waypoints(new_filepath) current_title = self.master.title() - old_filename = self.waypoint_filepath.name - self.waypoint_filepath = Path(new_filepath) - self.master.title(current_title.replace(old_filename, self.waypoint_filepath.name)) + old_filename = self.waypoints_filepath.name + self.waypoints_filepath = Path(new_filepath) + self.master.title(current_title.replace(old_filename, self.waypoints_filepath.name)) self.message("Save As" + "\"" + str(new_filepath) + "\"") return - """ - +++++ Fileメニューの"Exit"がクリックされたときに実行されるコールバック関数 +++++ + +++++ File -> Exit +++++ """ - def menu_exit_clicked(self, event=None): + def menu_exit(self, event=None): self.master.destroy() - - """ +++++ 現在のウェイポイント情報を指定のファイルに書き込む +++++ """ 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"]) + "}") + f.write(get_waypoint_yaml(self.waypoints, self.finish_pose)) return """ - +++++ 同次変換行列(mat_affine)にx,yの平行移動を加える +++++ + ############################### + イベントコールバック関数 + ############################### """ - def translate_mat(self, x, y): - mat = np.eye(3) - mat[0, 2] = float(x) - mat[1, 2] = float(y) - self.mat_affine = np.dot(mat, self.mat_affine) + + """ + +++++ キャンバス内でマウスを動かしたとき +++++ + """ + def mouse_move(self, event): + if not self.mymap: return + img_x, img_y = self.mymap.inv_transform(event.x, event.y) + if (img_x < 0) or (img_y < 0) or (img_x > self.mymap.width()) or (img_y > self.mymap.height()): + self.mouse_position["text"] = " Out of map " + return + x, y = self.mymap.image2real(img_x, img_y) + self.mouse_position["text"] = " ( x, y ) = ( {}, {} ) ".format(x, y) return - """ - +++++ mat_affineにscale倍のリサイズを加える +++++ + +++++ マウスホイールを回転したとき（タッチパッドをドラッグしたとき） +++++ """ - def scale_mat(self, scale): - mat = np.eye(3) - mat[0, 0] = scale - mat[1, 1] = scale - self.mat_affine = np.dot(mat, self.mat_affine) - return - - - - """ - +++++ 元画像をaffne変換して描画 +++++ - """ - def draw_image(self): - mat_inv = np.linalg.inv(self.mat_affine) - img = self.__map_img_pil.transform( - (self.canv_w, self.canv_w), - Image.Transform.AFFINE, tuple(mat_inv.flatten()), - Image.Resampling.NEAREST, - fillcolor = 160 - ) - self.draw_img_tk = ImageTk.PhotoImage(image=img) # 描画する画像を変数に保持 - if not self.canvas.find_withtag("map_image"): # 初めて画像を描画するとき - self.canvas.create_image(0, 0, anchor='nw', image=self.draw_img_tk, tags="map_image") # 画像の描画 + def mouse_wheel(self, event): + if not self.mymap: return + if event.delta > 0: + # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 + self.mymap.scale_at(event.x, event.y, 1.1) else: - self.canvas.itemconfig("map_image", image=self.draw_img_tk) # 既に描画された画像を差し替える + # 下に回転（タッチパッドなら上にドラッグ）=> 縮小 + self.mymap.scale_at(event.x, event.y, 0.9) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + return + + + """ + +++++ 左クリックされたとき +++++ + """ + def left_click(self, event): + self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 + return + + + """ + +++++ マウスを左クリックしながらドラッグしたとき +++++ + """ + def left_click_move(self, event): + if not self.mymap: return + if self.moving_waypoint: 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] + # ウェイポイント移動モードでないとき、地図を平行移動 + self.mymap.translate(delta_x, delta_y) + self.draw_image() + # origin, waypoints finish_pose を平行移動 + self.canvas.move("origin", delta_x, delta_y) + for id in self.waypoints.get_id_list(): + self.canvas.move(id, delta_x, delta_y) + self.canvas.move("finish_pose", delta_x, delta_y) + self.old_click_point = [event.x, event.y] + return + + + """ + +++++ マウスの左クリックを離したとき +++++ + """ + def left_click_release(self, event): + self.old_click_point = None + return + + + """ + +++++ 右クリックしたとき +++++ + """ + def right_click(self, event): + if not self.mymap: return + # クリックした座標の近くにあるオブジェクトを取得 + clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) + if clicked_obj: # 何かオブジェクトがクリックされていた場合 + return + # クリックされた座標 => 元画像の座標 の変換 + img_x, img_y = self.mymap.inv_transform(event.x, event.y) + # 変換後の元画像の座標がサイズから外れている場合（地図画像の外をクリックしている） + if (img_x < 0) or (img_y < 0) or (img_x > self.mymap.width()) or (img_y > self.mymap.height()): + return + self.popup_menu.post(event.x_root, event.y_root) # メニューをポップアップ + self.right_click_coord = [img_x, img_y] # クリックされた元画像上の座標を変数に格納 + return + + + """ + +++++ Ctrl押しながら左クリックしたとき +++++ + """ + def ctrl_left_click(self, event): + if not self.mymap: return + self.mymap.scale_at(event.x, event.y, 1.2) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom In") return + """ + +++++ Ctrl押しながら右クリックしたとき +++++ + """ + def ctrl_right_click(self, event): + if not self.mymap: return + self.mymap.scale_at(event.x, event.y, 0.8) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom Out") + return + """ +++++ ウィンドウサイズが変更されたとき、情報を更新する +++++ @@ -756,31 +341,365 @@ """ - +++++ キャンバス上（ウィンドウ上）の座標から、元の地図画像上の座標に変換 +++++ + ################################# + 原点とウェイポイントの表示 + ################################# """ - def canvas2image(self, cx, cy): - mat_inv = np.linalg.inv(self.mat_affine) - img_xy = np.dot(mat_inv, [cx, cy, 1]) - return img_xy[0], img_xy[1] + + """ + +++++ 地図上の原点に円を描画 +++++ + """ + def plot_origin(self): + x, y = self.mymap.transform(self.mymap.img_origin[0], self.mymap.img_origin[1]) + r = self.point_rad # 円の半径（ピクセル） + x0 = x - r + y0 = y - r + x1 = x + r + 1 + y1 = y + r + 1 + if self.canvas.find_withtag("origin"): + self.canvas.moveto("origin", x0, y0) + else: + self.canvas.create_oval(x0, y0, x1, y1, tags="origin", fill='cyan', outline='blue') + return + + + """ + +++++ 地図上にウェイポイントとフィニッシュポーズを描画 +++++ + """ + def plot_waypoints(self, id=None): + if not self.waypoints: return + # 引数にidが指定された場合、そのポイントのみを再描画して終了 + if id: + wp = self.waypoints.get_waypoint(id) + cx, cy = self.real2canvas(float(wp["x"]), float(wp["y"])) + x0 = cx - self.point_rad + y0 = cy - self.point_rad + self.canvas.moveto(id, round(x0), round(y0)) + return + + if len(self.waypoints.get_id_list()) == 0: + # 初めて描画する場合 + for n, wp in enumerate(self.waypoints.get_waypoint()): + id = self.create_waypoint(wp) + self.waypoints.set_id(num=n+1, id=id) + else: + # 既に描画されている場合 + for id in self.waypoints.get_id_list(): + wp = self.waypoints.get_waypoint(id) + cx, cy = self.real2canvas(float(wp["x"]), float(wp["y"])) + x0 = cx - self.point_rad + y0 = cy - self.point_rad + self.canvas.moveto(id, round(x0), round(y0)) + # Finish poseを描画 + cx, cy = self.real2canvas(self.finish_pose.x, self.finish_pose.y) + x0 = cx + y0 = cy + x1 = x0 + math.cos(self.finish_pose.yaw) * self.point_rad * 3 + y1 = y0 - math.sin(self.finish_pose.yaw) * self.point_rad * 3 + if self.finish_pose.id is not None: + # movetoだと上手くいかないので、毎回削除、再描画 + self.canvas.delete(self.finish_pose.id) + self.finish_pose.id = self.canvas.create_line(x0, y0, x1, y1, tags="finish_pose", + width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#AAF" + ) + return + + + """ + +++++ キャンバスに新たなウェイポイントを描画する +++++ + """ + def create_waypoint(self, waypoint: dict): + cx, cy = self.real2canvas(float(waypoint["x"]), float(waypoint["y"])) + x0 = round(cx - self.point_rad) + y0 = round(cy - self.point_rad) + x1 = round(cx + self.point_rad + 1) + y1 = round(cy + self.point_rad + 1) + id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FDD', outline='red', activefill='red') + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_clicked(event, wp_id)) + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_enter(event, wp_id)) + self.canvas.tag_bind(id, "", self.waypoint_leave) + self.canvas.tag_bind(id, "", self.waypoint_click_move) + return id + + + """ + +++++ ウェイポイントが左クリックされたときのコールバック +++++ + """ + def waypoint_clicked(self, event, wp_id): + if wp_id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え + self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') + self.editing_waypoint_id = wp_id + self.canvas.itemconfig(wp_id, fill='red') + self.disp_waypoint_info(wp_id) + self.message("Show selected waypoint information") + return + + + """ + +++++ ウェイポイントを左クリックしながら動かしたときのコールバック +++++ + """ + def waypoint_click_move(self, event): + if not self.moving_waypoint: 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] + 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 + img_x, img_y = self.mymap.inv_transform(px, py) + # マップ画像上の座標を、実際の座標に変換 + x, y = self.mymap.image2real(img_x, img_y) + # 編集中のウェイポイント情報を更新 + self.waypoints.set_waypoint_val(self.editing_waypoint_id, "x", x) + self.waypoints.set_waypoint_val(self.editing_waypoint_id, "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 + + + """ + +++++ ウェイポイントを示す円にカーソルが入ったときと出たときのコールバック +++++ + """ + def waypoint_enter(self, event, wp_id): + wp_num = self.waypoints.get_num(wp_id) + self.waypoint_num["text"] = " Waypoint No. {} " .format(str(wp_num)) + + def waypoint_leave(self, event): + self.waypoint_num["text"] = " Waypoint No. ----- " + + + + """ + ###################################### + 別窓でのウェイポイントの情報表示 + ###################################### + """ + + """ + +++++ ウェイポイントが左クリックされたとき、別窓で情報を表示する関数 +++++ + """ + def disp_waypoint_info(self, id): + point = self.waypoints.get_waypoint(id=id) + 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.withdraw() + self.wp_info_win.protocol("WM_DELETE_WINDOW", self.close_wp_info) + self.wp_info_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 + # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置 + for i, key in enumerate(point.keys()): + key_label = tk.Label(self.wp_info_win, text=key+":", width=6, 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, str(point[key]).lower()) + txt_box.grid(column=1, row=i, padx=2, pady=2, ipady=3, sticky=tk.EW) + # Apply, DnD(Drag & Drop), remove ボタン + 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, bg="#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, bg="#EEE") + dnd_btn["command"] = lambda obj=dnd_btn: self.dnd_btn_callback(dnd_btn) + dnd_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) + remove_btn = tk.Button(canv, text="Remove", width=7, height=1, bg="#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) + self.wp_info_win.deiconify() + 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, str(point[key]).lower()) + self.wp_info_win.title("Waypoint " + str(self.waypoints.get_num(id))) # タイトルを設定 + return + + + """ + +++++ Applyボタンを押したときのコールバック +++++ + """ + def apply_btn_callback(self): + point = self.waypoints.get_waypoint(id=self.editing_waypoint_id) + for i, key in enumerate(point.keys()): + txt_box = self.wp_info_win.grid_slaves(column=1, row=i)[0] + self.waypoints.set_waypoint_val(self.editing_waypoint_id, key, txt_box.get()) + self.plot_waypoints(id=self.editing_waypoint_id) + self.message("Apply changes of waypoint parameters") + return + + + """ + +++++ ドラッグ&ドロップボタン(DnD)を押したときのコールバック +++++ + """ + def dnd_btn_callback(self, obj=None): + if obj is None: return + btn = obj + # 押された状態とそうでない状態を切り替える + if btn["relief"] == tk.RAISED: + btn["relief"] = tk.SUNKEN + btn["bg"] = "#AAA" + self.moving_waypoint = True + self.message("Drag & Drop to move waypoint") + elif btn["relief"] == tk.SUNKEN: + btn["relief"] = tk.RAISED + btn["bg"] = "#EEE" + self.moving_waypoint = False + self.message("Show selected waypoint information") + return + + + """ + +++++ removeボタンを押したときのコールバック +++++ + """ + def remove_btn_callback(self): + self.waypoints.remove(self.editing_waypoint_id) + self.canvas.delete(self.editing_waypoint_id) # ウェイポイントを示す円を削除 + self.close_wp_info() + self.message("Removed waypoint") + 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 + + + + """ + ########################### + ウェイポイントの追加 + ########################### + """ + """ + +++++ 右クリックしてポップアップメニューのadd waypoint hereをクリックしたときのコールバック関数 +++++ + """ + def add_waypoint_here(self): + 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.mymap.pil_img.getpixel((img_xy[0], img_xy[1]))[0] == 0: + self.message("There is obstacles") + return + # 何番目のウェイポイントの次に追加するか入力させる + add_wp_win = tk.Toplevel() + add_wp_win.title("Add waypoint") + add_wp_win.protocol("WM_DELETE_WINDOW") + add_wp_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 + msg = tk.Label(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(add_wp_win, width=4, font=("Consolas",15)) + txt_box.grid(column=1, row=0, pady=10, sticky=tk.W) + # Add ボタン + add_btn = tk.Button(add_wp_win, text="Add", width=5, height=1, + command=lambda num_box=txt_box, win=add_wp_win: self.add_btn_callback(num_box, win) + ) + add_btn.grid(column=0, columnspan=2, row=1, padx=10, pady=10) + add_wp_win.update() + w = add_wp_win.winfo_width() + 10 + h = 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) + add_wp_win.geometry(geometry) + self.message("Add waypoint") + return + + + """ + +++++ add waypoint hereをクリックして開いた別窓のAddボタンのコールバック +++++ + """ + def add_btn_callback(self, num_box: tk.Entry, win: tk.Toplevel): + prev_num = num_box.get() + if (prev_num == ""): + self.message("Please enter number") + return + win.destroy() + prev_num = int(prev_num) + img_xy = self.right_click_coord + # ウェイポイント座標を計算 + x, y = self.mymap.image2real(img_xy[0], img_xy[1]) + # ウェイポイントを追加 + point = {} + for key in self.waypoints.point_keys: + if (key=="x"): point[key] = x + elif (key=="y"): point[key] = y + elif (key=="z"): point[key] = 0.0 + else: point[key] = "" + id = self.create_waypoint(point) + self.waypoints.insert(prev_num+1, point, id=id) + self.plot_waypoints(id=id) + self.editing_waypoint_id = id + self.canvas.itemconfig(id, fill='red') + self.disp_waypoint_info(id) + return + + + + """ + ############################################## + 地図画像の表示、メッセージ表示、座標変換 + ############################################## + """ + + """ + +++++ 元画像をaffne変換して描画 +++++ + """ + def draw_image(self): + if not self.canvas.find_withtag("map_image"): # 初めて画像を描画するとき + self.canvas.create_image(0, 0, anchor='nw', tags="map_image", + image=self.mymap.get_draw_image((self.canv_w, self.canv_h)) + ) + else: + # 既に描画された画像を差し替える + self.canvas.itemconfig("map_image", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) + return + + + """ + +++++ 画面上部にメッセージを表示する +++++ + """ + def message(self, msg): + if not isinstance(msg, str): + msg = str(msg) + self.msg_label["text"] = str(msg) - """ - +++++ 元の地図画像上の座標から、実際の座標に変換 +++++ - """ - def image2real(self, ix, iy): - x = (ix - self.img_origin[0]) * self.map_resolution - y = (-iy + self.img_origin[1]) * self.map_resolution - x = round(x, 6) - y = round(y, 6) - return x, y + def real2canvas(self, x, y): + img_x, img_y = self.mymap.real2image(x,y) + real_x, real_y = self.mymap.transform(img_x, img_y) + return real_x, real_y -#===== メイン処理　プログラムはここから実行される =====# + + + if __name__ == "__main__": - #rospy.init_node("manager_GUI") root = tk.Tk() # 大元になるウィンドウ - w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-100 + w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-300 root.geometry("%dx%d+0+0" % (w, h)) app = Application(master=root) # tk.Frameを継承したApplicationクラスのインスタンス app.mainloop() diff --git a/waypoint_manager/scripts/lib/__init__.py b/waypoint_manager/scripts/lib/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/waypoint_manager/scripts/lib/__init__.py diff --git a/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc b/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000..ec9801f --- /dev/null +++ b/waypoint_manager/scripts/lib/__pycache__/__init__.cpython-38.pyc Binary files differ diff --git a/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc b/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc new file mode 100644 index 0000000..b55e1d3 --- /dev/null +++ b/waypoint_manager/scripts/lib/__pycache__/mymaplib.cpython-38.pyc Binary files differ diff --git a/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc b/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc new file mode 100644 index 0000000..dcfb453 --- /dev/null +++ b/waypoint_manager/scripts/lib/__pycache__/waypointlib.cpython-38.pyc Binary files differ diff --git a/waypoint_manager/scripts/lib/mymaplib.py b/waypoint_manager/scripts/lib/mymaplib.py new file mode 100644 index 0000000..cfdf093 --- /dev/null +++ b/waypoint_manager/scripts/lib/mymaplib.py @@ -0,0 +1,82 @@ +import numpy as np +from PIL import Image, ImageTk +from pathlib import Path + + +class MyMap(): + + def __init__(self, path: Path, map_yaml): + img_path = path.with_name(map_yaml["image"]).resolve() + self.pil_img = Image.open(img_path).convert("RGBA") + self.tk_img = ImageTk.PhotoImage(self.pil_img) + self.origin = map_yaml["origin"] + self.resolution = map_yaml["resolution"] + self.img_origin = [-self.origin[0]/self.resolution, self.origin[1]/self.resolution+self.height()] + self.mat_affine = np.eye(3) + return + + + def translate(self, x, y): + self.affine_tf(x, y, 1) + return + + + def scale_at(self, x, y, scale): + self.affine_tf(-x, -y, 1) + self.affine_tf(0, 0, scale) + self.affine_tf(x, y, 1) + return + + + def affine_tf(self, x, y, scale): + x = float(x) + y = float(y) + scale = float(scale) + mat = np.array([[scale,0,x], [0,scale,y], [0,0,1]]) + self.mat_affine = np.dot(mat, self.mat_affine) + return + + + def get_draw_image(self, canvas_size): + mat_inv = np.linalg.inv(self.mat_affine) + img = self.pil_img.transform(canvas_size, + Image.Transform.AFFINE, tuple(mat_inv.flatten()), + Image.Resampling.NEAREST, + fillcolor="#0000" + ) + self.tk_img = ImageTk.PhotoImage(image=img) + return self.tk_img + + + def real2image(self, real_x, real_y): + img_x = self.img_origin[0] + real_x / self.resolution + img_y = self.img_origin[1] - real_y / self.resolution + return img_x, img_y + + + def image2real(self, img_x, img_y): + real_x = (img_x - self.img_origin[0]) * self.resolution + real_y = -(img_y - self.img_origin[1]) * self.resolution + real_x = round(real_x, 6) + real_y = round(real_y, 6) + return real_x, real_y + + + def transform(self, img_x, img_y): + mat = [img_x, img_y, 1] + tf_mat = np.dot(self.mat_affine, mat) + return tf_mat[0], tf_mat[1] + + + def inv_transform(self, x, y): + mat = [x, y, 1] + inv_affine = np.linalg.inv(self.mat_affine) + inv = np.dot(inv_affine, mat) + return inv[0], inv[1] + + + def width(self): + return self.pil_img.width + + def height(self): + return self.pil_img.height \ No newline at end of file diff --git a/waypoint_manager/scripts/lib/waypointlib.py b/waypoint_manager/scripts/lib/waypointlib.py new file mode 100644 index 0000000..ab54d33 --- /dev/null +++ b/waypoint_manager/scripts/lib/waypointlib.py @@ -0,0 +1,118 @@ +import numpy as np +import quaternion + + +class WaypointList(): + def __init__(self, wp_yaml): + self.waypoints = [] + for point in wp_yaml["waypoints"]: + wp = {} + for key, val in point["point"].items(): + wp[key] = val + self.append(wp) + + self.point_keys = self.waypoints[0].keys() + self.number_dict = {} + return + + + def append(self, waypoint: dict, id=None): + self.waypoints.append(waypoint) + if id: self.set_id(len(self.waypoints), id) + return len(self.waypoints) + + + def insert(self, num, waypoint: dict, id=None): + self.waypoints.insert(num-1, waypoint) + for i, n in self.number_dict.items(): + self.number_dict[i] = n+1 if (n>=num) else n + if id: self.set_id(num, id) + return + + + def remove(self, id): + num = self.get_num(str(id)) + self.waypoints.pop(num-1) + self.number_dict.pop(str(id)) + for i, n in self.number_dict.items(): + self.number_dict[i] = n-1 if (n>num) else n + return + + + def set_waypoint_val(self, id, key, val): + self.waypoints[self.get_num(id)-1][key] = val + return + + + def get_waypoint(self, id=None, num=None): + if id: return self.waypoints[self.get_num(str(id)) - 1] + if num: return self.waypoints[num-1] + return self.waypoints + + + def get_id_list(self): + return self.number_dict.keys() + + + def set_id(self, num, id): + self.number_dict[str(id)] = num + return + + + def get_num(self, id): + return self.number_dict[str(id)] + + + + +class FinishPose(): + def __init__(self, wp_yaml): + super().__init__() + + self.header = {} + for key, val in wp_yaml["finish_pose"]["header"].items(): + self.header[key] = val + + self.position = {} + for key, val in wp_yaml["finish_pose"]["pose"]["position"].items(): + self.position[key] = val + + self.orientation = {} + for key, val in wp_yaml["finish_pose"]["pose"]["orientation"].items(): + self.orientation[key] = val + + self.x = self.position["x"] + self.y = self.position["y"] + self.yaw = self.get_euler() + self.id = None + return + + + def get_euler(self): + o = self.orientation + q = np.quaternion(o["x"], o["y"], o["z"], o["w"]) + return quaternion.as_euler_angles(q)[1] + + + +def get_waypoint_yaml(waypoints: WaypointList, finish_pose: FinishPose): + s = ["waypoints:" + "\n"] + for point in waypoints.get_waypoint(): + s.append("- point: {") + for i, (key, val) in enumerate(point.items()): + if (i != 0): s.append(", ") + s.append(key + ": " + str(val).lower()) + s.append("}" + "\n") + + s.append("finish_pose:" + "\n") + seq, stamp, frame = (finish_pose.header["seq"], finish_pose.header["stamp"], finish_pose.header["frame_id"]) + s.append(" header: {" + "seq: {}, stamp: {}, frame_id: {}".format(seq,stamp,frame) + "}" + "\n") + s.append(" pose:" + "\n") + p = finish_pose.position + s.append(" position: {" + "x: {}, y: {}, z: {}".format(p["x"], p["y"], p["z"]) + "}" + "\n") + o = finish_pose.orientation + s.append(" orientation: {" + "x: {}, y: {}, z: {}, w: {}".format(o["x"],o["y"],o["z"],o["w"]) + "}") + return "".join(s) + + + \ No newline at end of file