diff --git a/apps/mylib/application.py b/apps/mylib/application.py index d668a26..c02b644 100644 --- a/apps/mylib/application.py +++ b/apps/mylib/application.py @@ -1,6 +1,5 @@ import tkinter as tk import tkinter.filedialog -import re from pathlib import Path from .tools import Tools @@ -26,6 +25,7 @@ ) self.open_menu.add_command(label="Base map", command=self.menu_open_base) self.open_menu.add_command(label="Additional map", command=self.menu_open_addtion, state="disabled") + self.open_menu.add_command(label="Multimaps dir", command=self.menu_open_multi) self.file_menu.add_cascade(label="Open", menu=self.open_menu) ## Fileメニューその他 self.file_menu.add_command(label="Export", command=self.menu_export, accelerator="Ctrl+E") @@ -36,7 +36,7 @@ self.bind_all("", self.menu_exit) ## 大元に作成したメニューバーを設定 self.menu_bar.add_cascade(label=" File ", menu=self.file_menu) # Fileメニューとしてバーに追加 - self.master.config(menu=self.menu_bar) + self.master.configure(menu=self.menu_bar) #### 仕切り線で大きさを変更できるウィンドウ #### paned_window = tk.PanedWindow(self.master, sashwidth=3, bg="gray", relief=tk.RAISED) @@ -61,6 +61,7 @@ self.tools.set_base_map(Path(map_path).resolve(), 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") return @@ -72,7 +73,33 @@ if not waypoints_path: return self.tools.add_map(Path(map_path).resolve(), Path(waypoints_path).resolve()) return + + def menu_open_multi(self, event=None): + dirpath = tkinter.filedialog.askdirectory( + parent=self.master, + title="Select multi maps directory", + initialdir=str(Path(".")) + ) + 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 + + self.open_menu.entryconfigure("Base map", state="disabled") + self.open_menu.entryconfigure("Multimaps dir", state="disabled") + self.open_menu.entryconfigure("Additional map", state="normal") + return + def menu_open(self, title): filepath = tkinter.filedialog.askopenfilename( @@ -87,7 +114,7 @@ def menu_export(self, event=None): if (len(self.tools.label_list) < 2): return win = tk.Toplevel() - win.geometry("800x300+50+50") + win.geometry("1000x300+50+50") win.minsize(width=500, height=300) win.attributes('-topmost', True) win.title("Export") @@ -136,7 +163,7 @@ # 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, path = self.tools.get_waypoints_yaml() + 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) @@ -144,9 +171,10 @@ ### それぞれをファイルに書き込む関数（ボタンコールバック） ### def export_btn_callback(): # multi maps - path = tbox1.get() + path = Path(tbox1.get()) + if not path.exists(): path.mkdir() for i, img in enumerate(map_img_list): - img_path = Path(path) / Path("map{}.pgm".format(i)) + img_path = path / Path("map{}.pgm".format(i)) img.save(str(img_path.resolve())) map_yaml_list[i]["image"] = "./"+str(img_path.name) str_yaml = "" @@ -155,17 +183,26 @@ 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: f.write(wp_yaml) - # merged map + # merged map (yaml, pgm) path = Path(tbox3.get()).resolve() - new_merged_yaml = re.sub("image: .*\n", "image: {}\n".format(str(path.with_suffix(".pgm"))), merged_yaml) + merged_yaml["image"] = "./" + str(path.with_suffix(".pgm").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: - f.write(new_merged_yaml) + f.write(str_yaml) merged_img_pil.save(str(path.with_suffix(".pgm"))) win.destroy() return diff --git a/apps/mylib/mapdisp.py b/apps/mylib/mapdisp.py index 2f54013..84ddfa7 100644 --- a/apps/mylib/mapdisp.py +++ b/apps/mylib/mapdisp.py @@ -180,10 +180,12 @@ self.base_scale = scale else: scale = self.base_scale - offset_x = (self.canv_w - img_w*scale) / 2 - offset_y = (self.canv_h - img_h*scale) / 2 + 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.rotate(new_map.origin[2]) new_map.scale_at(0, 0, scale) new_map.translate(offset_x, offset_y) self.canvas.create_image(0, 0, anchor=tk.NW, tags=key, diff --git a/apps/mylib/tools.py b/apps/mylib/tools.py index 5fbaab1..1380669 100644 --- a/apps/mylib/tools.py +++ b/apps/mylib/tools.py @@ -128,7 +128,9 @@ def set_base_map(self, map_path: Path, wp_path: Path): map_yaml = self.read_file(map_path) + if not ("image" in map_yaml): return wp_yaml = self.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) self.base_waypoints_path = wp_path @@ -137,7 +139,9 @@ def add_map(self, path: Path, wp_path: Path): map_yaml = self.read_file(path) + if not ("image" in map_yaml): return wp_yaml = self.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: @@ -259,6 +263,8 @@ waypoints = WaypointList(self.map_disp.waypoints_dict[self.map_disp.base_map_key].waypoints_yaml) # ベースのfinish poseをウェイポイントに変換 finish_pose: FinishPose = self.map_disp.waypoints_dict[self.map_disp.base_map_key + "fp"] + # 個別のウェイポイントをリスト化 + wp_yaml_list = [get_waypoint_yaml(waypoints, finish_pose)] wpc = waypoints.get_waypoint(num=1).copy() wpc["x"] = finish_pose.x wpc["y"] = finish_pose.y @@ -283,18 +289,21 @@ [math.sin(theta), math.cos(theta), real_y], [0, 0, 1 ]] wp_list: WaypointList = self.map_disp.waypoints_dict[self.map_disp.path2key(label.map_path)] - for wp in wp_list.get_waypoint(): + wp_list_copy = WaypointList(wp_list.waypoints_yaml) + for i, wp in enumerate(wp_list.get_waypoint()): tf_xy = np.dot(mat, [wp["x"], wp["y"], 1]) # ベースの座標系に変換 wpc = wp.copy() wpc["x"] = round(tf_xy[0], 6) wpc["y"] = round(tf_xy[1], 6) waypoints.append(wpc) + wp_list_copy.waypoints[i] = wpc # 2つ目以降の地図のfinish poseも変換 - finish_pose = FinishPose(self.map_disp.waypoints_dict[self.map_disp.path2key(label.map_path)].waypoints_yaml) + finish_pose = FinishPose(wp_list.waypoints_yaml) tf_xy = np.dot(mat, [finish_pose.x, finish_pose.y, 1]) finish_pose.x = tf_xy[0] finish_pose.y = tf_xy[1] finish_pose.yaw = finish_pose.yaw + theta + wp_yaml_list.append(get_waypoint_yaml(wp_list_copy, finish_pose)) # 最後以外のfinish poseはウェイポイントとして追加 if label != self.label_list[-1]: wpc = waypoints.get_waypoint(num=1).copy() @@ -304,7 +313,7 @@ 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), init_path + return get_waypoint_yaml(waypoints, finish_pose), wp_yaml_list, init_path def get_merged_map(self): @@ -362,13 +371,14 @@ origin = [left-base_map.img_origin[0], -lower+base_map.img_origin[1], 0] origin[0] = origin[0] * base_map.resolution origin[1] = origin[1] * base_map.resolution - line = "\n" - merged_yaml = "image: ./" + base_map.yaml_path.with_name(name+".pgm").name + line - merged_yaml += "resolution: " + str(base_map.resolution) + line - merged_yaml += "origin: " + str(origin) + line - merged_yaml += "negate: " + "0" + line - merged_yaml += "occupied_thresh: " + "0.65" + line - merged_yaml += "free_thresh: " + "0.196" + merged_yaml = { + "image":"./" + base_map.yaml_path.with_name(name+".pgm").name, + "resolution":base_map.resolution, + "origin":origin, + "negate":base_map.map_yaml["negate"], + "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