diff --git a/waypoint_manager/scripts/manager_GUI.py b/waypoint_manager/scripts/manager_GUI.py index 9bd2d1f..b9dab30 100755 --- a/waypoint_manager/scripts/manager_GUI.py +++ b/waypoint_manager/scripts/manager_GUI.py @@ -18,7 +18,6 @@ #### 画面上部のメニューを作成 #### 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) # 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) @@ -31,9 +30,19 @@ ) 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("", self.menu_save) #キーボードショートカットを設定 self.bind_all("", self.menu_saveas) self.bind_all("", self.menu_exit) + + 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) + self.show_menu.add_cascade(label="Footprint", menu=self.fp_menu) + self.menu_bar.add_cascade(label="View", menu=self.show_menu) + self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定 #### 画面上部に、システムからのメッセージを表示するラベルを配置 #### @@ -85,6 +94,8 @@ 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 @@ -201,6 +212,79 @@ with open(path, 'w') as f: f.write(get_waypoint_yaml(self.waypoints, self.finish_pose)) 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 + # 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(wp["x"], wp["y"], next_wp["x"], next_wp["y"]) + # last waypoint + create_footprint(next_wp["x"], 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 @@ -445,6 +529,7 @@ 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 @@ -545,14 +630,15 @@ def mouse_wheel(self, event): if not self.mymap: return if event.delta > 0: - # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 - self.mymap.scale_at(event.x, event.y, 1.1) + scale = 1.1 # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 else: - # 下に回転（タッチパッドなら上にドラッグ）=> 縮小 - self.mymap.scale_at(event.x, event.y, 0.9) + 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 @@ -582,7 +668,7 @@ # origin, waypoints finish_pose を平行移動 self.canvas.move("origin", delta_x, delta_y) if self.waypoints: - for id in self.waypoints.get_id_list(): + 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] @@ -621,10 +707,13 @@ """ def ctrl_left_click(self, event): if not self.mymap: return - self.mymap.scale_at(event.x, event.y, 1.2) + 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 @@ -634,10 +723,13 @@ """ def ctrl_right_click(self, event): if not self.mymap: return - self.mymap.scale_at(event.x, event.y, 0.8) + 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 @@ -702,4 +794,7 @@ 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クラスのインスタンス - app.mainloop() + try: + app.mainloop() + except KeyboardInterrupt as e: + print(e)