#import rospy
import tkinter as tk
import numpy as np
import ruamel.yaml
from PIL import Image, ImageTk
from pathlib import Path
#===== Applicationクラスの定義 tk.Frameクラスを継承 =====#
class Application(tk.Frame):
"""
+++++ コンストラクタ +++++
プログラムを実行したときに最初にすべき処理を全て記述する
"""
def __init__(self, master):
super().__init__(master) # スーパークラスのコンストラクタを実行
self.master.title("Waypoints Manager")
## 画面上部のメニューを作成
self.menu_bar = tk.Menu(self) # メニューバーを配置
self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) # バーに追加するメニューを作成
self.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.bind_all("<Control-s>", self.menu_save_clicked) #キーボードショートカットを設定
self.bind_all("<Control-Shift-S>", self.menu_saveas_clicked)
self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定
## map.pgm, map.yaml, waypoints.yaml の読み込み 3つの変数は再代入禁止
self.__map_img_pil, self.__map_yaml = self.get_map_info()
self.__waypoints = self.get_waypoints()
## canvasを配置
self.canvas = tk.Canvas(self.master, background="#a0a0a0") # 画像を描画する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.mat_affine = self.scale_mat(scale, self.mat_affine)
self.mat_affine = self.translate_mat(offset_x, offset_y, self.mat_affine)
self.draw_img_tk = self.draw_image(self.mat_affine)
## 右クリックしたときに表示するポップアップメニューを作成
self.popup_menu = tk.Menu(self, tearoff=tk.OFF)
self.popup_menu.add_command(label="add waypoint", command=self.add_waypoint)
self.right_click_coord = None # 右クリックしたときの座標を保持する変数
## マウスイベントを設定
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("<Button-3>", self.right_click)
## ウィンドウに関するコールバック
self.master.bind("<Configure>", self.window_resize_callback)
origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m)
resol = self.__map_yaml['resolution']
self.img_origin = [-origin[0]/resol, self.__map_img_pil.height+origin[1]/resol, 1]
self.plot_origin()
return
"""
+++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++
--- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように ---
"""
def get_map_info(self):
map_path = Path('..','..','waypoint_nav','maps','map') # .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)
return map_img_pil, map_yaml # この2つの変数を戻り値とする
"""
+++++ waypointsのパスを受け取り読み込んだデータを返す +++++
--- これもget_param()でパスを受け取り、読み込めるようにする ---
"""
def get_waypoints(self):
file_path = Path('..','..','waypoint_nav','param','waypoints.yaml')
with open(file_path) as file:
waypoints = ruamel.yaml.YAML().load(file)
return waypoints
"""
+++++ 地図上の原点に円を描画する +++++
"""
def plot_origin(self):
origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換
origin_affine = origin_affine.astype(np.int32) # 符号あり整数に変換
r = 10 # 円の半径（ピクセル）
x0 = origin_affine[0] - r
y0 = origin_affine[1] - r
x1 = origin_affine[0] + r + 1
y1 = origin_affine[1] + r + 1
# 円を描画できる位置かどうか判別
if (x0 < 0) or (y0 < 0) or (x1 > self.canv_w) or (y1 > self.canv_h):
return
elif self.canvas.find_withtag("origin"): # 既に円を描画済み
self.canvas.itemconfig("origin", x0=x0, y0=y0, x1=x1, y1=y1)
else: # 初めて円を描画する
self.canvas.create_oval(x0, y0, x1, y1, tags="origin", fill='cyan', outline='blue')
return
"""
+++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++
"""
def left_click_move(self, event):
print("x=" + str(event.x) + " y=" + str(event.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
# クリックされた座標 => 元画像の座標 の変換
mat_inv = np.linalg.inv(self.mat_affine)
img_xy = np.dot(mat_inv, np.array([[event.x], [event.y], [1]]))
# 変換後の元画像の座標がサイズから外れている場合何もしない（地図画像の外をクリックしている）
if (img_xy[0] < 0) or (img_xy[1] < 0) or \
(img_xy[0] > self.__map_img_pil.width) or (img_xy[1] > self.__map_img_pil.height):
return
self.popup_menu.post(event.x_root, event.y_root) # メニューをポップアップ
self.right_click_coord = img_xy # クリックされた元画像上の座標を変数に格納
return
"""
+++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++
"""
def add_waypoint(self):
print("Clicked \"add waypoint\"", self.right_click_coord[0], self.right_click_coord[1])
pix_xy = np.array(self.right_click_coord, np.uint32)
print("value=",self.__map_img_pil.getpixel((pix_xy[0], pix_xy[1])))
return
"""
+++++ マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 +++++
--- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも ---
"""
def mouse_wheel(self, event):
if event.delta > 0:
# 上に回転（タッチパッドなら下にドラッグ）=> 拡大
print(event.x, event.y, event.delta)
else:
# 下に回転（タッチパッドなら上にドラッグ）=> 縮小
print(event.x, event.y, event.delta)
# 回転の向きに対するevent.deltaの正負はプラットフォームにより異なる可能性あり
return
"""
+++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++
"""
def menu_save_clicked(self, event=None):
print("Clicked \"Save\"")
return
"""
+++++ Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 +++++
"""
def menu_saveas_clicked(self, event=None):
print("Clicked \"Save As\"")
return
"""
+++++ 左クリックされたときに実行されるコールバック関数 +++++
"""
def left_click(self, event):
self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示
# クリックした座標の近くにあるオブジェクトを取得
clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20)
if clicked_obj: # オブジェクトがクリックされた場合
tag = self.canvas.gettags(clicked_obj[0])[0]
print("Clicked " + tag)
return
"""
+++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++
"""
def translate_mat(self, x, y, mat_affine):
mat = np.eye(3)
mat[0, 2] = float(x)
mat[1, 2] = float(y)
return np.dot(mat, mat_affine)
"""
+++++ 引数のmat_affineにscale倍のリサイズを加えた同次変換行列を返す +++++
"""
def scale_mat(self, scale, mat_affine):
mat = np.eye(3)
mat[0, 0] = scale
mat[1, 1] = scale
return np.dot(mat, mat_affine)
"""
+++++ 元画像をaffne変換して描画、その画像を返す +++++
"""
def draw_image(self, mat_affine):
mat_inv = np.linalg.inv(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
)
tk_img = ImageTk.PhotoImage(image=img)
if not self.canvas.find_withtag("map_image"): # 初めて画像を描画するとき
self.canvas.create_image(0, 0, anchor='nw', image=tk_img, tags="map_image") # 画像の描画
else:
self.canvas.itemconfig("map_image", image=tk_img) # 既に描画された画像を差し替える
return tk_img
"""
+++++ ウィンドウサイズが変更されたとき、情報を更新する +++++
"""
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
#===== メイン処理 プログラムはここから実行される =====#
if __name__ == "__main__":
#rospy.init_node("manager_GUI")
root = tk.Tk() # 大元になるウィンドウ
w, h = root.winfo_screenwidth(), root.winfo_screenheight()
root.geometry("%dx%d+0+0" % (w, h))
app = Application(master=root) # tk.Frameを継承したApplicationクラスのインスタンス
app.mainloop()
