#import rospy
from textwrap import fill
import tkinter as tk
import tkinter.filedialog
import numpy as np
import quaternion
import math
import ruamel.yaml
from PIL import Image, ImageTk
from pathlib import Path
from ruamel.yaml.comments import CommentedMap
#===== 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.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.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 # 右クリックしたときの座標を保持する変数
## 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.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()
## 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.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)
"""
+++++ 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
"""
+++++ 地図上の原点に円を描画する +++++
"""
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 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_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 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 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
"""
+++++ ドラッグ&ドロップボタン(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
"""
+++++ ウェイポイント情報を表示するサブウィンドウを閉じたときのコールバック +++++
"""
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_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
"""
+++++ 右クリックしてポップアップメニューの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
"""
+++++ マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 +++++
--- 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_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 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()
"""
+++++ 現在のウェイポイント情報を指定のファイルに書き込む +++++
"""
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に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
"""
+++++ ウィンドウサイズが変更されたとき、情報を更新する +++++
"""
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 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()
