diff --git a/apps/mylib/application.py b/apps/mylib/application.py index c02b644..2659310 100644 --- a/apps/mylib/application.py +++ b/apps/mylib/application.py @@ -84,17 +84,9 @@ if not dirpath: return dirpath = Path(dirpath) if not (dirpath / Path("map0.yaml")).exists(): return - map_path = dirpath / Path("map0.yaml") - waypoints_path = dirpath / Path("waypoints0.yaml") - self.tools.set_base_map(map_path, waypoints_path) - i = 1 - while(True): - map_path = dirpath / Path("map{}.yaml".format(i)) - waypoints_path = dirpath / Path("waypoints{}.yaml".format(i)) - if not map_path.exists(): break - self.tools.add_map(map_path, waypoints_path) - i += 1 - + waypoints_path = self.menu_open(title="Select waypoints file for the map") + if not waypoints_path: return + self.tools.set_multimaps(dirpath, Path(waypoints_path).resolve()) self.open_menu.entryconfigure("Base map", state="disabled") self.open_menu.entryconfigure("Multimaps dir", state="disabled") self.open_menu.entryconfigure("Additional map", state="normal") @@ -113,25 +105,14 @@ def menu_export(self, event=None): if (len(self.tools.label_list) < 2): return + ## サブウィンドウを作成 win = tk.Toplevel() - win.geometry("1000x300+50+50") - win.minsize(width=500, height=300) + win.geometry("500x200+50+50") + win.minsize(width=500, height=200) win.attributes('-topmost', True) win.title("Export") - font = ("Consolas", 12) + font = ("Arial", 12) - ### ファイルパスを参照するダイアログを開く関数（ボタンコールバック） ### - def ref_btn_callback_f(entry: tk.Entry, init_dir): - filepath = tkinter.filedialog.asksaveasfilename( - parent=win, - title="File path to export", - initialdir=init_dir, - filetypes=[("YAML", ".yaml")] - ) - if not filepath: return - entry.delete(0, tk.END) - entry.insert(tk.END, str(filepath)) - ### ファルダパスを参照するダイアログを開く関数（ボタンコールバック） ### def ref_btn_callback_d(entry: tk.Entry, init_dir): dirpath = tkinter.filedialog.askdirectory( @@ -142,39 +123,33 @@ if not dirpath: return entry.delete(0, tk.END) entry.insert(tk.END, str(dirpath)) - - ### ファイルの保存場所の表示、変更をするフィールドを作成する関数 ### - def create_entry(label_txt, init_path, callback): - frame = tk.Frame(win) - frame.pack(expand=False, fill=tk.X, padx=5, pady=10) - label = tk.Label(frame, text=label_txt, anchor=tk.W, font=font) - label.grid(column=0, row=0, padx=3, pady=2, sticky=tk.EW) - ref_btn = tk.Button(frame, image=self.tools.folder_icon) - ref_btn.grid(column=1, row=1, sticky=tk.E, padx=5) - tbox = tk.Entry(frame, font=font) - tbox.grid(column=0, row=1, padx=20, pady=2, sticky=tk.EW) - tbox.insert(tk.END, init_path) - init_dir = str(Path(init_path).parent) - ref_btn["command"] = lambda entry=tbox, init_dir=init_dir: callback(entry, init_dir) - frame.grid_columnconfigure(0, weight=1) - return tbox + ## サブウィンドウにフォルダ入力エントリーを作成 + frame = tk.Frame(win) + frame.pack(expand=False, fill=tk.X, padx=5, pady=10) + label = tk.Label(frame, text="Directory :", anchor=tk.W, font=font) + label.grid(column=0, row=0, padx=3, pady=2, sticky=tk.EW) + ref_btn = tk.Button(frame, image=self.tools.folder_icon) + ref_btn.grid(column=1, row=1, sticky=tk.E, padx=5) + tbox = tk.Entry(frame, font=font) + tbox.grid(column=0, row=1, padx=20, pady=2, sticky=tk.EW) + init_dir = str(Path(".").resolve()) + tbox.insert(tk.END, init_dir) + ref_btn["command"] = lambda entry=tbox, init_dir=init_dir: ref_btn_callback_d(entry, init_dir) + frame.grid_columnconfigure(0, weight=1) + ##　マルチマップyaml、結合したウェイポイント、合成した地図画像と情報yamlを取得 - # multimap_yaml, path = self.tools.get_multimap_yaml() - map_img_list, map_yaml_list, path = self.tools.get_map_lists() - tbox1 = create_entry("- Multi mps directory:", str(path), ref_btn_callback_d) - wp_yaml, wp_yaml_list, path = self.tools.get_waypoints_yaml() - tbox2 = create_entry("- Merged waypoints yaml file:", str(path), ref_btn_callback_f) - merged_img_pil, merged_yaml, path = self.tools.get_merged_map() - tbox3 = create_entry("- Merged map (yaml, pgm) file:", str(path), ref_btn_callback_f) + map_img_list, map_yaml_list = self.tools.get_map_lists() + wp_yaml, wp_yaml_list = self.tools.get_waypoints_yaml() + merged_img_pil, merged_yaml = self.tools.get_merged_map() ### それぞれをファイルに書き込む関数（ボタンコールバック） ### def export_btn_callback(): # multi maps - path = Path(tbox1.get()) - if not path.exists(): path.mkdir() + dir_path = Path(tbox.get()).resolve() + if not dir_path.exists(): dir_path.mkdir() for i, img in enumerate(map_img_list): - img_path = path / Path("map{}.pgm".format(i)) + img_path = dir_path / Path("map{}.pgm".format(i)) img.save(str(img_path.resolve())) map_yaml_list[i]["image"] = "./"+str(img_path.name) str_yaml = "" @@ -183,27 +158,23 @@ str_yaml += "{}: {}".format(key, val) + line with open(str(img_path.with_suffix(".yaml")), 'w') as f: f.write(str_yaml) - # each waypoints - for i, str_yaml in enumerate(wp_yaml_list): - p = path / Path("waypoints{}.yaml".format(i)) - with open(str(p.resolve()), 'w') as f: - f.write(str_yaml) # merged waypoints - path = tbox2.get() - with open(path, 'w') as f: + wp_filepath = dir_path / Path("waypoints.yaml") + with open(wp_filepath, 'w') as f: f.write(wp_yaml) # merged map (yaml, pgm) - path = Path(tbox3.get()).resolve() - merged_yaml["image"] = "./" + str(path.with_suffix(".pgm").name) + yaml_path = dir_path / Path("merged_map.yaml") + img_path = yaml_path.with_suffix(".pgm") + merged_yaml["image"] = "./" + str(img_path.name) str_yaml = "" line = "\n" for key, val in merged_yaml.items(): str_yaml += "{}: {}".format(key, val) + line - with open(str(path.with_suffix(".yaml")), 'w') as f: + with open(str(yaml_path), 'w') as f: f.write(str_yaml) - merged_img_pil.save(str(path.with_suffix(".pgm"))) + merged_img_pil.save(str(img_path)) win.destroy() return diff --git a/apps/mylib/mapdisp.py b/apps/mylib/mapdisp.py index 84ddfa7..37e1cce 100644 --- a/apps/mylib/mapdisp.py +++ b/apps/mylib/mapdisp.py @@ -1,9 +1,18 @@ import tkinter as tk import numpy as np import math +import ruamel.yaml from PIL import Image, ImageTk from pathlib import Path -from mylib.waypointlib import WaypointList, FinishPose +from .waypointlib import WaypointList, FinishPose + + + +def read_file(path: Path): + with open(path) as file: # .yamlを読み込む + yaml = ruamel.yaml.YAML().load(file) + return yaml + class MyMap(): @@ -118,6 +127,7 @@ + class MapDisplay(tk.Frame): def __init__(self, master, theme, **kwargs): @@ -157,7 +167,7 @@ - def add_map(self, path: Path, map_yaml, waypoints_yaml: Path, base=False): + def add_map(self, path: Path, map_yaml, waypoints_yaml, base=False): key = self.path2key(path) if key in self.map_dict.keys(): return False @@ -183,8 +193,6 @@ base_map: MyMap = self.map_dict[self.base_map_key] img_x, img_y = base_map.real2image(new_map.origin[0], new_map.origin[1]) offset_x, offset_y = base_map.transform(img_x, img_y-img_h) - # offset_x = (self.canv_w - img_w*scale) / 2 - # offset_y = (self.canv_h - img_h*scale) / 2 new_map.scale_at(0, 0, scale) new_map.translate(offset_x, offset_y) @@ -202,7 +210,7 @@ def plot_origin(self): - base_map = self.map_dict[self.base_map_key] + base_map: MyMap = self.map_dict[self.base_map_key] canv_origin = base_map.transform(base_map.img_origin[0], base_map.img_origin[1]) r = self.point_rad x1 = canv_origin[0] - r diff --git a/apps/mylib/tools.py b/apps/mylib/tools.py index 1380669..15109d5 100644 --- a/apps/mylib/tools.py +++ b/apps/mylib/tools.py @@ -9,6 +9,13 @@ +def read_file(path: Path): + with open(path) as file: # .yamlを読み込む + yaml = ruamel.yaml.YAML().load(file) + return yaml + + + class LayerLabel(tk.Label): def __init__(self, master, *args, **kwargs): @@ -127,9 +134,9 @@ def set_base_map(self, map_path: Path, wp_path: Path): - map_yaml = self.read_file(map_path) + map_yaml = read_file(map_path) if not ("image" in map_yaml): return - wp_yaml = self.read_file(wp_path) + wp_yaml = read_file(wp_path) if not ("waypoints" in wp_yaml): return self.map_disp.add_map(map_path, map_yaml, wp_yaml, base=True) self.append_layer(map_path, base=True) @@ -138,21 +145,57 @@ def add_map(self, path: Path, wp_path: Path): - map_yaml = self.read_file(path) + map_yaml = read_file(path) if not ("image" in map_yaml): return - wp_yaml = self.read_file(wp_path) + wp_yaml = read_file(wp_path) if not ("waypoints" in wp_yaml): return if self.map_disp.add_map(path, map_yaml, wp_yaml): self.append_layer(path) else: - print("Selected map is already exist") + i = 2 + path2 = path.with_name(path.with_suffix("").name + "-" + str(i)) + while(not self.map_disp.add_map(path2, map_yaml, wp_yaml)): + i += 1 + path2 = path.with_name(path.with_suffix("").name + "-" + str(i)) + self.append_layer(path2) return - def read_file(self, path: Path): - with open(path) as file: # .yamlを読み込む - yaml = ruamel.yaml.YAML().load(file) - return yaml + def set_multimaps(self, dirpath: Path, wp_path: Path): + all_wp_yaml = read_file(wp_path) + map_idx = 0 + wp_idx = 0 + while(True): + map_path = dirpath / Path("map{}.yaml".format(map_idx)) + if (not map_path.exists()) or (wp_idx >= len(all_wp_yaml["waypoints"])): break + map_yaml = read_file(map_path) + wp_yaml = {"waypoints":[]} + while(True): + if wp_idx >= len(all_wp_yaml["waypoints"]): + wp_yaml["finish_pose"] = all_wp_yaml["finish_pose"] + break + point = all_wp_yaml["waypoints"][wp_idx] + if not "change_map" in point["point"].keys(): + wp_yaml["waypoints"].append(point) + wp_idx += 1 + else: + finish_pose = point["point"] + wp_yaml["finish_pose"] = { + "header":{"seq":0, "stamp":0, "frame_id":"map"}, + "pose":{ + "position":{"x":finish_pose["x"], "y":finish_pose["y"], "z":finish_pose["z"]}, + "orientation":{"x":0, "y":0, "z":0, "w":1} + } + } + break + base = (map_idx==0) + self.map_disp.add_map(map_path, map_yaml, wp_yaml, base=base) + self.append_layer(map_path, base=base) + if base: self.base_waypoints_path = wp_path + map_idx += 1 + wp_idx += 1 + + return def append_layer(self, path: Path, base=None): @@ -210,29 +253,6 @@ return - # def get_multimap_yaml(self): - # base_map: MyMap = self.map_disp.map_dict[self.map_disp.base_map_key] - # line = "\n" - # yaml = "multimap:" + line - # name = "multi" - # for label in self.label_list: - # yaml += "- map:" + line - # mymap: MyMap = self.map_disp.get_map(label.map_path) - # for key, val in mymap.map_yaml.items(): - # if key == "image": val = str(mymap.img_path) - # if key == "origin": - # corners = mymap.get_corners() - # img_x, img_y = base_map.inv_transform(corners[6],corners[7]) - # real_x, real_y = base_map.image2real(img_x, img_y) - # theta = mymap.get_rotate_angle() - # val = [real_x, real_y, theta] - # yaml += " " + key + ": " + str(val) + line - # yaml += " change_point: " + line - # name += "-" + mymap.yaml_path.with_suffix("").name - # init_path = base_map.yaml_path.with_name(name+".yaml").resolve() - # return yaml, init_path - - def get_map_lists(self): base_map: MyMap = self.map_disp.map_dict[self.map_disp.base_map_key] base_yaml = base_map.map_yaml @@ -254,8 +274,7 @@ real_x, real_y = base_map.image2real(img_x, img_y) yaml["origin"] = [real_x, real_y, 0.0] yaml_list.append(yaml) - init_path = base_map.yaml_path.resolve().parent - return img_list, yaml_list, init_path + return img_list, yaml_list def get_waypoints_yaml(self): @@ -312,8 +331,7 @@ wpc["stop"] = "true" wpc["change_map"] = "true" waypoints.append(wpc) - init_path = self.base_waypoints_path.with_name(name+".yaml").resolve() - return get_waypoint_yaml(waypoints, finish_pose), wp_yaml_list, init_path + return get_waypoint_yaml(waypoints, finish_pose), wp_yaml_list def get_merged_map(self): @@ -337,7 +355,7 @@ right = img_corners[:, [0, 2, 4, 6]].max() lower = img_corners[:, [1, 3, 5, 7]].max() # 合成後の画像を準備 - offset = 2 # 結合時の大きさの不揃いを解消 + offset = 2 # 結合時の画像サイズの不揃いを解消 img_size = (int(round(lower-upper)+offset), int(round(right-left)+offset)) #(y, x) unknown = np.full(img_size, 205, dtype=np.uint8) # 未確定領域 obstacles = np.full(img_size, 255, dtype=np.uint8) # 障害物が0, それ以外が255 @@ -379,6 +397,5 @@ "occupied_thresh":base_map.map_yaml["occupied_thresh"], "free_thresh":base_map.map_yaml["free_thresh"] } - init_path = base_map.yaml_path.with_name(name).resolve() - return merged_img, merged_yaml, init_path + return merged_img, merged_yaml