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("<Control-s>", self.menu_save_clicked) #キーボードショートカットを設定
- self.bind_all("<Control-Shift-S>", self.menu_saveas_clicked)
- self.bind_all("<Control-q>", 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("<Control-s>", self.menu_save_clicked) #キーボードショートカットを設定
+ self.bind_all("<Control-Shift-S>", self.menu_saveas_clicked)
+ self.bind_all("<Control-q>", 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("<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.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.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, "<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>", 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, "<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>", 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()