diff --git a/apps/icon/layer.png b/apps/icon/layer.png deleted file mode 100644 index 2ccd413..0000000 --- a/apps/icon/layer.png +++ /dev/null Binary files differ diff --git a/apps/icon/locked_button.png b/apps/icon/locked_button.png new file mode 100644 index 0000000..c23e36d --- /dev/null +++ b/apps/icon/locked_button.png Binary files differ diff --git a/apps/icon/maps.png b/apps/icon/maps.png new file mode 100644 index 0000000..aeb9c3e --- /dev/null +++ b/apps/icon/maps.png Binary files differ diff --git a/apps/icon/unlocked_button.png b/apps/icon/unlocked_button.png new file mode 100644 index 0000000..dd62ba5 --- /dev/null +++ b/apps/icon/unlocked_button.png Binary files differ diff --git a/apps/mylib/application.py b/apps/mylib/application.py index c02b644..9eab68f 100644 --- a/apps/mylib/application.py +++ b/apps/mylib/application.py @@ -54,12 +54,13 @@ ++++++++++ File menu functions ++++++++++ """ def menu_open_base(self, event=None): - map_path = self.menu_open(title="Select base map yaml file") + map_path = self.open_yaml(title="Select base map yaml file") if not map_path: return - waypoints_path = self.menu_open(title="Select waypoints file for the map") + waypoints_path = self.open_yaml(title="Select waypoints file for the map") if not waypoints_path: return - self.tools.set_base_map(Path(map_path).resolve(), Path(waypoints_path).resolve()) + suc = self.tools.set_base_map(Path(map_path).resolve(), Path(waypoints_path).resolve()) + if not suc: return self.open_menu.entryconfigure("Base map", state="disabled") self.open_menu.entryconfigure("Multimaps dir", state="disabled") self.open_menu.entryconfigure("Additional map", state="normal") @@ -67,9 +68,9 @@ def menu_open_addtion(self, event=None): - map_path = self.menu_open(title="Select additional map yaml file") + map_path = self.open_yaml(title="Select additional map yaml file") if not map_path: return - waypoints_path = self.menu_open(title="Select waypoints file for the map") + waypoints_path = self.open_yaml(title="Select waypoints file for the map") if not waypoints_path: return self.tools.add_map(Path(map_path).resolve(), Path(waypoints_path).resolve()) return @@ -84,24 +85,17 @@ 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.open_yaml(title="Select waypoints file for the map") + if not waypoints_path: return + suc = self.tools.set_multimaps(dirpath, Path(waypoints_path).resolve()) + if not suc: return 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): + def open_yaml(self, title): filepath = tkinter.filedialog.askopenfilename( parent=self.master, title=title, @@ -113,25 +107,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("700x200+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 +125,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 +160,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..d04627a 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): @@ -144,7 +154,6 @@ self.rotate_center = None self.old_map_rot = None self.selected_map_key = None - self.selected_map_center = None self.mode = 0 self.base_map_key = "" self.point_rad = 5 @@ -157,7 +166,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 +192,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 +209,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 @@ -220,7 +227,7 @@ def plot_waypoints(self): for key, wp_list in self.waypoints_dict.items(): if key[-2:] == "fp": - finish_pose = self.waypoints_dict[key] + finish_pose: FinishPose = self.waypoints_dict[key] img_x, img_y = self.map_dict[key[:-2]].real2image(finish_pose.x, finish_pose.y) cx, cy = self.map_dict[key[:-2]].transform(img_x, img_y) th = finish_pose.yaw - self.map_dict[key[:-2]].get_rotate_angle() @@ -238,7 +245,7 @@ if len(wp_list.get_id_list()) == 0: # 初めて描画する for n, wp in enumerate(wp_list.get_waypoint()): - id = self.create_waypoint(wp, self.map_dict[key]) + id = self.create_waypoint(key, wp, self.map_dict[key]) self.waypoints_dict[key].set_id(n+1, id) else: for id in wp_list.get_id_list(): @@ -251,7 +258,7 @@ return - def create_waypoint(self, waypoint: dict, mymap: MyMap): + def create_waypoint(self, key, waypoint: dict, mymap: MyMap): img_x, img_y = mymap.real2image(float(waypoint["x"]), float(waypoint["y"])) cx, cy = mymap.transform(img_x, img_y) r = self.point_rad-1 @@ -259,7 +266,7 @@ y0 = round(cy - r) x1 = round(cx + r + 1) y1 = round(cy + r + 1) - id = self.canvas.create_oval(x0, y0, x1, y1, fill='#F88', outline='#F88') + id = self.canvas.create_oval(x0, y0, x1, y1, fill='#F88', outline='#F88', tags=key+"wp") return id @@ -279,8 +286,10 @@ self.canvas.create_polygon(polygon, tags="selection_frame", fill="", outline="#FF0", dash=(5,3), width=5) self.canvas.lift(self.selected_map_key) self.canvas.lift("selection_frame") - self.canvas.lift("origin", self.selected_map_key) - self.plot_waypoints() + self.canvas.lift(self.selected_map_key+"wp") + self.canvas.lift(self.selected_map_key+"fp") + self.canvas.lift("origin") + # self.plot_waypoints() return @@ -288,8 +297,12 @@ key = self.path2key(path) if vision: self.canvas.itemconfigure(key, state=tk.NORMAL) + self.canvas.itemconfigure(key+"wp", state=tk.NORMAL) + self.canvas.itemconfigure(key+"fp", state=tk.NORMAL) else: self.canvas.itemconfigure(key, state=tk.HIDDEN) + self.canvas.itemconfigure(key+"wp", state=tk.HIDDEN) + self.canvas.itemconfigure(key+"fp", state=tk.HIDDEN) diff --git a/apps/mylib/tools.py b/apps/mylib/tools.py index 1380669..90736c3 100644 --- a/apps/mylib/tools.py +++ b/apps/mylib/tools.py @@ -2,6 +2,7 @@ import numpy as np import ruamel.yaml import math +from tkinter import messagebox from PIL import Image, ImageTk from pathlib import Path from .mapdisp import MapDisplay, MyMap @@ -9,6 +10,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): @@ -16,6 +24,7 @@ self.map_path = Path() self.map_transparency = tk.IntVar() self.map_transparency.set(100) + self.state = "unlocked" def set_map_path(self, path: Path): @@ -51,17 +60,29 @@ self.rot_btn.bind("", lambda event, btn=self.rot_btn: self.btn_leave(event, btn)) self.rot_btn.bind("", lambda event, btn=self.rot_btn, mode="rotate": self.btn_clicked(event, btn, mode)) self.rot_btn.grid(column=1, row=0) + ## lock, unlock ボタン + icon = Image.open(Path(__file__).parent.parent / Path("icon","locked_button.png")) + icon = icon.resize((30,30)) + self.locked_icon = ImageTk.PhotoImage(image=icon) + icon = Image.open(Path(__file__).parent.parent / Path("icon","unlocked_button.png")) + icon = icon.resize((30,30)) + self.unlocked_icon = ImageTk.PhotoImage(image=icon) + self.lock_btn = tk.Label(self.btn_frame, image=self.unlocked_icon, bg=theme["main"]) + self.lock_btn.bind("", lambda event, btn=self.lock_btn: self.btn_entry(event, btn)) + self.lock_btn.bind("", lambda event, btn=self.lock_btn: self.btn_leave(event, btn)) + self.lock_btn.bind("", self.lock_btn_clicked) + self.lock_btn.grid(column=0, row=1) ## 余白 space = tk.Frame(self, height=100, bg=theme["main"]) space.pack(expand=False, fill=tk.X) - #### レイヤーリストを表示 #### + #### マップリストを表示 #### ## アイコン - icon = Image.open(Path(__file__).parent.parent / Path("icon","layer.png")) - icon = icon.resize((20, 17)) + icon = Image.open(Path(__file__).parent.parent / Path("icon","maps.png")) + icon = icon.resize((20, 20)) self.layer_icon = ImageTk.PhotoImage(image=icon) layer_label = tk.Label(self, bg=theme["bg1"], - text=" Layers", fg="white", font=("Arial",11,"bold"), + text=" Maps", fg="white", font=("Arial",11,"bold"), image=self.layer_icon, compound=tk.LEFT ) layer_label.pack(expand=False, anchor=tk.W, ipadx=3, ipady=2, padx=5) @@ -111,6 +132,7 @@ def btn_clicked(self, event, btn: tk.Label, mode): if (len(self.label_list) == 0) or (self.selected_layer == self.base_map_layer): return + if self.selected_layer.state == "locked": return if btn.cget("bg") == "black": self.map_disp.mode = self.map_disp.Normal btn.configure(bg="#333") @@ -123,36 +145,116 @@ elif mode == "rotate": self.map_disp.mode = self.map_disp.RotateSelected btn.configure(bg="black") return + + + def lock_btn_clicked(self, event): + if (len(self.label_list) == 0) or (self.selected_layer == self.base_map_layer): return + if self.lock_btn.cget("bg") == "black": + self.lock_btn.configure(image=self.unlocked_icon, bg=self.theme["main"]) + self.selected_layer.state = "unlocked" + else: + self.lock_btn.configure(image=self.locked_icon, bg="black") + self.selected_layer.state = "locked" + return 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) + map_yaml = read_file(map_path) + if not ("image" in map_yaml): + messagebox.showerror(title="Format error", message="Selected map file is unexpected format.") + return False + wp_yaml = read_file(wp_path) + if (not "waypoints" in wp_yaml) or (not "finish_pose" in wp_yaml): + messagebox.showerror(title="Format error", message="Selected waypoints file is unexpected format.") + return False + try: + self.map_disp.add_map(map_path, map_yaml, wp_yaml, base=True) + except (FileNotFoundError, FileExistsError): + messagebox.showerror(title="Image file is not found", message="\""+map_yaml["image"]+"\" is not found.") + return False self.append_layer(map_path, base=True) + self.base_map_layer.state = "locked" + self.lock_btn.configure(image=self.locked_icon, bg="black") self.base_waypoints_path = wp_path - return + return True 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: - print("Selected map is already exist") - return + map_yaml = read_file(path) + if not ("image" in map_yaml): + messagebox.showerror(title="Format error", message="Selected map file is unexpected format.") + return False + wp_yaml = read_file(wp_path) + if (not "waypoints" in wp_yaml) or (not "finish_pose" in wp_yaml): + messagebox.showerror(title="Format error", message="Selected waypoints file is unexpected format.") + return False + try: + if self.map_disp.add_map(path, map_yaml, wp_yaml): + self.append_layer(path) + else: + 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) + path = path2 + except (FileNotFoundError, FileExistsError): + messagebox.showerror(title="Image file is not found", message="\""+map_yaml["image"]+"\" is not found.") + return False + self.selected_layer.map_transparency.set(70) + self.map_disp.set_transparency(path, 70) + self.tra_bar.configure(variable=self.selected_layer.map_transparency) + return True - 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) + if (not "waypoints" in wp_yaml) or (not "finish_pose" in wp_yaml): + messagebox.showerror(title="Format error", message="Selected waypoints file is unexpected format.") + return False + 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 + if map_idx==0: + self.map_disp.add_map(map_path, map_yaml, wp_yaml, base=True) + self.append_layer(map_path, base=True) + self.base_map_layer.state = "locked" + self.lock_btn.configure(image=self.locked_icon, bg="black") + self.base_waypoints_path = wp_path + else: + self.map_disp.add_map(map_path, map_yaml, wp_yaml) + self.append_layer(map_path) + self.selected_layer.map_transparency.set(70) + self.map_disp.set_transparency(map_path, 70) + self.tra_bar.configure(variable=self.selected_layer.map_transparency) + map_idx += 1 + wp_idx += 1 + + return True def append_layer(self, path: Path, base=None): @@ -176,6 +278,7 @@ self.selected_layer.configure(bg=self.theme["bg1"]) self.selected_layer = label self.tra_bar.configure(variable=self.selected_layer.map_transparency, state=tk.NORMAL) + self.lock_btn.configure(image=self.unlocked_icon, bg=self.theme["main"]) self.label_list.append(label) if base: self.base_map_layer = label return @@ -193,7 +296,6 @@ else: label.configure(image=self.visible_icon, fg="white") self.map_disp.set_vision_state(label.map_path, vision=True) - return if label != self.selected_layer: self.selected_layer.configure(bg=self.theme["bg1"]) @@ -201,6 +303,15 @@ self.selected_layer = label self.tra_bar.configure(variable=self.selected_layer.map_transparency) self.map_disp.select_map(self.selected_layer.map_path) + for btn in [self.move_btn, self.rot_btn]: + if btn.cget("bg") == "black": + self.map_disp.mode = self.map_disp.Normal + btn.configure(bg=self.theme["main"]) + break + if self.selected_layer.state == "locked": + self.lock_btn.configure(image=self.locked_icon, bg="black") + else: + self.lock_btn.configure(image=self.unlocked_icon, bg=self.theme["main"]) return @@ -210,29 +321,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 +342,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 +399,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 +423,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 +465,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 diff --git a/scripts/map_changer b/scripts/map_changer index 10e3792..f73f36a 100755 --- a/scripts/map_changer +++ b/scripts/map_changer @@ -6,6 +6,7 @@ from std_msgs.msg import UInt16 from nav_msgs.srv import LoadMap from std_srvs.srv import Empty +from std_srvs.srv import Trigger @@ -32,6 +33,8 @@ ## Service clients self.change_map = rospy.ServiceProxy("/change_map", LoadMap) self.amcl_update = rospy.ServiceProxy("/request_nomotion_update", Empty) + self.stop_nav = rospy.ServiceProxy("/stop_wp_nav", Trigger) + self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger) return @@ -45,6 +48,8 @@ def change_map_service_call(self): + self.stop_nav() + rospy.sleep(0.5) self.update_amcl_call() rospy.wait_for_service("/change_map") try: @@ -52,6 +57,7 @@ if res.result == 0: rospy.loginfo("Successfully changed the map") self.update_amcl_call() + self.resume_nav() return True else: rospy.logerr("Failed to change the map: result=", res.result) @@ -63,9 +69,10 @@ def update_amcl_call(self): rospy.wait_for_service("/request_nomotion_update") - for i in range(0,5): + for i in range(0, 5): self.amcl_update() rospy.sleep(0.5) + rospy.loginfo("Update amcl pose 5 times")