Newer
Older
waypoint_navigation / waypoint_manager / scripts / manager_GUI.py
@koki koki on 26 Oct 2022 34 KB fix
import tkinter as tk
import tkinter.filedialog
import math
import ruamel.yaml
from pathlib import Path
from lib.mymaplib import MyMap
from lib.waypointlib import WaypointList, FinishPose, get_waypoint_yaml




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.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=tk.DISABLED)
        self.file_menu.add_cascade(label="Open", menu=self.open_menu)
        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", 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, accelerator="Ctrl+Q")
        self.menu_bar.add_cascade(label="File", menu=self.file_menu)   # Fileメニューとしてバーに追加
        self.bind_all("<Control-s>", self.menu_save)   #キーボードショートカットを設定
        self.bind_all("<Control-Shift-S>", self.menu_saveas)
        self.bind_all("<Control-q>", self.menu_exit)

        self.edit_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF)
        self.edit_menu.add_command(label="Set Finish Pose", command=self.menu_move_finishpose)
        self.menu_bar.add_cascade(label="Edit", menu=self.edit_menu)

        self.show_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF)
        self.fp_menu = tk.Menu(self.show_menu, tearoff=tk.OFF)
        self.fp_menu.add_command(label="Set model", command=self.menu_set_footprint)
        self.show_fp = tk.BooleanVar()
        self.fp_menu.add_checkbutton(label="Show footprint", command=self.menu_show_footprint, 
            variable=self.show_fp, accelerator="Ctrl+F"
        )
        self.show_menu.add_cascade(label="Footprint", menu=self.fp_menu)
        self.menu_bar.add_cascade(label="View", menu=self.show_menu)
        self.bind_all("<Control-f>", self.menu_show_footprint)

        self.master.configure(menu=self.menu_bar)   # 大元に作成したメニューバーを設定

        #### 画面上部に、システムからのメッセージを表示するラベルを配置 ####
        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) = ", 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, state=tk.DISABLED)
        self.right_click_coord = None   # 右クリックしたときの座標を保持する変数

        #### canvasを配置、サイズを取得 ####
        self.canvas = tk.Canvas(self.master, bg="#034")   # 画像を描画するcanvas
        self.canvas.pack(expand=True, fill=tk.BOTH)   # canvasを配置
        self.update()   # 情報の更新をする(canvasのサイズなどの情報が更新される)
        self.canv_w = self.canvas.winfo_width()   # canvasの幅を取得
        self.canv_h = self.canvas.winfo_height()   # canvasの高さを取得

        #### イベントに対するコールバックを設定 ####
        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.mymap = None
        self.waypoints = None
        self.finish_pose = None
        self.waypoints_filepath = None
        self.editing_waypoint_id = None   # 編集中のウェイポイントを示す図形のオブジェクトID
        self.moving_waypoint = False   # ウェイポイントをDnDで動かしている最中かどうか
        self.setting_finish_pose = 0 # finish pose のセット中かどうか
        self.old_click_point = None   # 最後にカーソルのあった座標を保持
        self.wp_info_win = None   # ウェイポイント情報を表示するウィンドウ
        self.point_rad = 10   # 画像上に示すポイントの半径ピクセル
        self.footprint = [[0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4]]
        self.footprint_id = []
        return



    """
    ###############################
        メニューコールバック関数
    ###############################
    """
    """
    +++++ File -> Open -> Map +++++
    """
    def menu_open_map(self):
        map_path = tkinter.filedialog.askopenfilename(
            parent=self.master,
            title="Select map yaml file",
            initialdir=str(Path(".")),
            filetypes=[("YAML", ".yaml")]
        )
        if not map_path: return

        with open(map_path) as file:   # .yamlを読み込む
            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.mymap.width() / self.mymap.height()):
            # canvasの方が横長 画像をcanvasの縦に合わせてリサイズ
            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.mymap.width()
            offset_y = (self.canv_h - self.mymap.height()*scale) / 2
        
        self.mymap.scale_at(0, 0, scale)
        self.mymap.translate(offset_x, offset_y)
        self.draw_image()   # 画像を描画
        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 ")
        self.menu_open_waypoints()
        return
    

    """
    +++++ File -> Open -> Waypionts +++++
    """
    def menu_open_waypoints(self):
        filepath = tkinter.filedialog.askopenfilename(
            parent=self.master,
            title="Select map yaml file",
            initialdir=str(Path(".")),
            filetypes=[("YAML", ".yaml")]
        )
        if not filepath: return
        with open(filepath) as file:
            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())
        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 menu_save(self, event=None):
        self.save_waypoints(str(self.waypoints_filepath))
        self.message("Saved changes!")
        return


    """
    +++++  File -> Save As +++++
    """
    def menu_saveas(self, event=None):
        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)
        current_title = self.master.title()
        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 +++++
    """
    def menu_exit(self, event=None):
        self.master.destroy()


    """
    +++++  現在のウェイポイント情報を指定のファイルに書き込む +++++
    """
    def save_waypoints(self, path):
        with open(path, 'w') as f:
            f.write(get_waypoint_yaml(self.waypoints, self.finish_pose))
        return
    

    """
    +++++  Edit -> Set Finish Pose +++++
    """
    def menu_move_finishpose(self, event=None):
        self.message("Click any point to set finsih pose")
        self.setting_finish_pose = 1
        return


    """
    +++++  View -> Footprint -> Set +++++
    """
    def menu_set_footprint(self, event=None):
        sub_win = tk.Toplevel()
        sub_win.title("Set footprint model")
        sub_win.protocol("WM_DELETE_WINDOW")
        sub_win.attributes('-topmost', True)  # サブウィンドウを最前面で固定
        # テキストボックス
        footprint = str(self.footprint)
        txt_box = tk.Entry(sub_win, width=len(footprint)+10, font=("Consolas",12))
        txt_box.insert(tk.END, footprint)
        txt_box.pack(padx=10, pady=20)
        # Set ボタンコールバック
        def callback():
            try:
                self.footprint = eval(txt_box.get())
                if self.show_fp.get():
                    self.show_fp.set(False)
                    self.menu_show_footprint()
                self.show_fp.set(True)
                self.menu_show_footprint()
                sub_win.destroy()
            except SyntaxError:
                sub_win.title("Unexpectd format")
            return
        # Set ボタン
        set_btn = tk.Button(sub_win, text="Set", width=5, height=1, command=callback)
        set_btn.pack(pady=10)
        sub_win.update()
        w = sub_win.winfo_width()
        h = sub_win.winfo_height()
        x = int((self.canv_w - w) / 2)
        y = int((self.canv_h - h) / 2)
        geometry = "{}x{}+{}+{}".format(w, h, x, y)
        sub_win.geometry(geometry)
        return
    

    """
    +++++  View -> Footprint -> Show +++++
    """
    def menu_show_footprint(self, event=None):
        if not self.waypoints: return
        if event: self.show_fp.set(not self.show_fp.get())
        # create polygon as footprint
        def create_footprint(x, y, nx, ny):
            polygon = []
            th = math.atan2((ny - y), (nx -x))
            for xy in self.footprint:
                X = xy[0]*math.cos(th) - xy[1]*math.sin(th) + x
                Y = xy[0]*math.sin(th) + xy[1]*math.cos(th) + y
                cx, cy = self.real2canvas(float(X), float(Y))
                polygon.append([cx, cy])
            id = self.canvas.create_polygon(polygon, fill="", outline="green")
            self.canvas.lower(id)
            self.canvas.lift(id, "map_image")
            self.footprint_id.append(id)
            return
        ##
        if (self.show_fp.get()):
            for i in range(len(self.waypoints.get_waypoint()) - 1):
                wp = self.waypoints.get_waypoint(num=i+1)
                next_wp = self.waypoints.get_waypoint(num=i+2)
                create_footprint(float(wp["x"]), float(wp["y"]), float(next_wp["x"]), float(next_wp["y"]))
            # last waypoint
            create_footprint(float(next_wp["x"]), float(next_wp["y"]), self.finish_pose.x, self.finish_pose.y)
        else:
            for id in self.footprint_id:
                self.canvas.delete(id)
            self.footprint_id = []
        return



    """
    #################################
        原点とウェイポイントの表示
    #################################
    """
    """
    +++++ 地図上の原点に円を描画 +++++
    """
    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, "<Button-1>", lambda event, wp_id=id: self.waypoint_clicked(event, wp_id))
        self.canvas.tag_bind(id, "<Enter>", lambda event, wp_id=id: self.waypoint_enter(event, wp_id))
        self.canvas.tag_bind(id, "<Leave>", self.waypoint_leave)
        self.canvas.tag_bind(id, "<B1-Motion>", 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.moving_waypoint = False
            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: dict = self.waypoints.get_waypoint(id=id)
        if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()):
            # 既にウィンドウが表示されている場合、一度削除
            self.wp_info_win.destroy()
        # ウィンドウが表示されてない場合、初期化
        self.wp_info_win = tk.Toplevel()
        self.wp_info_win.lower()
        self.wp_info_win.protocol("WM_DELETE_WINDOW", self.close_wp_info)
        # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置
        label_width = max([len(key) for key in point.keys()]) + 2
        for i, key in enumerate(point.keys()):
            key_label = tk.Label(self.wp_info_win, text=key+":", width=label_width, 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.canvas.winfo_x() + self.canv_w - w
        y = self.canvas.winfo_y() + self.canv_h - h
        self.wp_info_win.lift()
        self.wp_info_win.attributes('-topmost', True)  # サブウィンドウを最前面で固定
        self.wp_info_win.geometry("+{}+{}".format(x, y))
        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.moving_waypoint = False
        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
    


    """
    ###############################
        イベントコールバック関数
    ###############################
    """
    """
    +++++ キャンバス内でマウスを動かしたとき +++++
    """
    def mouse_move(self, event):
        if not self.mymap: return

        if self.setting_finish_pose == 2:
            x0, y0, _, _ = self.canvas.coords("set_finish_pose")
            x, y = event.x, event.y
            theta = math.atan2((-y+y0), (x-x0))
            x1 = x0 + math.cos(theta) * self.point_rad * 3
            y1 = y0 - math.sin(theta) * self.point_rad * 3
            self.canvas.delete("set_finish_pose")
            self.canvas.create_line(x0, y0, x1, y1, tags="set_finish_pose",
                width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#F88"
            )
        
        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
    

    """
    +++++ マウスホイールを回転したとき(タッチパッドをドラッグしたとき) +++++
    """
    def mouse_wheel(self, event):
        if not self.mymap: return
        if event.delta > 0:
            scale = 1.1   # 上に回転(タッチパッドなら下にドラッグ)=> 拡大
        else:
            scale = 0.9   # 下に回転(タッチパッドなら上にドラッグ)=> 縮小
        self.mymap.scale_at(event.x, event.y, scale)
        self.draw_image()
        self.plot_origin()
        self.plot_waypoints()
        for id in self.footprint_id:
            self.canvas.scale(id, event.x, event.y, scale, scale)
        return
    

    """
    +++++ 左クリックされたとき +++++
    """
    def left_click(self, event):
        self.popup_menu.unpost()   # 右クリックで出るポップアップメニューを非表示

        if self.setting_finish_pose == 1:
            x0, y0 = event.x, event.y
            x1 = x0 + math.cos(self.finish_pose.yaw) * self.point_rad * 3
            y1 = y0 - math.sin(self.finish_pose.yaw) * self.point_rad * 3
            self.canvas.create_line(x0, y0, x1, y1, tags="set_finish_pose",
                width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#F88"
            )
            self.setting_finish_pose = 2
            return
        
        if self.setting_finish_pose == 2:
            x0, y0, _, _ = self.canvas.coords("set_finish_pose")
            x, y = event.x, event.y
            theta = math.atan2((-y+y0), (x-x0))
            x1 = x0 + math.cos(theta) * self.point_rad * 3
            y1 = y0 - math.sin(theta) * self.point_rad * 3
            self.canvas.delete("set_finish_pose")
            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"
            )
            img_x, img_y = self.mymap.inv_transform(x0, y0)
            real_x, real_y = self.mymap.image2real(img_x, img_y)
            self.finish_pose.x = real_x
            self.finish_pose.y = real_y
            self.finish_pose.yaw = theta
            self.setting_finish_pose = 0
            self.message("New finish pose is setted.")
        return
    

    """
    +++++ マウスを左クリックしながらドラッグしたとき +++++
    """
    def left_click_move(self, event):
        if not self.mymap: return
        if self.moving_waypoint: return
        if self.setting_finish_pose != 0: 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)
        if self.waypoints:
            for id in list(self.waypoints.get_id_list()) + self.footprint_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.mymap: return
        if self.setting_finish_pose != 0: 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
        if self.setting_finish_pose != 0: return
        scale = 1.2
        self.mymap.scale_at(event.x, event.y, scale)
        self.draw_image()
        self.plot_origin()
        self.plot_waypoints()
        for id in self.footprint_id:
            self.canvas.scale(id, event.x, event.y, scale, scale)
        self.message("Zoom In")
        return


    """
    +++++ Ctrl押しながら右クリックしたとき +++++
    """
    def ctrl_right_click(self, event):
        if not self.mymap: return
        if self.setting_finish_pose != 0: return
        scale = 0.8
        self.mymap.scale_at(event.x, event.y, scale)
        self.draw_image()
        self.plot_origin()
        self.plot_waypoints()
        for id in self.footprint_id:
            self.canvas.scale(id, event.x, event.y, scale, scale)
        self.message("Zoom Out")
        return
    

    """
    +++++ ウィンドウサイズが変更されたとき、情報を更新する +++++
    """
    def window_resize_callback(self, event):
        cw = self.canvas.winfo_width()
        ch = self.canvas.winfo_height()
        if (self.canv_w != cw) or (self.canv_h != ch):
            self.canv_w = cw
            self.canv_h = ch
            self.draw_image()
        return



    """
    ##############################################
        地図画像の表示、メッセージ表示、座標変換
    ##############################################
    """
    """
    +++++ 元画像をaffne変換して描画 +++++
    """
    def draw_image(self):
        if not self.mymap: return
        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 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__":
    root = tk.Tk()   # 大元になるウィンドウ
    w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-100
    root.geometry("%dx%d+0+0" % (w, h))
    app = Application(master=root)   # tk.Frameを継承したApplicationクラスのインスタンス
    try:
        app.mainloop()
    except KeyboardInterrupt as e:
        app.destroy()