Newer
Older
waypoint_navigation / waypoint_manager / scripts / manager_GUI.py
@koki0624 koki0624 on 14 Nov 2022 45 KB update
import tkinter as tk
import tkinter.filedialog
import math
import ruamel.yaml
import gc
from pathlib import Path
from tkinter import messagebox
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_set_finishpose)
        self.edit_menu.add_command(label="From To", command=self.menu_from_to)
        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, font=("Consolas",13))
        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=("Consolas", 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=("Consolas", 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: tk.Toplevel = 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)
        if  not "image" in map_yaml.keys():
            messagebox.showerror(title="Format error", message="Selected map file is unexpected format.")
            return
        try:
            del self.mymap
            self.mymap = MyMap(Path(map_path), map_yaml)
        except FileNotFoundError:
            messagebox.showerror(title="Image file is not found", message="\""+map_yaml["image"]+"\"  is not found.")
            self.mymap = None
            return
        self.message("Read map file " + map_path)
        self.canvas.delete("all")
        if self.waypoints is not None: self.waypoints.number_dict = {}
        ## キャンバスサイズに合わせて画像を表示
        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()   # 原点を示す円を描画
        if self.waypoints_filepath is not None:
            self.master.title(self.waypoints_filepath.name + " - " + Path(map_path).name)
        else:
            self.master.title(Path(map_path).name)
        self.open_menu.entryconfigure("Waypoints", state=tk.NORMAL)
        if self.waypoints is None:
            self.message("Please open waypoints file ")
            self.menu_open_waypoints()
        else:
            self.plot_waypoints()
        gc.collect()
        return
    

    """
    +++++ File -> Open -> Waypionts +++++
    """
    def menu_open_waypoints(self):
        if (self.waypoints is not None) and (self.master.title()[0] == "*"):
            yn = messagebox.askyesno("Confirm", "Do you want to save changes to " + str(self.waypoints_filepath))
            if yn: self.save_waypoints(str(self.waypoints_filepath)) # Yes
            self.master.title(str(self.master.title()).replace("* " + self.waypoints_filepath.name + " - ", ""))
        elif (self.waypoints is not None):
            self.master.title(str(self.master.title()).replace(self.waypoints_filepath.name + " - ", ""))

        self.canvas.delete("all")
        self.draw_image()
        self.plot_origin()
        if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()):
            self.wp_info_win.destroy()

        filepath = tkinter.filedialog.askopenfilename(
            parent=self.master,
            title="Select waypoints yaml file",
            initialdir=str(Path(".")),
            filetypes=[("YAML", ".yaml")]
        )
        if not filepath: return
        with open(filepath) as file:
            wp_yaml = ruamel.yaml.YAML().load(file)
        if (not "waypoints" in wp_yaml.keys()) or (not "finish_pose" in wp_yaml.keys()):
            messagebox.showerror(title="Format error", message="Selected waypoints file is unexpected format.")
            return
        del self.waypoints
        self.waypoints = WaypointList(wp_yaml)
        self.finish_pose = FinishPose(wp_yaml)
        self.waypoints_filepath = Path(filepath)
        self.plot_waypoints()
        self.master.title(self.waypoints_filepath.name + " - " + self.master.title())
        self.message("Read waypoints file " + filepath)
        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)
        gc.collect()
        return
    

    """
    +++++ File -> Save +++++
    """
    def menu_save(self, event=None):
        if not self.waypoints: return
        self.save_waypoints(str(self.waypoints_filepath))
        self.message("Saved changes!")
        title = self.master.title()
        if title[0] == "*": self.master.title(str(title).replace("* ", ""))
        return


    """
    +++++  File -> Save As +++++
    """
    def menu_saveas(self, event=None):
        if not self.waypoints: return
        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) + "\"")
        title = self.master.title()
        if title[0] == "*": self.master.title(str(title).replace("* ", ""))
        return


    """
    +++++  File -> Exit +++++
    """
    def menu_exit(self, event=None):
        title = self.master.title()
        if title[0] != "*":
            self.master.destroy()
            return
        res = messagebox.askyesnocancel(title="Unsaved changes", message="Do you want to save changes before close?")
        if res is None: return # cancel
        if res == True: self.menu_save()
        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_set_finishpose(self, event=None):
        if not self.waypoints: return
        self.message("Click any point to set finsih pose. If you want to cancel, right click anywhere.")
        self.setting_finish_pose = 1
        return
    

    """
    +++++  Edit -> From To +++++
    """
    def menu_from_to(self, event=None):
        if not self.waypoints: return
        sub_win = tk.Toplevel()
        sub_win.title("Set parameter from some number's waypoint to any number's one")
        sub_win.attributes('-topmost', True)  # サブウィンドウを最前面で固定
        #
        font = ("Consolas", 12)
        frame1 = tk.Frame(sub_win)
        from_label = tk.Label(frame1, text="From No.", font=font)
        from_label.grid(row=0, column=0)
        from_txt_box = tk.Entry(frame1, width=4, font=font)
        from_txt_box.grid(row=0, column=1)
        to_label = tk.Label(frame1, text="to No.", font=font)
        to_label.grid(row=0, column=2)
        to_txt_box = tk.Entry(frame1, width=4, font=font)
        to_txt_box.grid(row=0, column=3)
        frame1.pack(padx=10, pady=10)
        #
        frame2 = tk.Frame(sub_win)
        name_label = tk.Label(frame2, text="Parameter", font=font)
        name_label.grid(row=0, column=0)
        value_label=tk.Label(frame2, text="Value", font=font)
        value_label.grid(row=0, column=1)
        name_txt_box = tk.Entry(frame2, width=20, font=font)
        name_txt_box.grid(row=1, column=0, padx=20)
        value_txt_box = tk.Entry(frame2, width=20, font=font)
        value_txt_box.grid(row=1, column=1, padx=20)
        frame2.pack(padx=10, pady=10)
        #
        def set_btn_callback():
            from_num = from_txt_box.get()
            to_num = to_txt_box.get()
            if (len(from_num) == 0) or (len(to_num) == 0):
                messagebox.showwarning(title="Warning", message="Enter waypoint numbers.")
                return
            from_num = int(from_num)
            to_num = int(to_num)
            if (from_num < 1) or (to_num > len(self.waypoints.get_waypoint())):
                messagebox.showerror(title="Error", message="Entered number is out of range of the number of waypoints")
                return
            param = name_txt_box.get()
            value = value_txt_box.get()
            if (len(param) == 0) or (len(value) == 0):
                messagebox.showwarning(title="Warning", message="Enter parameter name and value")
                return
            for n in range(from_num, to_num+1):
                self.waypoints.waypoints[n-1][param] = value
            sub_win.destroy()
            self.update_title()
            self.plot_waypoints()
            return
        frame3 = tk.Frame(sub_win)
        set_btn = tk.Button(frame3, text="Set", width=5, command=set_btn_callback)
        set_btn.pack(side=tk.RIGHT, padx=50, pady=10)
        cancel_btn = tk.Button(frame3, text="Cancel", width=8, command=sub_win.destroy)
        cancel_btn.pack(side=tk.LEFT, padx=50, pady=10)
        frame3.pack(padx=10, pady=20, expand=True, fill=tk.X)
        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):
        img_x, img_y = self.mymap.real2image(float(waypoint["x"]), float(waypoint["y"]))
        cx, cy = self.mymap.transform(img_x, img_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)
        if (img_x < 0) or (img_y < 0) or (img_x > self.mymap.width()) or (img_y > self.mymap.height()):
            id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FEE', outline='#FAA', activefill='#F88')
            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)
        else:
            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: tk.Entry = 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]
        self.update_title()
        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)
            del_btn = tk.Button(self.wp_info_win, text="-", width=2, bg="red", fg="white", font=("",13,"bold"))
            del_btn["command"] = lambda name=key, val=str(point[key]).lower(): self.del_param_btn_callback(name, val)
            del_btn.grid(column=2, row=i, padx=5, pady=5)
        # New parameter
        new_param_btn = tk.Button(self.wp_info_win, text="New Parameter", bg="#AFA")
        new_param_btn["command"] = self.new_param_btn_callback
        new_param_btn.grid(column=0, columnspan=2, row=self.wp_info_win.grid_size()[1], pady=10)
        # Apply, DnD(Drag & Drop), remove ボタン
        canv = tk.Canvas(self.wp_info_win)
        canv.grid(column=0, columnspan=3, row=self.wp_info_win.grid_size()[1], sticky=tk.EW, pady=5)
        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")
        self.update_title()
        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")
        self.update_title()
        return
    

    """
    +++++ New Parameter ボタンを押したときのコールバック +++++
    """
    def new_param_btn_callback(self):
        # Sub window
        sub_win = tk.Toplevel()
        sub_win.title("Add new parameter")
        sub_win.attributes('-topmost', True)
        font = ("Consolas",15)
        # Message label
        label = tk.Label(sub_win, text="Name : Value", font=font, anchor=tk.CENTER, width=20)
        label.grid(row=0, column=0, columnspan=3, padx=10, pady=10)
        # Text box
        name_entry = tk.Entry(sub_win, font=font, width=15)
        name_entry.grid(column=0, row=1, padx=10, pady=5)
        colon = tk.Label(sub_win, text=":", font=font, anchor=tk.CENTER, width=2)
        colon.grid(row=1, column=1)
        value_entry = tk.Entry(sub_win, font=font, width=15)
        value_entry.grid(column=2, row=1, padx=10, pady=5)
        # Buttons
        def add_param():
            name = name_entry.get()
            value = value_entry.get()
            self.waypoints.set_waypoint_val(self.editing_waypoint_id, key=name, val=value)
            sub_win.destroy()
            self.wp_info_win.destroy()
            self.disp_waypoint_info(self.editing_waypoint_id)
            self.update_title()
            gc.collect()
            return
        add_btn = tk.Button(sub_win, text="Add", width=5, height=1, font=font, anchor=tk.CENTER, bg="#AFA")
        add_btn["command"] = add_param
        add_btn.grid(column=2, row=2, pady=20)
        cancel_btn = tk.Button(sub_win, text="Cancel", width=7, height=1, font=font, anchor=tk.CENTER)
        cancel_btn["command"] = sub_win.destroy
        cancel_btn.grid(column=0, row=2, pady=20)
        # Window position
        sub_win.update()
        w = sub_win.winfo_width() + 10
        h = sub_win.winfo_height()
        x = int((self.canv_w - w) / 2)
        y = int((self.canv_h - h) / 2)
        sub_win.geometry("{}x{}+{}+{}".format(w, h, x, y))
        return
    

    def del_param_btn_callback(self, name, val):
        msg = "Are you sure you want to delete this parameter?\n\n"
        msg += str(name) + ": " + str(val)
        yn = messagebox.askyesno("Delete parameter", message=msg)
        if (yn == True):
            self.waypoints.delete_waypoint_param(self.editing_waypoint_id, name)
            self.wp_info_win.destroy()
            self.disp_waypoint_info(self.editing_waypoint_id)
            self.update_title()
        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()
        gc.collect()
        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_x, img_y = self.right_click_coord
        if self.mymap.pil_img.getpixel((img_x, img_y))[0] == 0:
            messagebox.showwarning(title="Warning", 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 new waypoint at 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):
        num = num_box.get()
        if (num == ""):
            win.attributes('-topmost', False)
            messagebox.showwarning(title="Warning", message="The number has not been entered.")
            win.attributes('-topmost', True)
            return
        num = int(num)
        if (num < 0) or (num > len(self.waypoints.waypoints)+1):
            win.attributes('-topmost', False)
            messagebox.showwarning(title="Warning", message="The number is out of range.")
            win.attributes('-topmost', True)
            return
        win.destroy()
        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(num, point, id=id)
        self.plot_waypoints(id=id)
        self.editing_waypoint_id = id
        self.canvas.itemconfig(id, fill='red')
        self.disp_waypoint_info(id)
        self.update_title()
        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
            img_x, img_y = self.mymap.inv_transform(x0, y0)
            if (img_x < 0) or (img_y < 0) or (img_x > self.mymap.width()) or (img_y > self.mymap.height()):
                retry = messagebox.askretrycancel(title="Cannot set finish pose here", message="The point is out of the map.")
                if not retry: self.setting_finish_pose = 0
                return
            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 set.")
            self.update_title()
        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 == 1:
            self.setting_finish_pose = 0
            self.message("Canceled.")
            return
        if self.setting_finish_pose == 2:
            self.canvas.delete("set_finish_pose")
            self.setting_finish_pose = 0
            self.message("Canceled.")
            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)))
        self.canvas.tag_lower("map_image")
        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)
        cx, cy = self.mymap.transform(img_x, img_y)
        return cx, cy
    

    def update_title(self):
        title = self.master.title()
        if title[0] != "*": 
            self.master.title("* "+title)






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クラスのインスタンス
    root.protocol("WM_DELETE_WINDOW", app.menu_exit)
    try:
        app.mainloop()
    except KeyboardInterrupt as e:
        app.menu_exit()