diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index 73bf1c0..1d40c7e 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -14,750 +14,750 @@ #===== Applicationクラスの定義　tk.Frameクラスを継承 =====# class Application(tk.Frame): - """ - +++++ コンストラクタ +++++ - プログラムを実行したときに最初にすべき処理を全て記述する - """ - def __init__(self, master): - super().__init__(master) # スーパークラスのコンストラクタを実行 - self.master.title("Waypoints Manager") + """ + +++++ コンストラクタ +++++ + プログラムを実行したときに最初にすべき処理を全て記述する + """ + 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.file_menu.add_command(label="Save", # FileメニューにSaveコマンドを追加 - command=self.menu_save_clicked, #コールバック関数を設定 - accelerator="Ctrl+S"# 右側に表示するキーボードショートカット - ) - self.file_menu.add_command(label="Save As", # 同様にSave Asコマンドを追加 - command=self.menu_saveas_clicked, - accelerator="Ctrl+Shift+S" - ) - self.file_menu.add_separator() - self.file_menu.add_command(label="Exit", command=self.menu_exit_clicked, accelerator="Ctrl+Q") - self.bind_all("", self.menu_save_clicked) #キーボードショートカットを設定 - self.bind_all("", self.menu_saveas_clicked) - self.bind_all("", self.menu_exit_clicked) - self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定 + ## 画面上部のメニューを作成 + 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.file_menu.add_command(label="Save", # FileメニューにSaveコマンドを追加 + command=self.menu_save_clicked, #コールバック関数を設定 + accelerator="Ctrl+S"# 右側に表示するキーボードショートカット + ) + self.file_menu.add_command(label="Save As", # 同様にSave Asコマンドを追加 + command=self.menu_saveas_clicked, + accelerator="Ctrl+Shift+S" + ) + self.file_menu.add_separator() + self.file_menu.add_command(label="Exit", command=self.menu_exit_clicked, accelerator="Ctrl+Q") + self.bind_all("", self.menu_save_clicked) #キーボードショートカットを設定 + self.bind_all("", self.menu_saveas_clicked) + self.bind_all("", self.menu_exit_clicked) + self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定 - ## 画面上部に、システムからのメッセージを表示するラベルを配置 - self.msg_label = tk.Label(self.master, text="Start up", anchor=tk.E) - self.msg_label.pack(fill=tk.X) + ## 画面上部に、システムからのメッセージを表示するラベルを配置 + self.msg_label = tk.Label(self.master, text="Start up", anchor=tk.E) + self.msg_label.pack(fill=tk.X) - ## 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 - self.status_bar = tk.Frame(self.master) - self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, text="(x, y, val) = ", - width=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.status_bar.pack(side=tk.BOTTOM, fill=tk.X) + ## 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 + self.status_bar = tk.Frame(self.master) + self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, text="(x, y, val) = ", + width=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.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.right_click_coord = None # 右クリックしたときの座標を保持する変数 + ## 右クリックしたときに表示するポップアップメニューを作成 + self.popup_menu = tk.Menu(self, tearoff=tk.OFF) + self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint_here) + self.right_click_coord = None # 右クリックしたときの座標を保持する変数 - ## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止 - self.__map_img_pil, self.__map_yaml = self.get_map_info() - self.__waypoints, self.waypoint_filepath = self.get_waypoints() - self.current_waypoints = self.__waypoints # 編集中のウェイポイント情報を保持する + ## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止 + self.__map_img_pil, self.__map_yaml = self.get_map_info() + self.__waypoints, self.waypoint_filepath = self.get_waypoints() + self.current_waypoints = self.__waypoints # 編集中のウェイポイント情報を保持する - ## canvasを配置 - self.canvas = tk.Canvas(self.master, background="#AAA") # 画像を描画するcanvas - self.canvas.pack(expand=True, fill=tk.BOTH) # canvasを配置 - self.update() # 情報の更新をする(canvasのサイズなどの情報が更新される) + ## canvasを配置 + self.canvas = tk.Canvas(self.master, background="#AAA") # 画像を描画する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.mat_affine = np.eye(3) # 同次変換行列の初期化 - 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): - # canvasの方が横長　画像をcanvasの縦に合わせてリサイズ - scale = self.canv_h / self.__map_img_pil.height - offset_x = (self.canv_w - self.__map_img_pil.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 - - self.scale_mat(scale) # 同次変換行列を更新 - self.translate_mat(offset_x, offset_y) # 同次変換行列を更新 - self.draw_img_tk = ImageTk.PhotoImage(image=self.__map_img_pil) # 描画する画像を保持する変数を初期化 - self.draw_image() # 画像を描画 + ## 画像をcanvasのサイズにフィッティングして描画 + self.canv_w = self.canvas.winfo_width() # canvasの幅を取得 + self.canv_h = self.canvas.winfo_height() # canvasの高さを取得 + self.mat_affine = np.eye(3) # 同次変換行列の初期化 + 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): + # canvasの方が横長　画像をcanvasの縦に合わせてリサイズ + scale = self.canv_h / self.__map_img_pil.height + offset_x = (self.canv_w - self.__map_img_pil.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 + + self.scale_mat(scale) # 同次変換行列を更新 + self.translate_mat(offset_x, offset_y) # 同次変換行列を更新 + self.draw_img_tk = ImageTk.PhotoImage(image=self.__map_img_pil) # 描画する画像を保持する変数を初期化 + self.draw_image() # 画像を描画 - ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 - self.point_rad = 10 - 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() + ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 + self.point_rad = 10 + 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() - ## waypoints.yamlからウェイポイント情報を取得し、画像にポイントを描画 - self.waypoints_id = np.array([], np.uint16) - self.wplabel_id = np.array([], np.uint16) - self.finish_pose = None - self.finishpose_id = None - self.plot_waypoints() + ## waypoints.yamlからウェイポイント情報を取得し、画像にポイントを描画 + self.waypoints_id = np.array([], np.uint16) + self.wplabel_id = np.array([], np.uint16) + self.finish_pose = None + self.finishpose_id = None + self.plot_waypoints() - ## マウスイベントに対するコールバックを設定 - self.master.bind("", self.mouse_move) - self.master.bind("", self.mouse_wheel) - self.master.bind("", self.left_click_move) - self.master.bind("", self.left_click) - self.master.bind("", self.left_click_release) - 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.master.bind("", self.mouse_move) + self.master.bind("", self.mouse_wheel) + self.master.bind("", self.left_click_move) + self.master.bind("", self.left_click) + self.master.bind("", self.left_click_release) + 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.old_click_point = None # 最後にカーソルのあった座標を保持 - self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ - self.editing_waypoint_id = None # 編集中のウェイポイントを示す図形のオブジェクトID - self.moving_waypoint = False # ウェイポイントをDnDで動かしている最中かどうか - self.add_wp_win = None # add waypoint hereをクリックしたときに表示するウィンドウ - return + ## その他必要になる変数の初期化 + self.old_click_point = None # 最後にカーソルのあった座標を保持 + self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ + self.editing_waypoint_id = None # 編集中のウェイポイントを示す図形のオブジェクトID + self.moving_waypoint = False # ウェイポイントをDnDで動かしている最中かどうか + self.add_wp_win = None # add waypoint hereをクリックしたときに表示するウィンドウ + return - """ - +++++ 画面上部にメッセージを表示する +++++ - """ - def message(self, msg): - if not isinstance(msg, str): - msg = str(msg) - self.msg_label["text"] = str(msg) + """ + +++++ 画面上部にメッセージを表示する +++++ + """ + def message(self, msg): + if not isinstance(msg, str): + msg = str(msg) + self.msg_label["text"] = str(msg) - """ - +++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++ - --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- - """ - def get_map_info(self): - map_path = Path('..','..','waypoint_nav','maps','map_gakunai') # .pgmと.yamlの手前までのパス - map_img_pil = Image.open(Path(str(map_path)+'.pgm')) # .pgmをplillowで読み込む - with open(str(map_path)+'.yaml') as file: # .yamlを読み込む - map_yaml = ruamel.yaml.YAML().load(file) - self.master.title(map_path.name + " - " + self.master.title()) - return map_img_pil, map_yaml # この2つの変数を戻り値とする - + """ + +++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++ + --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- + """ + def get_map_info(self): + map_path = Path('..','..','waypoint_nav','maps','map_gakunai') # .pgmと.yamlの手前までのパス + map_img_pil = Image.open(Path(str(map_path)+'.pgm')) # .pgmをplillowで読み込む + with open(str(map_path)+'.yaml') as file: # .yamlを読み込む + map_yaml = ruamel.yaml.YAML().load(file) + self.master.title(map_path.name + " - " + self.master.title()) + return map_img_pil, map_yaml # この2つの変数を戻り値とする + - """ - +++++ waypointsのパスを受け取り読み込んだデータを返す +++++ - --- これもget_param()でパスを受け取り、読み込めるようにする --- - """ - def get_waypoints(self): - file_path = Path('..','..','waypoint_nav','param','waypoints_gakunai.yaml') - with open(file_path) as file: - waypoints = ruamel.yaml.YAML().load(file) - self.master.title(file_path.name + " - " + self.master.title()) - return waypoints, file_path + """ + +++++ waypointsのパスを受け取り読み込んだデータを返す +++++ + --- これもget_param()でパスを受け取り、読み込めるようにする --- + """ + def get_waypoints(self): + file_path = Path('..','..','waypoint_nav','param','waypoints_gakunai.yaml') + with open(file_path) as file: + waypoints = ruamel.yaml.YAML().load(file) + self.master.title(file_path.name + " - " + self.master.title()) + return waypoints, file_path - """ - +++++ 地図上の原点に円を描画する +++++ - """ - def plot_origin(self): - origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換 - r = self.point_rad # 円の半径（ピクセル） - 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_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): - 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 plot_waypoints(self, id=None): + 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") - 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") + return + - """ - +++++ ウェイポイントを左クリックしながら動かしたときのコールバック +++++ - """ - 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_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_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 waypoint_leave(self, event): + self.waypoint_num["text"] = "Waypoint No. -----" + - """ - +++++ キャンバス内でマウスを動かしたときのコールバック +++++ - """ - def mouse_move(self, event): - 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 mouse_move(self, event): + 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 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=4, 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 + """ + +++++ ウェイポイントが左クリックされたとき、別窓で情報を表示する関数 +++++ + """ + 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=4, 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 + """ + +++++ 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 - + """ + +++++ ドラッグ&ドロップボタン(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 - + """ + +++++ 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 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 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_move(self, event): + 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 left_click_release(self, event): + self.old_click_point = None + return - """ - +++++ 右クリックしたときのコールバック関数 +++++ - """ - def right_click(self, event): - # クリックした座標の近くにあるオブジェクトを取得 - 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 + """ + +++++ 右クリックしたときのコールバック関数 +++++ + """ + def right_click(self, event): + # クリックした座標の近くにあるオブジェクトを取得 + 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をクリックしたときのコールバック関数 +++++ + """ + 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 - + """ + +++++ 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): - 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 - + """ + +++++ マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 +++++ + --- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも --- + """ + def mouse_wheel(self, event): + 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): - 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_left_click(self, event): + self.translate_mat(-event.x, -event.y) + self.scale_mat(1.2) + self.translate_mat(event.x, event.y) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom In") + return + - """ - +++++ Ctrl押しながら右クリックしたときのコールバック +++++ - """ - def ctrl_right_click(self, event): - self.translate_mat(-event.x, -event.y) - self.scale_mat(0.8) - self.translate_mat(event.x, event.y) - self.draw_image() - self.plot_origin() - self.plot_waypoints() - self.message("Zoom Out") - return + """ + +++++ Ctrl押しながら右クリックしたときのコールバック +++++ + """ + def ctrl_right_click(self, event): + self.translate_mat(-event.x, -event.y) + self.scale_mat(0.8) + self.translate_mat(event.x, event.y) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom Out") + return - """ - +++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++ - """ - def menu_save_clicked(self, event=None): - self.save_waypoints(self.waypoint_filepath) - self.message("Saved changes!") - return - + """ + +++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++ + """ + def menu_save_clicked(self, event=None): + self.save_waypoints(self.waypoint_filepath) + self.message("Saved changes!") + return + - """ - +++++ Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 +++++ - """ - def menu_saveas_clicked(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.waypoint_filepath.name - self.waypoint_filepath = Path(new_filepath) - self.master.title(current_title.replace(old_filename, self.waypoint_filepath.name)) - self.message("Save As" + "\"" + str(new_filepath) + "\"") - return - + """ + +++++ Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 +++++ + """ + def menu_saveas_clicked(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.waypoint_filepath.name + self.waypoint_filepath = Path(new_filepath) + self.master.title(current_title.replace(old_filename, self.waypoint_filepath.name)) + self.message("Save As" + "\"" + str(new_filepath) + "\"") + return + - """ - +++++ Fileメニューの"Exit"がクリックされたときに実行されるコールバック関数 +++++ - """ - def menu_exit_clicked(self, event=None): - self.master.destroy() + """ + +++++ Fileメニューの"Exit"がクリックされたときに実行されるコールバック関数 +++++ + """ + def menu_exit_clicked(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"]) + "}") - return + """ + +++++ 現在のウェイポイント情報を指定のファイルに書き込む +++++ + """ + 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"]) + "}") + 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) - 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) + 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 + """ + +++++ 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") # 画像の描画 - else: - self.canvas.itemconfig("map_image", image=self.draw_img_tk) # 既に描画された画像を差し替える - 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") # 画像の描画 + else: + self.canvas.itemconfig("map_image", image=self.draw_img_tk) # 既に描画された画像を差し替える + 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 - 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 + return - """ - +++++ キャンバス上（ウィンドウ上）の座標から、元の地図画像上の座標に変換 +++++ - """ - 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 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 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 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 #===== メイン処理　プログラムはここから実行される =====# if __name__ == "__main__": - #rospy.init_node("manager_GUI") - 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クラスのインスタンス - app.mainloop() + #rospy.init_node("manager_GUI") + 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クラスのインスタンス + app.mainloop()