diff --git a/.gitignore b/.gitignore index 3265fc3..ebaf40e 100644 --- a/.gitignore +++ b/.gitignore @@ -8,7 +8,7 @@ # Local History for Visual Studio Code .history/ *.pptx -mylib/__pycache__/ +apps/mylib/__pycache__/ # Built Visual Studio Code Extensions *.vsix diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..2fbb164 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 3.0.2) +project(multi_map_manager) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES multi_map_manager +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/multi_map_manager.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/multi_map_manager_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/map_changer + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_multi_map_manager.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/apps/icon/folder.png b/apps/icon/folder.png new file mode 100644 index 0000000..397b8cf --- /dev/null +++ b/apps/icon/folder.png Binary files differ diff --git a/apps/icon/invisible.png b/apps/icon/invisible.png new file mode 100644 index 0000000..de968b3 --- /dev/null +++ b/apps/icon/invisible.png Binary files differ diff --git a/apps/icon/layer.png b/apps/icon/layer.png new file mode 100644 index 0000000..2ccd413 --- /dev/null +++ b/apps/icon/layer.png Binary files differ diff --git a/apps/icon/move_button.png b/apps/icon/move_button.png new file mode 100644 index 0000000..8229622 --- /dev/null +++ b/apps/icon/move_button.png Binary files differ diff --git a/apps/icon/rotate_button.png b/apps/icon/rotate_button.png new file mode 100644 index 0000000..3a3a019 --- /dev/null +++ b/apps/icon/rotate_button.png Binary files differ diff --git a/apps/icon/visible.png b/apps/icon/visible.png new file mode 100644 index 0000000..9fa24fc --- /dev/null +++ b/apps/icon/visible.png Binary files differ diff --git a/apps/map_merger.py b/apps/map_merger.py new file mode 100644 index 0000000..dfbc869 --- /dev/null +++ b/apps/map_merger.py @@ -0,0 +1,11 @@ +import tkinter as tk +from mylib.application import Application + + +if __name__ == "__main__": + root = tk.Tk() + w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-100 + root.geometry("%dx%d+0+0" % (w, h)) + root.title("Multi Map & Waypoints Merger") + app = Application(root) + app.mainloop() \ No newline at end of file diff --git a/apps/map_trimmer.py b/apps/map_trimmer.py new file mode 100644 index 0000000..24b689a --- /dev/null +++ b/apps/map_trimmer.py @@ -0,0 +1,299 @@ +import tkinter as tk +import tkinter.filedialog +import ruamel.yaml +from pathlib import Path +from mylib.mapdisp import MyMap +from PIL import Image, ImageDraw + + +class Application(tk.Frame): + + def __init__(self, master): + super().__init__(master) + self.theme = {"main": "#444", + "bg1": "#222"} + + #### 画面上部のメニューバーを作成 #### + self.menu_bar = tk.Menu(self) + ## Fileメニューの作成 + self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF, + bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5 + ) + self.menu_bar.add_cascade(label=" File ", menu=self.file_menu) + ## File -> Open + self.file_menu.add_command(label="Open", command=self.menu_open, accelerator="Ctrl+O") + ## File -> Save As + self.file_menu.add_command(label="Save As", command=self.menu_saveas, accelerator="Ctrl+Shift+S") + ## File -> Exit メニュー + self.file_menu.add_separator() # 仕切り + self.file_menu.add_command(label="Exit", command=self.menu_exit, accelerator="Ctrl+Q") + ## Edit メニューの作成 + self.edit_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF, + bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5 + ) + self.menu_bar.add_cascade(label=" Edit ", menu=self.edit_menu) + ## Edit -> Trim + self.edit_menu.add_command(label="Trim", command=self.menu_trim, accelerator="Ctrl+T") + + #### メニューバーを設定 #### + self.master.configure(menu=self.menu_bar) + + #### キーバインド #### + self.bind_all("", self.menu_trim) + self.bind_all("", self.menu_saveas) + self.bind_all("", self.menu_open) + self.bind_all("", self.menu_exit) + + #### マップを表示するキャンバス #### + self.canvas = tk.Canvas(self.master, bg="black", highlightthickness=0) + self.canvas.pack(expand=True, fill=tk.BOTH, padx=5, pady=5) + self.update() + self.canv_w = self.canvas.winfo_width() + self.canv_h = self.canvas.winfo_height() + + #### マウスイベントコールバック #### + self.canvas.bind("", self.left_click_move) + self.canvas.bind("", self.left_click) + self.canvas.bind("", self.left_click_release) + self.canvas.bind("", lambda event, scale=1.1: self.ctrl_click(event, scale)) + self.canvas.bind("", lambda event, scale=0.9: self.ctrl_click(event, scale)) + self.canvas.bind("", self.resize_callback) + + #### 変数 #### + self.mymap = None + self.origin_img = [] + self.trim_range = [] + self.trimming_mode = False + return + + + + """ + ++++++++++ File menu functions ++++++++++ + """ + def menu_open(self, event=None): + filepath = tkinter.filedialog.askopenfilename( + parent=self.master, + title="Select map yaml file", + initialdir=str(Path(".")), + filetypes=[("YAML", ".yaml")] + ) + if not filepath: return + with open(filepath) as file: # .yamlを読み込む + map_yaml = ruamel.yaml.YAML().load(file) + + self.mymap = MyMap(Path(filepath).resolve(), map_yaml) + img_w = self.mymap.original_img_pil.width + img_h = self.mymap.original_img_pil.height + scale = 1 + offset_x = 0 + offset_y = 0 + if (self.canv_w / self.canv_h) > (img_w / img_h): + # canvasの方が横長　画像をcanvasの縦に合わせてリサイズ + scale = (self.canv_h) / img_h + offset_x = (self.canv_w - img_w*scale) / 2 + else: + # canvasの方が縦長　画像をcanvasの横に合わせてリサイズ + scale = (self.canv_w) / img_w + offset_y = (self.canv_h - img_h*scale) / 2 + + self.mymap.scale_at(0, 0, scale) + self.mymap.translate(offset_x, offset_y) + self.canvas.create_image(0, 0, anchor=tk.NW, tags="map", + image=self.mymap.get_draw_image((self.canv_w, self.canv_h)) + ) + self.trim_range = [0, 0, img_w, img_h] # [x1, y1, x2, y2] 画像における左上のxy, 右下のxy + origin = self.mymap.origin + resolution = self.mymap.resolution + self.origin_img = [-origin[0]/resolution, origin[1]/resolution+self.mymap.original_img_pil.height] + self.plot_origin() + self.master.title(str(filepath)) + return + + + def menu_saveas(self, event=None): + new_filepath = tkinter.filedialog.asksaveasfilename( + parent=self.master, + title="Save map YAML and PGM files.", + initialdir=str(self.mymap.yaml_path), + filetypes=[("PGM YAML", ".pgm .yaml")], + ) + if len(new_filepath) == 0: return + # origin + origin = [(self.trim_range[0]-self.origin_img[0])*self.mymap.resolution, + (-self.trim_range[3]+self.origin_img[1])*self.mymap.resolution, + 0.0] + # Write map yaml file + yaml_path = Path(new_filepath).with_suffix(".yaml") + line = "\n" + yaml = ["image: ./" + yaml_path.with_suffix(".pgm").name + line] + yaml.append("resolution: " + str(self.mymap.resolution) + line) + yaml.append("origin: " + str(origin) + line) + yaml.append("negate: " + str(self.mymap.map_yaml["negate"]) + line) + yaml.append("occupied_thresh: " + str(self.mymap.map_yaml["occupied_thresh"]) + line) + yaml.append("free_thresh: " + str(self.mymap.map_yaml["free_thresh"]) + line) + with open(yaml_path.resolve(), 'w') as f: + f.write("".join(yaml)) + # Trim and save map image + trimmed_img = self.mymap.original_img_pil.crop(tuple(self.trim_range)) + trimmed_img.save(str(yaml_path.with_suffix(".pgm"))) + return + + + def menu_exit(self, event=None): + self.master.destroy() + + + + """ + ++++++++++ Edit menu functions ++++++++++ + """ + def menu_trim(self, event=None): + if not self.mymap: return + if self.trimming_mode: + self.canvas.delete("upper", "right", "lower", "left", "ul", "ur", "lr", "ll") + self.trimming_mode = False + self.set_alpha(0) + return + self.trimming_mode = True + self.set_alpha(128) + r = 10 + ul_x, ul_y = self.mymap.transform(self.trim_range[0], self.trim_range[1]) + lr_x, lr_y = self.mymap.transform(self.trim_range[2], self.trim_range[3]) + cxy = [ul_x, ul_y, lr_x, ul_y, lr_x, lr_y, ul_x, lr_y] + # トリミング範囲の上下左右に直線を描画 + for tag, x1, y1, x2, y2 in [("upper",0,1,2,3), ("right",2,3,4,5), ("lower",4,5,6,7), ("left",6,7,0,1)]: + self.canvas.create_line(cxy[x1], cxy[y1], cxy[x2], cxy[y2], tags=tag, fill="#FFF", width=2) + #　四隅にマーカーを描画 + for tag, xidx, yidx in [("ul",0,1), ("ur",2,3), ("lr",4,5), ("ll",6,7)]: + cx, cy = cxy[xidx], cxy[yidx] + self.canvas.create_oval(cx-r, cy-r, cx+r, cy+r, tags=tag, fill="#BBB", outline="#FFF", activefill="#FFF") + self.canvas.tag_bind(tag, "", lambda event, tag=tag: self.move_trim_range(event, tag)) + return + + + def move_trim_range(self, event, tag): + 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.old_click_point = [event.x, event.y] + # 四隅の円の移動 + self.canvas.move(tag, delta_x, delta_y) + if tag == "ul": + self.canvas.move("ur", 0, delta_y) + self.canvas.move("ll", delta_x, 0) + elif tag == "ur": + self.canvas.move("ul", 0, delta_y) + self.canvas.move("lr", delta_x, 0) + elif tag == "lr": + self.canvas.move("ll", 0, delta_y) + self.canvas.move("ur", delta_x, 0) + else: # "ll" + self.canvas.move("lr", 0, delta_y) + self.canvas.move("ul", delta_x, 0) + # 左上のマーカーの座標を取得 + ul = self.canvas.bbox("ul") + ul_x = (ul[0] + ul[2]) / 2 + ul_y = (ul[1] + ul[3]) / 2 + # 右下の座標 + lr = self.canvas.bbox("lr") + lr_x = (lr[0] + lr[2]) / 2 + lr_y = (lr[1] + lr[3]) / 2 + # マーカー間の線を移動 + self.canvas.coords("upper", ul_x, ul_y, lr_x, ul_y) + self.canvas.coords("right", lr_x, ul_y, lr_x, lr_y) + self.canvas.coords("lower", lr_x, lr_y, ul_x, lr_y) + self.canvas.coords("left" , ul_x, lr_y, ul_x, ul_y) + # 画像上の位置に変換し、トリミングの範囲を更新 + img_ul_x, img_ul_y = self.mymap.inv_transform(ul_x, ul_y) + img_lr_x, img_lr_y = self.mymap.inv_transform(lr_x, lr_y) + img_ul_x = max(img_ul_x, 0) + img_ul_y = max(img_ul_y, 0) + img_lr_x = min(img_lr_x, self.mymap.original_img_pil.width) + img_lr_y = min(img_lr_y, self.mymap.original_img_pil.height) + self.trim_range = [img_ul_x, img_ul_y, img_lr_x, img_lr_y] + # 画像を更新して描画 + self.set_alpha(128) + return + + + def set_alpha(self, a: int): + im_a = Image.new("L", self.mymap.original_img_pil.size, a) + draw = ImageDraw.Draw(im_a) + draw.rectangle(tuple(self.trim_range), fill=255) + self.mymap.original_img_pil.putalpha(im_a) + self.canvas.itemconfigure("map", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) + return + + + + """ + ++++++++++ Plot and move point marker representing origin ++++++++++ + """ + def plot_origin(self): + canv_origin = self.mymap.transform(self.origin_img[0], self.origin_img[1]) + r = 5 + x1 = canv_origin[0] - r + y1 = canv_origin[1] - r + x2 = canv_origin[0] + r + 1 + y2 = canv_origin[1] + r + 1 + if self.canvas.find_withtag("origin"): + self.canvas.moveto("origin", x1, y1) + else: + self.canvas.create_oval(x1, y1, x2, y2, tags="origin", fill='cyan', outline='blue') + self.canvas.lift("origin") + return + + + + """ + ++++++++++ Mouse event callback functions ++++++++++ + """ + def left_click_move(self, event): + if (not self.mymap) or (self.trimming_mode) : 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.old_click_point = [event.x, event.y] + self.mymap.translate(delta_x, delta_y) + self.canvas.itemconfigure("map", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) + self.canvas.move("origin", delta_x, delta_y) + return + + + def ctrl_click(self, event, scale): + if (not self.mymap) or (self.trimming_mode): return + self.mymap.scale_at(event.x, event.y, scale) + self.canvas.itemconfigure("map", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) + self.plot_origin() + return + + + def left_click(self, event): + self.old_click_point = [event.x, event.y] + return + + + def left_click_release(self, event): + self.old_click_point = None + return + + + def resize_callback(self, event): + self.canv_w = self.canvas.winfo_width() + self.canv_h = self.canvas.winfo_height() + return + + + + +if __name__ == "__main__": + root = tk.Tk() + w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-200 + root.geometry("%dx%d+0+0" % (w, h)) + app = Application(root) + app.mainloop() \ No newline at end of file diff --git a/apps/mylib/__init__.py b/apps/mylib/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/apps/mylib/__init__.py diff --git a/apps/mylib/application.py b/apps/mylib/application.py new file mode 100644 index 0000000..e2cb76a --- /dev/null +++ b/apps/mylib/application.py @@ -0,0 +1,185 @@ +import tkinter as tk +import tkinter.filedialog +import re +from pathlib import Path +from .tools import Tools + + + +class Application(tk.Frame): + + def __init__(self, master): + super().__init__(master) + self.theme = {"main": "#444", + "bg1": "#222"} + + #### 画面上部のメニューバーを作成 #### + self.menu_bar = tk.Menu(self) + ## Fileメニューの作成 + self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF, + bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5 + ) + ## Fileメニュー内のOpenメニューを作成 + self.open_menu = tk.Menu(self.file_menu, tearoff=tk.OFF, + bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5, + disabledforeground="black" + ) + 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.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") + self.file_menu.add_separator() + self.file_menu.add_command(label="Exit", command=self.menu_exit, accelerator="Ctrl+Q") + ## キーボードショートカットを設定 + self.bind_all("", self.menu_export) + 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) + + #### 仕切り線で大きさを変更できるウィンドウ #### + paned_window = tk.PanedWindow(self.master, sashwidth=3, bg="gray", relief=tk.RAISED) + paned_window.pack(expand=True, fill=tk.BOTH) + + #### ツールボタン群とレイヤーリストを表示するフレーム #### + self.tools = Tools(paned_window, self.theme, width=300, bg=self.theme["main"]) + paned_window.add(self.tools, minsize=self.tools.min_width, padx=2, pady=2) + ## マップ画像を表示するフレームを追加 + paned_window.add(self.tools.map_disp, minsize=self.tools.map_disp.min_width, padx=2, pady=2) + return + + + """ + ++++++++++ File menu functions ++++++++++ + """ + def menu_open_base(self, event=None): + map_path = self.menu_open(title="Select base map yaml file") + if not map_path: return + waypoints_path = self.menu_open(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()) + self.open_menu.entryconfigure("Base map", state="disabled") + self.open_menu.entryconfigure("Additional map", state="normal") + return + + + def menu_open_addtion(self, event=None): + map_path = self.menu_open(title="Select additional map yaml file") + if not map_path: return + waypoints_path = self.menu_open(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 + + + def menu_open(self, title): + filepath = tkinter.filedialog.askopenfilename( + parent=self.master, + title=title, + initialdir=str(Path(".")), + filetypes=[("YAML", ".yaml")] + ) + return filepath + + + def menu_export(self, event=None): + if (len(self.tools.label_list) < 2): return + win = tk.Toplevel() + win.geometry("800x300+50+50") + win.minsize(width=500, height=300) + win.attributes('-topmost', True) + win.title("Export") + font = ("Consolas", 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( + parent=win, + title="Directory path to export maps", + initialdir=init_dir + ) + 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 + + ##　マルチマップ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, 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) + + ### それぞれをファイルに書き込む関数（ボタンコールバック） ### + def export_btn_callback(): + # multi maps + path = tbox1.get() + for i, img in enumerate(map_img_list): + img_path = Path(path) / Path("map{}.pgm".format(i)) + img.save(str(img_path.resolve())) + map_yaml_list[i]["image"] = str(img_path.resolve()) + str_yaml = "" + line = "\n" + for key, val in map_yaml_list[i].items(): + str_yaml += "{}: {}".format(key, val) + line + with open(str(img_path.with_suffix(".yaml")), 'w') as f: + f.write(str_yaml) + + # merged waypoints + path = tbox2.get() + with open(path, 'w') as f: + f.write(wp_yaml) + + # merged map + path = Path(tbox3.get()).resolve() + new_merged_yaml = re.sub("image: .*\n", "image: {}\n".format(str(path.with_suffix(".pgm"))), merged_yaml) + with open(str(path.with_suffix(".yaml")), 'w') as f: + f.write(new_merged_yaml) + merged_img_pil.save(str(path.with_suffix(".pgm"))) + win.destroy() + return + + ## エクスポートボタン、キャンセルボタン + export_btn = tk.Button(win, text="Export", font=font) + export_btn["command"] = export_btn_callback + export_btn.pack(side=tk.RIGHT, padx=50, pady=20) + cancel_btn = tk.Button(win, text="Cancel", font=font) + cancel_btn["command"] = win.destroy + cancel_btn.pack(side=tk.LEFT, padx=50, pady=20) + return + + + def menu_exit(self, event=None): + self.master.destroy() + diff --git a/apps/mylib/mapdisp.py b/apps/mylib/mapdisp.py new file mode 100644 index 0000000..2f54013 --- /dev/null +++ b/apps/mylib/mapdisp.py @@ -0,0 +1,399 @@ +import tkinter as tk +import numpy as np +import math +from PIL import Image, ImageTk +from pathlib import Path +from mylib.waypointlib import WaypointList, FinishPose + + +class MyMap(): + + def __init__(self, path: Path, map_yaml): + self.yaml_path = path.resolve() + if map_yaml["image"][0] == "/": + self.img_path = map_yaml["image"] + else: + self.img_path = self.yaml_path.with_name(map_yaml["image"]).resolve() + self.original_img_pil = Image.open(self.img_path).convert("RGBA") + self.pil_img = self.original_img_pil.copy() + self.tk_img = ImageTk.PhotoImage(self.pil_img) + self.origin = map_yaml["origin"] + self.resolution = map_yaml["resolution"] + self.map_yaml = map_yaml + self.mat_affine = np.eye(3) + self.img_origin = [-self.origin[0]/self.resolution, self.original_img_pil.height+self.origin[1]/self.resolution] + return + + + def translate(self, x, y): + self.affine(x, y, 1) + return + + + def scale_at(self, x, y, scale): + self.affine(-x, -y, 1) + self.affine(0, 0, scale) + self.affine(x, y, 1) + return + + + def rotate(self, theta, canv_center=None): + if not canv_center: + cx, cy = self.original_img_pil.width/2, self.original_img_pil.height/2 + canv_center = np.dot(self.mat_affine, [cx, cy, 1]) + self.translate(-canv_center[0], -canv_center[1]) + mat = [[math.cos(theta), -math.sin(theta), 0], + [math.sin(theta), math.cos(theta), 0], + [0, 0, 1]] + self.mat_affine = (np.dot(mat, self.mat_affine)) + self.translate(canv_center[0], canv_center[1]) + return + + + def affine(self, x, y, scale): + x = float(x) + y = float(y) + scale = float(scale) + mat = [[scale,0,x], [0,scale,y], [0,0,1]] + self.mat_affine = np.dot(mat, self.mat_affine) + return + + + def get_draw_image(self, canvas_size): + mat_inv = np.linalg.inv(self.mat_affine) + self.pil_img = self.original_img_pil.transform(canvas_size, + Image.Transform.AFFINE, tuple(mat_inv.flatten()), Image.Resampling.NEAREST, + fillcolor="#0000" + ) + self.tk_img = ImageTk.PhotoImage(image=self.pil_img) + return self.tk_img + + + def get_corners(self): + # 1 2 画像の四隅の座標を左の順番で返す + # 4 3 + w, h = self.original_img_pil.width, self.original_img_pil.height + xy1 = self.mat_affine[:, 2] + xy2 = np.dot(self.mat_affine, [w, 0, 1]) + xy3 = np.dot(self.mat_affine, [w, h, 1]) + xy4 = np.dot(self.mat_affine, [0, h, 1]) + return xy1[0], xy1[1], xy2[0], xy2[1], xy3[0], xy3[1], xy4[0], xy4[1] + + + def set_transparency(self, a): + self.original_img_pil.putalpha(int(a/100*255)) + return + + + def transform(self, img_x, img_y): + mat = [img_x, img_y, 1] + tf_mat = np.dot(self.mat_affine, mat) + return tf_mat[0], tf_mat[1] + + + def inv_transform(self, x, y): + mat = [x, y, 1] + inv_affine = np.linalg.inv(self.mat_affine) + inv = np.dot(inv_affine, mat) + return inv[0], inv[1] + + + def image2real(self, img_x, img_y): + real_x = (img_x - self.img_origin[0]) * self.resolution + real_y = -(img_y - self.img_origin[1]) * self.resolution + real_x = round(real_x, 6) + real_y = round(real_y, 6) + return real_x, real_y + + + def real2image(self, real_x, real_y): + img_x = self.img_origin[0] + real_x / self.resolution + img_y = self.img_origin[1] - real_y / self.resolution + return img_x, img_y + + + def get_rotate_angle(self): + return math.atan2(self.mat_affine[1,0], self.mat_affine[0,0]) + + + + +class MapDisplay(tk.Frame): + + def __init__(self, master, theme, **kwargs): + super().__init__(master, kwargs) + self.min_width = 400 + + self.canvas = tk.Canvas(self, bg="black", highlightthickness=0) + self.canvas.pack(expand=True, fill=tk.BOTH, padx=5, pady=5) + self.update() + self.canvas.bind("", self.left_click_move) + self.canvas.bind("", self.left_click) + self.canvas.bind("", self.left_click_release) + self.canvas.bind("", lambda event, scale=1.1: self.ctrl_click(event, scale)) + self.canvas.bind("", lambda event, scale=0.9: self.ctrl_click(event, scale)) + self.canvas.bind("", self.resize_callback) + + self.canv_w = self.canvas.winfo_width() + self.canv_h = self.canvas.winfo_height() + self.base_scale = 1 + self.map_dict = {} + self.waypoints_dict = {} + self.theme = theme + self.old_click_point = None + 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 + + # mode を識別するための定数 + self.Normal = 0 + self.MoveSelected = 1 + self.RotateSelected = 2 + return + + + + def add_map(self, path: Path, map_yaml, waypoints_yaml: Path, base=False): + key = self.path2key(path) + if key in self.map_dict.keys(): + return False + new_map = MyMap(path, map_yaml) + img_w = new_map.original_img_pil.width + img_h = new_map.original_img_pil.height + scale = 1 + offset_x = 0 + offset_y = 0 + if base: + if (self.canv_w / self.canv_h) > (img_w / img_h): + # canvasの方が横長　画像をcanvasの縦に合わせてリサイズ + scale = self.canv_h / img_h + offset_x = (self.canv_w - img_w*scale) / 2 + else: + # canvasの方が縦長　画像をcanvasの横に合わせてリサイズ + scale = self.canv_w / img_w + offset_y = (self.canv_h - img_h*scale) / 2 + self.base_map_key = key + 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 + + # 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, + image=new_map.get_draw_image((self.canv_w, self.canv_h)) + ) + self.map_dict[key] = new_map + self.select_map(path) + self.plot_origin() + # waypoints + self.waypoints_dict[key] = WaypointList(waypoints_yaml) + self.waypoints_dict[key+"fp"] = FinishPose(waypoints_yaml) + self.plot_waypoints() + return True + + + def plot_origin(self): + base_map = 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 + y1 = canv_origin[1] - r + x2 = canv_origin[0] + r + 1 + y2 = canv_origin[1] + r + 1 + if self.canvas.find_withtag("origin"): + self.canvas.moveto("origin", x1, y1) + else: + self.canvas.create_oval(x1, y1, x2, y2, tags="origin", fill='cyan', outline='blue') + self.canvas.lift("origin") + return + + + def plot_waypoints(self): + for key, wp_list in self.waypoints_dict.items(): + if key[-2:] == "fp": + finish_pose = 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() + x0 = cx + y0 = cy + x1 = x0 + math.cos(th) * self.point_rad * 3 + y1 = y0 - math.sin(th) * self.point_rad * 3 + if self.waypoints_dict[key].id is not None: + self.canvas.delete(self.waypoints_dict[key].id) # movetoだとX,毎回削除,再描画 + self.waypoints_dict[key].id = self.canvas.create_line(x0, y0, x1, y1, tags=key, + width=5, arrow=tk.LAST, arrowshape=(4,5,3), fill="#AAF" + ) + self.canvas.lift(self.waypoints_dict[key].id, key[:-2]) + continue + + 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]) + self.waypoints_dict[key].set_id(n+1, id) + else: + for id in wp_list.get_id_list(): + wp = wp_list.get_waypoint(id) + img_x, img_y = self.map_dict[key].real2image(float(wp["x"]), float(wp["y"])) + cx, cy = self.map_dict[key].transform(img_x, img_y) + x0, y0 = cx-self.point_rad, cy-self.point_rad + self.canvas.moveto(id, round(x0), round(y0)) + self.canvas.lift(id, key) + return + + + def create_waypoint(self, 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 + x0 = round(cx - r) + 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') + return id + + + def set_transparency(self, path: Path, alpha): + key = self.path2key(path) + self.map_dict[key].set_transparency(alpha) + self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) + + + + def select_map(self, path: Path): + self.selected_map_key = self.path2key(path) + polygon = self.map_dict[self.selected_map_key].get_corners() + if self.canvas.find_withtag("selection_frame"): + self.canvas.coords("selection_frame", polygon) + else: + 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() + return + + + def set_vision_state(self, path: Path, vision: bool): + key = self.path2key(path) + if vision: + self.canvas.itemconfigure(key, state=tk.NORMAL) + else: + self.canvas.itemconfigure(key, state=tk.HIDDEN) + + + + def path2key(self, path: Path): + return path.with_suffix("").name + + + def get_map(self, path: Path): + key = self.path2key(path) + return self.map_dict[key] + + + def left_click_move(self, event): + if (len(self.map_dict) == 0): 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.old_click_point = [event.x, event.y] + + if self.mode == self.RotateSelected: + # 選択されているマップを回転する + if self.rotate_center is None: + self.rotate_center = self.old_click_point + r = self.point_rad + x1, y1 = self.rotate_center[0]-r, self.rotate_center[1]-r + x2, y2 = self.rotate_center[0]+r, self.rotate_center[1]+r + self.canvas.create_oval(x1, y1, x2, y2, fill="#FFA", outline="#FF0", tags="rotation_center") + return + key = self.selected_map_key + cx, cy = self.rotate_center + if (abs(event.x-cx) < 20) and (abs(event.y-cy) < 20): return + map_rot = math.atan2((event.y - cy), (event.x - cx)) + if self.old_map_rot is None: + self.old_map_rot = map_rot + return + theta = map_rot - self.old_map_rot + self.map_dict[key].rotate(theta, canv_center=self.rotate_center) + self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) + if self.canvas.find_withtag("rotation_line"): + self.canvas.coords("rotation_line", cx, cy, event.x, event.y) + else: + self.canvas.create_line(cx, cy, event.x, event.y, tags="rotation_line", width=1, fill="#FAA") + polygon = polygon = self.map_dict[key].get_corners() + self.canvas.coords("selection_frame", polygon) + self.canvas.lift("origin") + self.plot_waypoints() + self.old_map_rot = map_rot + return + + elif self.mode == self.MoveSelected: + # 選択されているマップのみ移動する + key = self.selected_map_key + self.map_dict[key].translate(delta_x, delta_y) + self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) + + else: + # 全てのマップを移動する + for key in self.map_dict.keys(): + self.map_dict[key].translate(delta_x, delta_y) + self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) + self.canvas.move("origin", delta_x, delta_y) + + self.canvas.move("selection_frame", delta_x, delta_y) + self.canvas.lift("origin") + self.plot_waypoints() + return + + + def ctrl_click(self, event, scale): + if (len(self.map_dict) == 0): return + self.base_scale *= scale + for key in self.map_dict.keys(): + self.map_dict[key].scale_at(event.x, event.y, scale) + self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) + + self.plot_origin() + self.plot_waypoints() + polygon = polygon = self.map_dict[self.selected_map_key].get_corners() + self.canvas.coords("selection_frame", polygon) + self.canvas.lift("selection_frame") + self.canvas.lift("origin") + return + + + def left_click(self, event): + if (self.mode != self.RotateSelected): return + self.old_click_point = [event.x, event.y] + return + + + def left_click_release(self, event): + self.old_click_point = None + self.old_map_rot = None + self.rotate_center = None + if self.canvas.find_withtag("rotation_line"): + self.canvas.delete("rotation_line") + if self.canvas.find_withtag("rotation_center"): + self.canvas.delete("rotation_center") + return + + + def resize_callback(self, event): + self.canv_w = self.canvas.winfo_width() + self.canv_h = self.canvas.winfo_height() + return + + diff --git a/apps/mylib/tools.py b/apps/mylib/tools.py new file mode 100644 index 0000000..6d743b5 --- /dev/null +++ b/apps/mylib/tools.py @@ -0,0 +1,375 @@ +import tkinter as tk +from turtle import fillcolor +import numpy as np +import ruamel.yaml +import math +from PIL import Image, ImageTk +from pathlib import Path +from .mapdisp import MapDisplay, MyMap +from .waypointlib import FinishPose, WaypointList, get_waypoint_yaml + + + +class LayerLabel(tk.Label): + + def __init__(self, master, *args, **kwargs): + super().__init__(master, *args, **kwargs) + self.map_path = Path() + self.map_transparency = tk.IntVar() + self.map_transparency.set(100) + + + def set_map_path(self, path: Path): + self.map_path = path + + + +class Tools(tk.Frame): + + def __init__(self, master, theme, **kwargs): + super().__init__(master, kwargs) + self.min_width = 200 + width = self.cget("width") + + #### ツールボタン群を配置 #### + self.btn_frame = tk.Frame(self, height=200, width=width, bg=theme["main"]) + self.btn_frame.pack(expand=False, fill=tk.X) + ## move ボタン + icon = Image.open(Path(__file__).parent.parent / Path("icon","move_button.png")) + icon = icon.resize((30, 30)) + self.move_btn_icon = ImageTk.PhotoImage(image=icon) + self.move_btn = tk.Label(self.btn_frame, image=self.move_btn_icon, bg=theme["main"]) + self.move_btn.bind("", lambda event, btn=self.move_btn: self.btn_entry(event, btn)) + self.move_btn.bind("", lambda event, btn=self.move_btn: self.btn_leave(event, btn)) + self.move_btn.bind("", lambda event, btn=self.move_btn, mode="move": self.btn_clicked(event, btn, mode)) + self.move_btn.grid(column=0, row=0) + ## rotate ボタン + icon = Image.open(Path(__file__).parent.parent / Path("icon","rotate_button.png")) + icon = icon.resize((30, 30)) + self.rot_btn_icon = ImageTk.PhotoImage(image=icon) + self.rot_btn = tk.Label(self.btn_frame, image=self.rot_btn_icon, bg=theme["main"]) + self.rot_btn.bind("", lambda event, btn=self.rot_btn: self.btn_entry(event, btn)) + 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) + ## 余白 + 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)) + self.layer_icon = ImageTk.PhotoImage(image=icon) + layer_label = tk.Label(self, bg=theme["bg1"], + text=" Layers", 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) + ## レイヤーリストを表示するフレームを配置 + self.layers_frame = tk.Frame(self, width=width, bg=theme["bg1"]) + self.layers_frame.pack(expand=True, fill=tk.BOTH, padx=5) + ## 透明度調節用のスクロールバーを配置 + self.tra_bar = tk.Scale(self.layers_frame, from_=0, to=100, resolution=1, highlightthickness=0, + label="Transparency", fg="white", font=("Ariel",9,"bold"), bg=theme["bg1"], + activebackground="grey", borderwidth=2, sliderlength=15, sliderrelief=tk.RAISED, + orient=tk.HORIZONTAL, showvalue=True, state=tk.DISABLED, command=self.change_transparency + ) + self.tra_bar.pack(expand=False, fill=tk.X, padx=3, pady=5) + ## アイコンを読み込む + icon = Image.open(Path(__file__).parent.parent / Path("icon","visible.png")) + icon = icon.resize((18, 17)) + self.visible_icon = ImageTk.PhotoImage(image=icon) + icon = Image.open(Path(__file__).parent.parent / Path("icon","invisible.png")) + icon = icon.resize((18, 17)) + self.invisible_icon = ImageTk.PhotoImage(image=icon) + icon = Image.open(Path(__file__).parent.parent / Path("icon","folder.png")) + icon = icon.resize((20, 14)) + self.folder_icon = ImageTk.PhotoImage(image=icon) + + #### マップ画像を表示するフレームを生成 #### + self.map_disp = MapDisplay(self.master, theme, bg=theme["main"]) + + #### 変数 + self.label_list = [] + self.base_map_layer = LayerLabel(self.layers_frame) + self.selected_layer = LayerLabel(self.layers_frame) + self.base_waypoints_path = Path() + self.theme = theme + return + + + + def btn_entry(self, event, btn: tk.Label): + if btn.cget("bg") == "black": return + btn.configure(bg="#333") + + + def btn_leave(self, event, btn: tk.Label): + if btn.cget("bg") == "black": return + btn.configure(bg=self.theme["main"]) + + + 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 btn.cget("bg") == "black": + self.map_disp.mode = self.map_disp.Normal + btn.configure(bg="#333") + return + # 既に他のモードだった場合ボタンの状態を切り替える + if self.map_disp.mode == self.map_disp.MoveSelected: self.move_btn.configure(bg=self.theme["main"]) + elif self.map_disp.mode == self.map_disp.RotateSelected: self.rot_btn.configure(bg=self.theme["main"]) + # MapDisplayにモードを設定 + if mode == "move": self.map_disp.mode = self.map_disp.MoveSelected + elif mode == "rotate": self.map_disp.mode = self.map_disp.RotateSelected + btn.configure(bg="black") + return + + + + def set_base_map(self, map_path: Path, wp_path: Path): + map_yaml = self.read_file(map_path) + wp_yaml = self.read_file(wp_path) + 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 + return + + + def add_map(self, path: Path, wp_path: Path): + map_yaml = self.read_file(path) + wp_yaml = self.read_file(wp_path) + if self.map_disp.add_map(path, map_yaml, wp_yaml): + self.append_layer(path) + else: + print("Selected map is already exist") + return + + + def read_file(self, path: Path): + with open(path) as file: # .yamlを読み込む + yaml = ruamel.yaml.YAML().load(file) + return yaml + + + def append_layer(self, path: Path, base=None): + name = " " + path.with_suffix("").name + font = ("Arial",10) + if base: + name += " ( base )" + font = ("Arial", 10, "bold") + label = LayerLabel(self.layers_frame, bg=self.theme["main"], bd=0, + text=name, fg="white", font=font, anchor=tk.W, padx=10, pady=2, + image=self.visible_icon, compound=tk.LEFT + ) + label.set_map_path(path) + label.bind("", lambda event, label=label: self.layerlabel_clicked(event, label)) + if self.label_list: + label.pack(expand=False, side=tk.TOP, fill=tk.X, padx=2, pady=2, ipady=3, before=self.label_list[-1]) + else: + label.pack(expand=False, side=tk.TOP, fill=tk.X, padx=2, pady=2, ipady=3) + + if self.selected_layer is not None: + 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.label_list.append(label) + if base: self.base_map_layer = label + return + + + def layerlabel_clicked(self, event, label: LayerLabel): + px = label.cget("padx") + imw = self.visible_icon.width() + if (px-3 < event.x) and (event.x < px+imw+3): + # 可視アイコンがクリックされたとき + fg = label.cget("fg") + if fg == "white": + label.configure(image=self.invisible_icon, fg="grey") + self.map_disp.set_vision_state(label.map_path, vision=False) + 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"]) + label.configure(bg=self.theme["main"]) + self.selected_layer = label + self.tra_bar.configure(variable=self.selected_layer.map_transparency) + self.map_disp.select_map(self.selected_layer.map_path) + return + + + def change_transparency(self, event): + alpha = self.selected_layer.map_transparency.get() + self.map_disp.set_transparency(self.selected_layer.map_path, alpha) + 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 + img_list = [base_map.original_img_pil] + yaml_list = [base_yaml] + for label in self.label_list[1:]: + mymap: MyMap = self.map_disp.get_map(label.map_path) + # image + img = mymap.original_img_pil.convert("L") + theta = math.degrees(mymap.get_rotate_angle()) + img = img.rotate(theta, expand=True, fillcolor=205) + img_list.append(img) + # yaml + yaml = mymap.map_yaml + corners = np.array(mymap.get_corners()) + left = corners[[0,2,4,6]].min() + lower = corners[[1,3,5,7]].max() + img_x, img_y = base_map.inv_transform(left, lower) + 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 + + + def get_waypoints_yaml(self): + # ベースのウェイポイントをコピー + 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"] + wpc = waypoints.get_waypoint(num=1).copy() + wpc["x"] = finish_pose.x + wpc["y"] = finish_pose.y + wpc["stop"] = "true" + wpc["change_map"] = "true" + waypoints.append(wpc) + # 残りのウェイポイントを追加していく + base_map: MyMap = self.map_disp.map_dict[self.map_disp.base_map_key] + name = "waypoints-" + base_map.yaml_path.with_suffix("").name + for label in self.label_list[1:]: + mymap: MyMap = self.map_disp.get_map(label.map_path) + name += "-" + mymap.yaml_path.with_suffix("").name + # 2つ目以降の地図のもともとの原点のキャンバス上の位置を取得 + origin = mymap.transform(mymap.img_origin[0], mymap.img_origin[1]) + # ベースのマップから見た原点の座標を計算 + img_x, img_y = base_map.inv_transform(origin[0], origin[1]) + real_x, real_y = base_map.image2real(img_x, img_y) + # 地図の回転角 + theta = -mymap.get_rotate_angle() + # 2つ目以降の地図に対応するウェイポイントの座標を、ベースの地図の座標系に変換する同次変換行列 + mat = [[math.cos(theta), -math.sin(theta), real_x], + [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(): + 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) + # 2つ目以降の地図のfinish poseも変換 + finish_pose = FinishPose(self.map_disp.waypoints_dict[self.map_disp.path2key(label.map_path)].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 + # 最後以外のfinish poseはウェイポイントとして追加 + if label != self.label_list[-1]: + wpc = waypoints.get_waypoint(num=1).copy() + wpc["x"] = finish_pose.x + wpc["y"] = finish_pose.y + 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), init_path + + + def get_merged_map(self): + # 1 2 画像の四隅のキャンバス上の座標を取得 + # 4 3 + base_map: MyMap = self.map_disp.map_dict[self.map_disp.base_map_key] + corners = base_map.get_corners() # x1, y1, x2, y2, x3, y3, x4, y4 + img_corners = np.array(corners) + # ベースの地図画像を基準として画像上の座標に変換 + for i in range(0,8,2): + img_corners[i:i+2] = base_map.inv_transform(corners[i], corners[i+1]) + for label in self.label_list[1:]: + mymap: MyMap = self.map_disp.get_map(label.map_path) + corners = np.array(mymap.get_corners()) + for i in range(0,8,2): + corners[i:i+2] = base_map.inv_transform(corners[i], corners[i+1]) + img_corners = np.vstack((img_corners, corners)) + # 合成する地図画像の中で最も外側にある座標を取得 + left = img_corners[:, [0, 2, 4, 6]].min() + upper = img_corners[:, [1, 3, 5, 7]].min() + right = img_corners[:, [0, 2, 4, 6]].max() + lower = img_corners[:, [1, 3, 5, 7]].max() + # 合成後の画像を準備 + 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 + free_area = np.full(img_size, 0, dtype=np.uint8) # 走行可能領域が255, それ以外が0 + # ベースの画像を合成 + img = np.array(base_map.original_img_pil.convert("L"), dtype=np.uint8) + x, y = int(round(img_corners[0,0]-left)), int(round(img_corners[0,1]-upper)) + h, w = img.shape + obstacles[y:y+h, x:x+w] = obstacles[y:y+h, x:x+w] & np.where(img>100, 255, 0) + free_area[y:y+h, x:x+w] = free_area[y:y+h, x:x+w] | np.where(img<230, 0, 255) + name = "merged-" + base_map.yaml_path.with_suffix("").name + # その他の画像も合成 + for i in range(1, len(self.label_list)): + label = self.label_list[i] + mymap: MyMap = self.map_disp.get_map(label.map_path) + img = mymap.original_img_pil.convert("L") + img = img.rotate(-math.degrees(mymap.get_rotate_angle()), + resample=Image.Resampling.NEAREST, expand=True, fillcolor=205) + img = np.array(img, dtype=np.uint8) + x = int(round(img_corners[i, [0, 2, 4, 6]].min() - left)) + y = int(round(img_corners[i, [1, 3, 5, 7]].min() - upper)) + h, w = img.shape + obstacles[y:y+h, x:x+w] = obstacles[y:y+h, x:x+w] & np.where(img>100, 255, 0) + free_area[y:y+h, x:x+w] = free_area[y:y+h, x:x+w] | np.where(img<230, 0, 255) + name += "-" + Path(label.map_path).with_suffix("").name + # 未確定領域、走行可能領域、障害物を合成し、画像に変換 + merged_img = (unknown | free_area) & obstacles + merged_img = merged_img[:-offset, :-offset] # offset分を削除 + merged_img = Image.fromarray(merged_img, "L") + # 原点から合成後の画像の左下までの距離 + 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" + init_path = base_map.yaml_path.with_name(name).resolve() + return merged_img, merged_yaml, init_path + diff --git a/apps/mylib/waypointlib.py b/apps/mylib/waypointlib.py new file mode 100644 index 0000000..5174be1 --- /dev/null +++ b/apps/mylib/waypointlib.py @@ -0,0 +1,106 @@ +import numpy as np +import quaternion + + +class WaypointList(): + def __init__(self, wp_yaml): + self.waypoints = [] + for point in wp_yaml["waypoints"]: + wp = {} + for key, val in point["point"].items(): + wp[key] = val + self.append(wp) + + self.point_keys = self.waypoints[0].keys() + self.number_dict = {} + self.waypoints_yaml = wp_yaml + return + + + def append(self, waypoint: dict, id=None): + self.waypoints.append(waypoint) + if id: self.set_id(len(self.waypoints), id) + return len(self.waypoints) + + + def insert(self, num, waypoint: dict, id=None): + self.waypoints.insert(num-1, waypoint) + for i, n in self.number_dict.items(): + self.number_dict[i] = n+1 if (n>=num) else n + if id: self.set_id(num, id) + return + + + def get_waypoint(self, id=None, num=None): + if id: return self.waypoints[self.get_num(str(id)) - 1] + if num: return self.waypoints[num-1] + return self.waypoints + + + def get_id_list(self): + return self.number_dict.keys() + + + def set_id(self, num, id): + self.number_dict[str(id)] = num + return + + + def get_num(self, id): + return self.number_dict[str(id)] + + + + +class FinishPose(): + def __init__(self, wp_yaml): + super().__init__() + + self.header = {} + for key, val in wp_yaml["finish_pose"]["header"].items(): + self.header[key] = val + + self.position = {} + for key, val in wp_yaml["finish_pose"]["pose"]["position"].items(): + self.position[key] = val + + self.orientation = {} + for key, val in wp_yaml["finish_pose"]["pose"]["orientation"].items(): + self.orientation[key] = val + + self.x = self.position["x"] + self.y = self.position["y"] + self.yaw = self.get_euler() + self.id = None + return + + + def get_euler(self): + o = self.orientation + q = np.quaternion(o["x"], o["y"], o["z"], o["w"]) + return quaternion.as_euler_angles(q)[1] + + + + +def get_waypoint_yaml(waypoints: WaypointList, finish_pose: FinishPose): + s = ["waypoints:" + "\n"] + for point in waypoints.get_waypoint(): + s.append("- point: {") + for i, (key, val) in enumerate(point.items()): + if (i != 0): s.append(", ") + s.append(key + ": " + str(val).lower()) + s.append("}" + "\n") + + s.append("finish_pose:" + "\n") + seq, stamp, frame = (finish_pose.header["seq"], finish_pose.header["stamp"], finish_pose.header["frame_id"]) + s.append(" header: {" + "seq: {}, stamp: {}, frame_id: {}".format(seq,stamp,frame) + "}" + "\n") + s.append(" pose:" + "\n") + x, y, z = finish_pose.x, finish_pose.y, finish_pose.position["z"] + s.append(" position: {" + "x: {}, y: {}, z: {}".format(x, y, z) + "}" + "\n") + q = quaternion.from_euler_angles([0, 0, finish_pose.yaw]) + s.append(" orientation: {" + "x: {}, y: {}, z: {}, w: {}".format(q.x, q.y, q.z, q.w) + "}") + return "".join(s) + + + \ No newline at end of file diff --git a/icon/folder.png b/icon/folder.png deleted file mode 100644 index 397b8cf..0000000 --- a/icon/folder.png +++ /dev/null Binary files differ diff --git a/icon/invisible.png b/icon/invisible.png deleted file mode 100644 index de968b3..0000000 --- a/icon/invisible.png +++ /dev/null Binary files differ diff --git a/icon/layer.png b/icon/layer.png deleted file mode 100644 index 2ccd413..0000000 --- a/icon/layer.png +++ /dev/null Binary files differ diff --git a/icon/move_button.png b/icon/move_button.png deleted file mode 100644 index 8229622..0000000 --- a/icon/move_button.png +++ /dev/null Binary files differ diff --git a/icon/rotate_button.png b/icon/rotate_button.png deleted file mode 100644 index 3a3a019..0000000 --- a/icon/rotate_button.png +++ /dev/null Binary files differ diff --git a/icon/visible.png b/icon/visible.png deleted file mode 100644 index 9fa24fc..0000000 --- a/icon/visible.png +++ /dev/null Binary files differ diff --git a/main.py b/main.py deleted file mode 100644 index dfbc869..0000000 --- a/main.py +++ /dev/null @@ -1,11 +0,0 @@ -import tkinter as tk -from mylib.application import Application - - -if __name__ == "__main__": - root = tk.Tk() - w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-100 - root.geometry("%dx%d+0+0" % (w, h)) - root.title("Multi Map & Waypoints Merger") - app = Application(root) - app.mainloop() \ No newline at end of file diff --git a/map_trimmer.py b/map_trimmer.py deleted file mode 100644 index 24b689a..0000000 --- a/map_trimmer.py +++ /dev/null @@ -1,299 +0,0 @@ -import tkinter as tk -import tkinter.filedialog -import ruamel.yaml -from pathlib import Path -from mylib.mapdisp import MyMap -from PIL import Image, ImageDraw - - -class Application(tk.Frame): - - def __init__(self, master): - super().__init__(master) - self.theme = {"main": "#444", - "bg1": "#222"} - - #### 画面上部のメニューバーを作成 #### - self.menu_bar = tk.Menu(self) - ## Fileメニューの作成 - self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF, - bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5 - ) - self.menu_bar.add_cascade(label=" File ", menu=self.file_menu) - ## File -> Open - self.file_menu.add_command(label="Open", command=self.menu_open, accelerator="Ctrl+O") - ## File -> Save As - self.file_menu.add_command(label="Save As", command=self.menu_saveas, accelerator="Ctrl+Shift+S") - ## File -> Exit メニュー - self.file_menu.add_separator() # 仕切り - self.file_menu.add_command(label="Exit", command=self.menu_exit, accelerator="Ctrl+Q") - ## Edit メニューの作成 - self.edit_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF, - bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5 - ) - self.menu_bar.add_cascade(label=" Edit ", menu=self.edit_menu) - ## Edit -> Trim - self.edit_menu.add_command(label="Trim", command=self.menu_trim, accelerator="Ctrl+T") - - #### メニューバーを設定 #### - self.master.configure(menu=self.menu_bar) - - #### キーバインド #### - self.bind_all("", self.menu_trim) - self.bind_all("", self.menu_saveas) - self.bind_all("", self.menu_open) - self.bind_all("", self.menu_exit) - - #### マップを表示するキャンバス #### - self.canvas = tk.Canvas(self.master, bg="black", highlightthickness=0) - self.canvas.pack(expand=True, fill=tk.BOTH, padx=5, pady=5) - self.update() - self.canv_w = self.canvas.winfo_width() - self.canv_h = self.canvas.winfo_height() - - #### マウスイベントコールバック #### - self.canvas.bind("", self.left_click_move) - self.canvas.bind("", self.left_click) - self.canvas.bind("", self.left_click_release) - self.canvas.bind("", lambda event, scale=1.1: self.ctrl_click(event, scale)) - self.canvas.bind("", lambda event, scale=0.9: self.ctrl_click(event, scale)) - self.canvas.bind("", self.resize_callback) - - #### 変数 #### - self.mymap = None - self.origin_img = [] - self.trim_range = [] - self.trimming_mode = False - return - - - - """ - ++++++++++ File menu functions ++++++++++ - """ - def menu_open(self, event=None): - filepath = tkinter.filedialog.askopenfilename( - parent=self.master, - title="Select map yaml file", - initialdir=str(Path(".")), - filetypes=[("YAML", ".yaml")] - ) - if not filepath: return - with open(filepath) as file: # .yamlを読み込む - map_yaml = ruamel.yaml.YAML().load(file) - - self.mymap = MyMap(Path(filepath).resolve(), map_yaml) - img_w = self.mymap.original_img_pil.width - img_h = self.mymap.original_img_pil.height - scale = 1 - offset_x = 0 - offset_y = 0 - if (self.canv_w / self.canv_h) > (img_w / img_h): - # canvasの方が横長　画像をcanvasの縦に合わせてリサイズ - scale = (self.canv_h) / img_h - offset_x = (self.canv_w - img_w*scale) / 2 - else: - # canvasの方が縦長　画像をcanvasの横に合わせてリサイズ - scale = (self.canv_w) / img_w - offset_y = (self.canv_h - img_h*scale) / 2 - - self.mymap.scale_at(0, 0, scale) - self.mymap.translate(offset_x, offset_y) - self.canvas.create_image(0, 0, anchor=tk.NW, tags="map", - image=self.mymap.get_draw_image((self.canv_w, self.canv_h)) - ) - self.trim_range = [0, 0, img_w, img_h] # [x1, y1, x2, y2] 画像における左上のxy, 右下のxy - origin = self.mymap.origin - resolution = self.mymap.resolution - self.origin_img = [-origin[0]/resolution, origin[1]/resolution+self.mymap.original_img_pil.height] - self.plot_origin() - self.master.title(str(filepath)) - return - - - def menu_saveas(self, event=None): - new_filepath = tkinter.filedialog.asksaveasfilename( - parent=self.master, - title="Save map YAML and PGM files.", - initialdir=str(self.mymap.yaml_path), - filetypes=[("PGM YAML", ".pgm .yaml")], - ) - if len(new_filepath) == 0: return - # origin - origin = [(self.trim_range[0]-self.origin_img[0])*self.mymap.resolution, - (-self.trim_range[3]+self.origin_img[1])*self.mymap.resolution, - 0.0] - # Write map yaml file - yaml_path = Path(new_filepath).with_suffix(".yaml") - line = "\n" - yaml = ["image: ./" + yaml_path.with_suffix(".pgm").name + line] - yaml.append("resolution: " + str(self.mymap.resolution) + line) - yaml.append("origin: " + str(origin) + line) - yaml.append("negate: " + str(self.mymap.map_yaml["negate"]) + line) - yaml.append("occupied_thresh: " + str(self.mymap.map_yaml["occupied_thresh"]) + line) - yaml.append("free_thresh: " + str(self.mymap.map_yaml["free_thresh"]) + line) - with open(yaml_path.resolve(), 'w') as f: - f.write("".join(yaml)) - # Trim and save map image - trimmed_img = self.mymap.original_img_pil.crop(tuple(self.trim_range)) - trimmed_img.save(str(yaml_path.with_suffix(".pgm"))) - return - - - def menu_exit(self, event=None): - self.master.destroy() - - - - """ - ++++++++++ Edit menu functions ++++++++++ - """ - def menu_trim(self, event=None): - if not self.mymap: return - if self.trimming_mode: - self.canvas.delete("upper", "right", "lower", "left", "ul", "ur", "lr", "ll") - self.trimming_mode = False - self.set_alpha(0) - return - self.trimming_mode = True - self.set_alpha(128) - r = 10 - ul_x, ul_y = self.mymap.transform(self.trim_range[0], self.trim_range[1]) - lr_x, lr_y = self.mymap.transform(self.trim_range[2], self.trim_range[3]) - cxy = [ul_x, ul_y, lr_x, ul_y, lr_x, lr_y, ul_x, lr_y] - # トリミング範囲の上下左右に直線を描画 - for tag, x1, y1, x2, y2 in [("upper",0,1,2,3), ("right",2,3,4,5), ("lower",4,5,6,7), ("left",6,7,0,1)]: - self.canvas.create_line(cxy[x1], cxy[y1], cxy[x2], cxy[y2], tags=tag, fill="#FFF", width=2) - #　四隅にマーカーを描画 - for tag, xidx, yidx in [("ul",0,1), ("ur",2,3), ("lr",4,5), ("ll",6,7)]: - cx, cy = cxy[xidx], cxy[yidx] - self.canvas.create_oval(cx-r, cy-r, cx+r, cy+r, tags=tag, fill="#BBB", outline="#FFF", activefill="#FFF") - self.canvas.tag_bind(tag, "", lambda event, tag=tag: self.move_trim_range(event, tag)) - return - - - def move_trim_range(self, event, tag): - 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.old_click_point = [event.x, event.y] - # 四隅の円の移動 - self.canvas.move(tag, delta_x, delta_y) - if tag == "ul": - self.canvas.move("ur", 0, delta_y) - self.canvas.move("ll", delta_x, 0) - elif tag == "ur": - self.canvas.move("ul", 0, delta_y) - self.canvas.move("lr", delta_x, 0) - elif tag == "lr": - self.canvas.move("ll", 0, delta_y) - self.canvas.move("ur", delta_x, 0) - else: # "ll" - self.canvas.move("lr", 0, delta_y) - self.canvas.move("ul", delta_x, 0) - # 左上のマーカーの座標を取得 - ul = self.canvas.bbox("ul") - ul_x = (ul[0] + ul[2]) / 2 - ul_y = (ul[1] + ul[3]) / 2 - # 右下の座標 - lr = self.canvas.bbox("lr") - lr_x = (lr[0] + lr[2]) / 2 - lr_y = (lr[1] + lr[3]) / 2 - # マーカー間の線を移動 - self.canvas.coords("upper", ul_x, ul_y, lr_x, ul_y) - self.canvas.coords("right", lr_x, ul_y, lr_x, lr_y) - self.canvas.coords("lower", lr_x, lr_y, ul_x, lr_y) - self.canvas.coords("left" , ul_x, lr_y, ul_x, ul_y) - # 画像上の位置に変換し、トリミングの範囲を更新 - img_ul_x, img_ul_y = self.mymap.inv_transform(ul_x, ul_y) - img_lr_x, img_lr_y = self.mymap.inv_transform(lr_x, lr_y) - img_ul_x = max(img_ul_x, 0) - img_ul_y = max(img_ul_y, 0) - img_lr_x = min(img_lr_x, self.mymap.original_img_pil.width) - img_lr_y = min(img_lr_y, self.mymap.original_img_pil.height) - self.trim_range = [img_ul_x, img_ul_y, img_lr_x, img_lr_y] - # 画像を更新して描画 - self.set_alpha(128) - return - - - def set_alpha(self, a: int): - im_a = Image.new("L", self.mymap.original_img_pil.size, a) - draw = ImageDraw.Draw(im_a) - draw.rectangle(tuple(self.trim_range), fill=255) - self.mymap.original_img_pil.putalpha(im_a) - self.canvas.itemconfigure("map", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) - return - - - - """ - ++++++++++ Plot and move point marker representing origin ++++++++++ - """ - def plot_origin(self): - canv_origin = self.mymap.transform(self.origin_img[0], self.origin_img[1]) - r = 5 - x1 = canv_origin[0] - r - y1 = canv_origin[1] - r - x2 = canv_origin[0] + r + 1 - y2 = canv_origin[1] + r + 1 - if self.canvas.find_withtag("origin"): - self.canvas.moveto("origin", x1, y1) - else: - self.canvas.create_oval(x1, y1, x2, y2, tags="origin", fill='cyan', outline='blue') - self.canvas.lift("origin") - return - - - - """ - ++++++++++ Mouse event callback functions ++++++++++ - """ - def left_click_move(self, event): - if (not self.mymap) or (self.trimming_mode) : 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.old_click_point = [event.x, event.y] - self.mymap.translate(delta_x, delta_y) - self.canvas.itemconfigure("map", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) - self.canvas.move("origin", delta_x, delta_y) - return - - - def ctrl_click(self, event, scale): - if (not self.mymap) or (self.trimming_mode): return - self.mymap.scale_at(event.x, event.y, scale) - self.canvas.itemconfigure("map", image=self.mymap.get_draw_image((self.canv_w, self.canv_h))) - self.plot_origin() - return - - - def left_click(self, event): - self.old_click_point = [event.x, event.y] - return - - - def left_click_release(self, event): - self.old_click_point = None - return - - - def resize_callback(self, event): - self.canv_w = self.canvas.winfo_width() - self.canv_h = self.canvas.winfo_height() - return - - - - -if __name__ == "__main__": - root = tk.Tk() - w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-200 - root.geometry("%dx%d+0+0" % (w, h)) - app = Application(root) - app.mainloop() \ No newline at end of file diff --git a/maps/multi-nakaniwa_sim_1-2.yaml b/maps/multi-nakaniwa_sim_1-2.yaml deleted file mode 100644 index f54ac69..0000000 --- a/maps/multi-nakaniwa_sim_1-2.yaml +++ /dev/null @@ -1,17 +0,0 @@ -multimap: -- map: - image: /home/ubuntu/catkin_ws/src/multi_map_manager/maps/mymap_nakaniwa_sim_1.pgm - resolution: 0.05 - origin: [-60.0, -60.0, 0.0] - negate: 0 - occupied_thresh: 0.65 - free_thresh: 0.196 - change_point: -- map: - image: /home/ubuntu/catkin_ws/src/multi_map_manager/maps/mymap_nakaniwa_sim_2.pgm - resolution: 0.05 - origin: [75.041557, 77.16171, 3.138419320330558] - negate: 0 - occupied_thresh: 0.65 - free_thresh: 0.196 - change_point: diff --git a/multimaps/nakaniwa_sim/map0.pgm b/multimaps/nakaniwa_sim/map0.pgm new file mode 100644 index 0000000..2f5060c --- /dev/null +++ b/multimaps/nakaniwa_sim/map0.pgm Binary files differ diff --git a/multimaps/nakaniwa_sim/map0.yaml b/multimaps/nakaniwa_sim/map0.yaml new file mode 100644 index 0000000..a47e244 --- /dev/null +++ b/multimaps/nakaniwa_sim/map0.yaml @@ -0,0 +1,6 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/multimaps/nakaniwa_sim/map0.pgm +resolution: 0.05 +origin: [-60.0, -60.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/multimaps/nakaniwa_sim/map1.pgm b/multimaps/nakaniwa_sim/map1.pgm new file mode 100644 index 0000000..4d2389a --- /dev/null +++ b/multimaps/nakaniwa_sim/map1.pgm Binary files differ diff --git a/multimaps/nakaniwa_sim/map1.yaml b/multimaps/nakaniwa_sim/map1.yaml new file mode 100644 index 0000000..71a1f95 --- /dev/null +++ b/multimaps/nakaniwa_sim/map1.yaml @@ -0,0 +1,6 @@ +image: /home/ubuntu/catkin_ws/src/tsukuba2022/multimaps/nakaniwa_sim/map1.pgm +resolution: 0.05 +origin: [-45.050354, -43.236632, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/mylib/__init__.py b/mylib/__init__.py deleted file mode 100644 index e69de29..0000000 --- a/mylib/__init__.py +++ /dev/null diff --git a/mylib/application.py b/mylib/application.py deleted file mode 100644 index 4d9dee2..0000000 --- a/mylib/application.py +++ /dev/null @@ -1,160 +0,0 @@ -import tkinter as tk -import tkinter.filedialog -import re -from pathlib import Path -from .tools import Tools - - - -class Application(tk.Frame): - - def __init__(self, master): - super().__init__(master) - self.theme = {"main": "#444", - "bg1": "#222"} - - #### 画面上部のメニューバーを作成 #### - self.menu_bar = tk.Menu(self) - ## Fileメニューの作成 - self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF, - bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5 - ) - ## Fileメニュー内のOpenメニューを作成 - self.open_menu = tk.Menu(self.file_menu, tearoff=tk.OFF, - bg=self.theme["main"], fg="white", activebackground="gray", activeborderwidth=5, - disabledforeground="black" - ) - 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.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") - self.file_menu.add_separator() - self.file_menu.add_command(label="Exit", command=self.menu_exit, accelerator="Ctrl+Q") - ## キーボードショートカットを設定 - self.bind_all("", self.menu_export) - 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) - - #### 仕切り線で大きさを変更できるウィンドウ #### - paned_window = tk.PanedWindow(self.master, sashwidth=3, bg="gray", relief=tk.RAISED) - paned_window.pack(expand=True, fill=tk.BOTH) - - #### ツールボタン群とレイヤーリストを表示するフレーム #### - self.tools = Tools(paned_window, self.theme, width=300, bg=self.theme["main"]) - paned_window.add(self.tools, minsize=self.tools.min_width, padx=2, pady=2) - ## マップ画像を表示するフレームを追加 - paned_window.add(self.tools.map_disp, minsize=self.tools.map_disp.min_width, padx=2, pady=2) - return - - - """ - ++++++++++ File menu functions ++++++++++ - """ - def menu_open_base(self, event=None): - map_path = self.menu_open(title="Select base map yaml file") - if not map_path: return - waypoints_path = self.menu_open(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()) - self.open_menu.entryconfigure("Base map", state="disabled") - self.open_menu.entryconfigure("Additional map", state="normal") - return - - - def menu_open_addtion(self, event=None): - map_path = self.menu_open(title="Select additional map yaml file") - if not map_path: return - waypoints_path = self.menu_open(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 - - - def menu_open(self, title): - filepath = tkinter.filedialog.askopenfilename( - parent=self.master, - title=title, - initialdir=str(Path(".")), - filetypes=[("YAML", ".yaml")] - ) - return filepath - - - def menu_export(self, event=None): - if (len(self.tools.label_list) < 2): return - win = tk.Toplevel() - win.geometry("800x300+50+50") - win.minsize(width=500, height=300) - win.attributes('-topmost', True) - win.title("Export") - font = ("Consolas", 12) - - ### ファイルパスを参照するダイアログを開く関数（ボタンコールバック） ### - def ref_btn_callback(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 create_entry(label_txt, init_path): - 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: ref_btn_callback(entry, init_dir) - frame.grid_columnconfigure(0, weight=1) - return tbox - - ##　マルチマップyaml、結合したウェイポイント、合成した地図画像と情報yamlを取得 - multimap_yaml, path = self.tools.get_multimap_yaml() - tbox1 = create_entry("- Multi map yaml file:", str(path)) - wp_yaml, path = self.tools.get_waypoints_yaml() - tbox2 = create_entry("- Merged waypoints yaml file:", str(path)) - merged_img_pil, merged_yaml, path = self.tools.get_merged_map() - tbox3 = create_entry("- Merged map (yaml, pgm) file:", str(path)) - - ### それぞれをファイルに書き込む関数（ボタンコールバック） ### - def export_btn_callback(): - path = tbox1.get() - with open(path, 'w') as f: - f.write(multimap_yaml) - path = tbox2.get() - with open(path, 'w') as f: - f.write(wp_yaml) - path = Path(tbox3.get()).resolve() - new_merged_yaml = re.sub("image: .*\n", "image: {}\n".format(str(path.with_suffix(".pgm"))), merged_yaml) - with open(str(path.with_suffix(".yaml")), 'w') as f: - f.write(new_merged_yaml) - merged_img_pil.save(str(path.with_suffix(".pgm"))) - win.destroy() - return - - ## エクスポートボタン、キャンセルボタン - export_btn = tk.Button(win, text="Export", font=font) - export_btn["command"] = export_btn_callback - export_btn.pack(side=tk.RIGHT, padx=50, pady=20) - cancel_btn = tk.Button(win, text="Cancel", font=font) - cancel_btn["command"] = win.destroy - cancel_btn.pack(side=tk.LEFT, padx=50, pady=20) - return - - - def menu_exit(self, event=None): - self.master.destroy() - diff --git a/mylib/mapdisp.py b/mylib/mapdisp.py deleted file mode 100644 index 2f54013..0000000 --- a/mylib/mapdisp.py +++ /dev/null @@ -1,399 +0,0 @@ -import tkinter as tk -import numpy as np -import math -from PIL import Image, ImageTk -from pathlib import Path -from mylib.waypointlib import WaypointList, FinishPose - - -class MyMap(): - - def __init__(self, path: Path, map_yaml): - self.yaml_path = path.resolve() - if map_yaml["image"][0] == "/": - self.img_path = map_yaml["image"] - else: - self.img_path = self.yaml_path.with_name(map_yaml["image"]).resolve() - self.original_img_pil = Image.open(self.img_path).convert("RGBA") - self.pil_img = self.original_img_pil.copy() - self.tk_img = ImageTk.PhotoImage(self.pil_img) - self.origin = map_yaml["origin"] - self.resolution = map_yaml["resolution"] - self.map_yaml = map_yaml - self.mat_affine = np.eye(3) - self.img_origin = [-self.origin[0]/self.resolution, self.original_img_pil.height+self.origin[1]/self.resolution] - return - - - def translate(self, x, y): - self.affine(x, y, 1) - return - - - def scale_at(self, x, y, scale): - self.affine(-x, -y, 1) - self.affine(0, 0, scale) - self.affine(x, y, 1) - return - - - def rotate(self, theta, canv_center=None): - if not canv_center: - cx, cy = self.original_img_pil.width/2, self.original_img_pil.height/2 - canv_center = np.dot(self.mat_affine, [cx, cy, 1]) - self.translate(-canv_center[0], -canv_center[1]) - mat = [[math.cos(theta), -math.sin(theta), 0], - [math.sin(theta), math.cos(theta), 0], - [0, 0, 1]] - self.mat_affine = (np.dot(mat, self.mat_affine)) - self.translate(canv_center[0], canv_center[1]) - return - - - def affine(self, x, y, scale): - x = float(x) - y = float(y) - scale = float(scale) - mat = [[scale,0,x], [0,scale,y], [0,0,1]] - self.mat_affine = np.dot(mat, self.mat_affine) - return - - - def get_draw_image(self, canvas_size): - mat_inv = np.linalg.inv(self.mat_affine) - self.pil_img = self.original_img_pil.transform(canvas_size, - Image.Transform.AFFINE, tuple(mat_inv.flatten()), Image.Resampling.NEAREST, - fillcolor="#0000" - ) - self.tk_img = ImageTk.PhotoImage(image=self.pil_img) - return self.tk_img - - - def get_corners(self): - # 1 2 画像の四隅の座標を左の順番で返す - # 4 3 - w, h = self.original_img_pil.width, self.original_img_pil.height - xy1 = self.mat_affine[:, 2] - xy2 = np.dot(self.mat_affine, [w, 0, 1]) - xy3 = np.dot(self.mat_affine, [w, h, 1]) - xy4 = np.dot(self.mat_affine, [0, h, 1]) - return xy1[0], xy1[1], xy2[0], xy2[1], xy3[0], xy3[1], xy4[0], xy4[1] - - - def set_transparency(self, a): - self.original_img_pil.putalpha(int(a/100*255)) - return - - - def transform(self, img_x, img_y): - mat = [img_x, img_y, 1] - tf_mat = np.dot(self.mat_affine, mat) - return tf_mat[0], tf_mat[1] - - - def inv_transform(self, x, y): - mat = [x, y, 1] - inv_affine = np.linalg.inv(self.mat_affine) - inv = np.dot(inv_affine, mat) - return inv[0], inv[1] - - - def image2real(self, img_x, img_y): - real_x = (img_x - self.img_origin[0]) * self.resolution - real_y = -(img_y - self.img_origin[1]) * self.resolution - real_x = round(real_x, 6) - real_y = round(real_y, 6) - return real_x, real_y - - - def real2image(self, real_x, real_y): - img_x = self.img_origin[0] + real_x / self.resolution - img_y = self.img_origin[1] - real_y / self.resolution - return img_x, img_y - - - def get_rotate_angle(self): - return math.atan2(self.mat_affine[1,0], self.mat_affine[0,0]) - - - - -class MapDisplay(tk.Frame): - - def __init__(self, master, theme, **kwargs): - super().__init__(master, kwargs) - self.min_width = 400 - - self.canvas = tk.Canvas(self, bg="black", highlightthickness=0) - self.canvas.pack(expand=True, fill=tk.BOTH, padx=5, pady=5) - self.update() - self.canvas.bind("", self.left_click_move) - self.canvas.bind("", self.left_click) - self.canvas.bind("", self.left_click_release) - self.canvas.bind("", lambda event, scale=1.1: self.ctrl_click(event, scale)) - self.canvas.bind("", lambda event, scale=0.9: self.ctrl_click(event, scale)) - self.canvas.bind("", self.resize_callback) - - self.canv_w = self.canvas.winfo_width() - self.canv_h = self.canvas.winfo_height() - self.base_scale = 1 - self.map_dict = {} - self.waypoints_dict = {} - self.theme = theme - self.old_click_point = None - 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 - - # mode を識別するための定数 - self.Normal = 0 - self.MoveSelected = 1 - self.RotateSelected = 2 - return - - - - def add_map(self, path: Path, map_yaml, waypoints_yaml: Path, base=False): - key = self.path2key(path) - if key in self.map_dict.keys(): - return False - new_map = MyMap(path, map_yaml) - img_w = new_map.original_img_pil.width - img_h = new_map.original_img_pil.height - scale = 1 - offset_x = 0 - offset_y = 0 - if base: - if (self.canv_w / self.canv_h) > (img_w / img_h): - # canvasの方が横長　画像をcanvasの縦に合わせてリサイズ - scale = self.canv_h / img_h - offset_x = (self.canv_w - img_w*scale) / 2 - else: - # canvasの方が縦長　画像をcanvasの横に合わせてリサイズ - scale = self.canv_w / img_w - offset_y = (self.canv_h - img_h*scale) / 2 - self.base_map_key = key - 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 - - # 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, - image=new_map.get_draw_image((self.canv_w, self.canv_h)) - ) - self.map_dict[key] = new_map - self.select_map(path) - self.plot_origin() - # waypoints - self.waypoints_dict[key] = WaypointList(waypoints_yaml) - self.waypoints_dict[key+"fp"] = FinishPose(waypoints_yaml) - self.plot_waypoints() - return True - - - def plot_origin(self): - base_map = 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 - y1 = canv_origin[1] - r - x2 = canv_origin[0] + r + 1 - y2 = canv_origin[1] + r + 1 - if self.canvas.find_withtag("origin"): - self.canvas.moveto("origin", x1, y1) - else: - self.canvas.create_oval(x1, y1, x2, y2, tags="origin", fill='cyan', outline='blue') - self.canvas.lift("origin") - return - - - def plot_waypoints(self): - for key, wp_list in self.waypoints_dict.items(): - if key[-2:] == "fp": - finish_pose = 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() - x0 = cx - y0 = cy - x1 = x0 + math.cos(th) * self.point_rad * 3 - y1 = y0 - math.sin(th) * self.point_rad * 3 - if self.waypoints_dict[key].id is not None: - self.canvas.delete(self.waypoints_dict[key].id) # movetoだとX,毎回削除,再描画 - self.waypoints_dict[key].id = self.canvas.create_line(x0, y0, x1, y1, tags=key, - width=5, arrow=tk.LAST, arrowshape=(4,5,3), fill="#AAF" - ) - self.canvas.lift(self.waypoints_dict[key].id, key[:-2]) - continue - - 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]) - self.waypoints_dict[key].set_id(n+1, id) - else: - for id in wp_list.get_id_list(): - wp = wp_list.get_waypoint(id) - img_x, img_y = self.map_dict[key].real2image(float(wp["x"]), float(wp["y"])) - cx, cy = self.map_dict[key].transform(img_x, img_y) - x0, y0 = cx-self.point_rad, cy-self.point_rad - self.canvas.moveto(id, round(x0), round(y0)) - self.canvas.lift(id, key) - return - - - def create_waypoint(self, 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 - x0 = round(cx - r) - 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') - return id - - - def set_transparency(self, path: Path, alpha): - key = self.path2key(path) - self.map_dict[key].set_transparency(alpha) - self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) - - - - def select_map(self, path: Path): - self.selected_map_key = self.path2key(path) - polygon = self.map_dict[self.selected_map_key].get_corners() - if self.canvas.find_withtag("selection_frame"): - self.canvas.coords("selection_frame", polygon) - else: - 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() - return - - - def set_vision_state(self, path: Path, vision: bool): - key = self.path2key(path) - if vision: - self.canvas.itemconfigure(key, state=tk.NORMAL) - else: - self.canvas.itemconfigure(key, state=tk.HIDDEN) - - - - def path2key(self, path: Path): - return path.with_suffix("").name - - - def get_map(self, path: Path): - key = self.path2key(path) - return self.map_dict[key] - - - def left_click_move(self, event): - if (len(self.map_dict) == 0): 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.old_click_point = [event.x, event.y] - - if self.mode == self.RotateSelected: - # 選択されているマップを回転する - if self.rotate_center is None: - self.rotate_center = self.old_click_point - r = self.point_rad - x1, y1 = self.rotate_center[0]-r, self.rotate_center[1]-r - x2, y2 = self.rotate_center[0]+r, self.rotate_center[1]+r - self.canvas.create_oval(x1, y1, x2, y2, fill="#FFA", outline="#FF0", tags="rotation_center") - return - key = self.selected_map_key - cx, cy = self.rotate_center - if (abs(event.x-cx) < 20) and (abs(event.y-cy) < 20): return - map_rot = math.atan2((event.y - cy), (event.x - cx)) - if self.old_map_rot is None: - self.old_map_rot = map_rot - return - theta = map_rot - self.old_map_rot - self.map_dict[key].rotate(theta, canv_center=self.rotate_center) - self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) - if self.canvas.find_withtag("rotation_line"): - self.canvas.coords("rotation_line", cx, cy, event.x, event.y) - else: - self.canvas.create_line(cx, cy, event.x, event.y, tags="rotation_line", width=1, fill="#FAA") - polygon = polygon = self.map_dict[key].get_corners() - self.canvas.coords("selection_frame", polygon) - self.canvas.lift("origin") - self.plot_waypoints() - self.old_map_rot = map_rot - return - - elif self.mode == self.MoveSelected: - # 選択されているマップのみ移動する - key = self.selected_map_key - self.map_dict[key].translate(delta_x, delta_y) - self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) - - else: - # 全てのマップを移動する - for key in self.map_dict.keys(): - self.map_dict[key].translate(delta_x, delta_y) - self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) - self.canvas.move("origin", delta_x, delta_y) - - self.canvas.move("selection_frame", delta_x, delta_y) - self.canvas.lift("origin") - self.plot_waypoints() - return - - - def ctrl_click(self, event, scale): - if (len(self.map_dict) == 0): return - self.base_scale *= scale - for key in self.map_dict.keys(): - self.map_dict[key].scale_at(event.x, event.y, scale) - self.canvas.itemconfigure(key, image=self.map_dict[key].get_draw_image((self.canv_w, self.canv_h))) - - self.plot_origin() - self.plot_waypoints() - polygon = polygon = self.map_dict[self.selected_map_key].get_corners() - self.canvas.coords("selection_frame", polygon) - self.canvas.lift("selection_frame") - self.canvas.lift("origin") - return - - - def left_click(self, event): - if (self.mode != self.RotateSelected): return - self.old_click_point = [event.x, event.y] - return - - - def left_click_release(self, event): - self.old_click_point = None - self.old_map_rot = None - self.rotate_center = None - if self.canvas.find_withtag("rotation_line"): - self.canvas.delete("rotation_line") - if self.canvas.find_withtag("rotation_center"): - self.canvas.delete("rotation_center") - return - - - def resize_callback(self, event): - self.canv_w = self.canvas.winfo_width() - self.canv_h = self.canvas.winfo_height() - return - - diff --git a/mylib/tools.py b/mylib/tools.py deleted file mode 100644 index 5dd620f..0000000 --- a/mylib/tools.py +++ /dev/null @@ -1,349 +0,0 @@ -import tkinter as tk -import numpy as np -import ruamel.yaml -import math -from PIL import Image, ImageTk -from pathlib import Path -from .mapdisp import MapDisplay, MyMap -from .waypointlib import FinishPose, WaypointList, get_waypoint_yaml - - - -class LayerLabel(tk.Label): - - def __init__(self, master, *args, **kwargs): - super().__init__(master, *args, **kwargs) - self.map_path = Path() - self.map_transparency = tk.IntVar() - self.map_transparency.set(100) - - - def set_map_path(self, path: Path): - self.map_path = path - - - -class Tools(tk.Frame): - - def __init__(self, master, theme, **kwargs): - super().__init__(master, kwargs) - self.min_width = 200 - width = self.cget("width") - - #### ツールボタン群を配置 #### - self.btn_frame = tk.Frame(self, height=200, width=width, bg=theme["main"]) - self.btn_frame.pack(expand=False, fill=tk.X) - ## move ボタン - icon = Image.open(Path("icon","move_button.png")) - icon = icon.resize((30, 30)) - self.move_btn_icon = ImageTk.PhotoImage(image=icon) - self.move_btn = tk.Label(self.btn_frame, image=self.move_btn_icon, bg=theme["main"]) - self.move_btn.bind("", lambda event, btn=self.move_btn: self.btn_entry(event, btn)) - self.move_btn.bind("", lambda event, btn=self.move_btn: self.btn_leave(event, btn)) - self.move_btn.bind("", lambda event, btn=self.move_btn, mode="move": self.btn_clicked(event, btn, mode)) - self.move_btn.grid(column=0, row=0) - ## rotate ボタン - icon = Image.open(Path("icon","rotate_button.png")) - icon = icon.resize((30, 30)) - self.rot_btn_icon = ImageTk.PhotoImage(image=icon) - self.rot_btn = tk.Label(self.btn_frame, image=self.rot_btn_icon, bg=theme["main"]) - self.rot_btn.bind("", lambda event, btn=self.rot_btn: self.btn_entry(event, btn)) - 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) - ## 余白 - space = tk.Frame(self, height=100, bg=theme["main"]) - space.pack(expand=False, fill=tk.X) - - #### レイヤーリストを表示 #### - ## アイコン - icon = Image.open(Path("icon","layer.png")) - icon = icon.resize((20, 17)) - self.layer_icon = ImageTk.PhotoImage(image=icon) - layer_label = tk.Label(self, bg=theme["bg1"], - text=" Layers", 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) - ## レイヤーリストを表示するフレームを配置 - self.layers_frame = tk.Frame(self, width=width, bg=theme["bg1"]) - self.layers_frame.pack(expand=True, fill=tk.BOTH, padx=5) - ## 透明度調節用のスクロールバーを配置 - self.tra_bar = tk.Scale(self.layers_frame, from_=0, to=100, resolution=1, highlightthickness=0, - label="Transparency", fg="white", font=("Ariel",9,"bold"), bg=theme["bg1"], - activebackground="grey", borderwidth=2, sliderlength=15, sliderrelief=tk.RAISED, - orient=tk.HORIZONTAL, showvalue=True, state=tk.DISABLED, command=self.change_transparency - ) - self.tra_bar.pack(expand=False, fill=tk.X, padx=3, pady=5) - ## アイコンを読み込む - icon = Image.open(Path("icon","visible.png")) - icon = icon.resize((18, 17)) - self.visible_icon = ImageTk.PhotoImage(image=icon) - icon = Image.open(Path("icon","invisible.png")) - icon = icon.resize((18, 17)) - self.invisible_icon = ImageTk.PhotoImage(image=icon) - icon = Image.open(Path("icon","folder.png")) - icon = icon.resize((20, 14)) - self.folder_icon = ImageTk.PhotoImage(image=icon) - - #### マップ画像を表示するフレームを生成 #### - self.map_disp = MapDisplay(self.master, theme, bg=theme["main"]) - - #### 変数 - self.label_list = [] - self.base_map_layer = LayerLabel(self.layers_frame) - self.selected_layer = LayerLabel(self.layers_frame) - self.base_waypoints_path = Path() - self.theme = theme - return - - - - def btn_entry(self, event, btn: tk.Label): - if btn.cget("bg") == "black": return - btn.configure(bg="#333") - - - def btn_leave(self, event, btn: tk.Label): - if btn.cget("bg") == "black": return - btn.configure(bg=self.theme["main"]) - - - 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 btn.cget("bg") == "black": - self.map_disp.mode = self.map_disp.Normal - btn.configure(bg="#333") - return - # 既に他のモードだった場合ボタンの状態を切り替える - if self.map_disp.mode == self.map_disp.MoveSelected: self.move_btn.configure(bg=self.theme["main"]) - elif self.map_disp.mode == self.map_disp.RotateSelected: self.rot_btn.configure(bg=self.theme["main"]) - # MapDisplayにモードを設定 - if mode == "move": self.map_disp.mode = self.map_disp.MoveSelected - elif mode == "rotate": self.map_disp.mode = self.map_disp.RotateSelected - btn.configure(bg="black") - return - - - - def set_base_map(self, map_path: Path, wp_path: Path): - map_yaml = self.read_file(map_path) - wp_yaml = self.read_file(wp_path) - 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 - return - - - def add_map(self, path: Path, wp_path: Path): - map_yaml = self.read_file(path) - wp_yaml = self.read_file(wp_path) - if self.map_disp.add_map(path, map_yaml, wp_yaml): - self.append_layer(path) - else: - print("Selected map is already exist") - return - - - def read_file(self, path: Path): - with open(path) as file: # .yamlを読み込む - yaml = ruamel.yaml.YAML().load(file) - return yaml - - - def append_layer(self, path: Path, base=None): - name = " " + path.with_suffix("").name - font = ("Arial",10) - if base: - name += " ( base )" - font = ("Arial", 10, "bold") - label = LayerLabel(self.layers_frame, bg=self.theme["main"], bd=0, - text=name, fg="white", font=font, anchor=tk.W, padx=10, pady=2, - image=self.visible_icon, compound=tk.LEFT - ) - label.set_map_path(path) - label.bind("", lambda event, label=label: self.layerlabel_clicked(event, label)) - if self.label_list: - label.pack(expand=False, side=tk.TOP, fill=tk.X, padx=2, pady=2, ipady=3, before=self.label_list[-1]) - else: - label.pack(expand=False, side=tk.TOP, fill=tk.X, padx=2, pady=2, ipady=3) - - if self.selected_layer is not None: - 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.label_list.append(label) - if base: self.base_map_layer = label - return - - - def layerlabel_clicked(self, event, label: LayerLabel): - px = label.cget("padx") - imw = self.visible_icon.width() - if (px-3 < event.x) and (event.x < px+imw+3): - # 可視アイコンがクリックされたとき - fg = label.cget("fg") - if fg == "white": - label.configure(image=self.invisible_icon, fg="grey") - self.map_disp.set_vision_state(label.map_path, vision=False) - 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"]) - label.configure(bg=self.theme["main"]) - self.selected_layer = label - self.tra_bar.configure(variable=self.selected_layer.map_transparency) - self.map_disp.select_map(self.selected_layer.map_path) - return - - - def change_transparency(self, event): - alpha = self.selected_layer.map_transparency.get() - self.map_disp.set_transparency(self.selected_layer.map_path, alpha) - 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_waypoints_yaml(self): - # ベースのウェイポイントをコピー - 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"] - wpc = waypoints.get_waypoint(num=1).copy() - wpc["x"] = finish_pose.x - wpc["y"] = finish_pose.y - wpc["stop"] = "true" - wpc["change_map"] = "true" - waypoints.append(wpc) - # 残りのウェイポイントを追加していく - base_map: MyMap = self.map_disp.map_dict[self.map_disp.base_map_key] - name = "waypoints-" + base_map.yaml_path.with_suffix("").name - for label in self.label_list[1:]: - mymap: MyMap = self.map_disp.get_map(label.map_path) - name += "-" + mymap.yaml_path.with_suffix("").name - # 2つ目以降の地図のもともとの原点のキャンバス上の位置を取得 - origin = mymap.transform(mymap.img_origin[0], mymap.img_origin[1]) - # ベースのマップから見た原点の座標を計算 - img_x, img_y = base_map.inv_transform(origin[0], origin[1]) - real_x, real_y = base_map.image2real(img_x, img_y) - # 地図の回転角 - theta = -mymap.get_rotate_angle() - # 2つ目以降の地図に対応するウェイポイントの座標を、ベースの地図の座標系に変換する同次変換行列 - mat = [[math.cos(theta), -math.sin(theta), real_x], - [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(): - 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) - # 2つ目以降の地図のfinish poseも変換 - finish_pose = FinishPose(self.map_disp.waypoints_dict[self.map_disp.path2key(label.map_path)].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 - # 最後以外のfinish poseはウェイポイントとして追加 - if label != self.label_list[-1]: - wpc = waypoints.get_waypoint(num=1).copy() - wpc["x"] = finish_pose.x - wpc["y"] = finish_pose.y - 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), init_path - - - def get_merged_map(self): - # 1 2 画像の四隅のキャンバス上の座標を取得 - # 4 3 - base_map: MyMap = self.map_disp.map_dict[self.map_disp.base_map_key] - corners = base_map.get_corners() # x1, y1, x2, y2, x3, y3, x4, y4 - img_corners = np.array(corners) - # ベースの地図画像を基準として画像上の座標に変換 - for i in range(0,8,2): - img_corners[i:i+2] = base_map.inv_transform(corners[i], corners[i+1]) - for label in self.label_list[1:]: - mymap: MyMap = self.map_disp.get_map(label.map_path) - corners = np.array(mymap.get_corners()) - for i in range(0,8,2): - corners[i:i+2] = base_map.inv_transform(corners[i], corners[i+1]) - img_corners = np.vstack((img_corners, corners)) - # 合成する地図画像の中で最も外側にある座標を取得 - left = img_corners[:, [0, 2, 4, 6]].min() - upper = img_corners[:, [1, 3, 5, 7]].min() - right = img_corners[:, [0, 2, 4, 6]].max() - lower = img_corners[:, [1, 3, 5, 7]].max() - # 合成後の画像を準備 - 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 - free_area = np.full(img_size, 0, dtype=np.uint8) # 走行可能領域が255, それ以外が0 - # ベースの画像を合成 - img = np.array(base_map.original_img_pil.convert("L"), dtype=np.uint8) - x, y = int(round(img_corners[0,0]-left)), int(round(img_corners[0,1]-upper)) - h, w = img.shape - obstacles[y:y+h, x:x+w] = obstacles[y:y+h, x:x+w] & np.where(img>100, 255, 0) - free_area[y:y+h, x:x+w] = free_area[y:y+h, x:x+w] | np.where(img<230, 0, 255) - name = "merged-" + base_map.yaml_path.with_suffix("").name - # その他の画像も合成 - for i in range(1, len(self.label_list)): - label = self.label_list[i] - mymap: MyMap = self.map_disp.get_map(label.map_path) - img = mymap.original_img_pil.convert("L") - img = img.rotate(-math.degrees(mymap.get_rotate_angle()), - resample=Image.Resampling.NEAREST, expand=True, fillcolor=205) - img = np.array(img, dtype=np.uint8) - x = int(round(img_corners[i, [0, 2, 4, 6]].min() - left)) - y = int(round(img_corners[i, [1, 3, 5, 7]].min() - upper)) - h, w = img.shape - obstacles[y:y+h, x:x+w] = obstacles[y:y+h, x:x+w] & np.where(img>100, 255, 0) - free_area[y:y+h, x:x+w] = free_area[y:y+h, x:x+w] | np.where(img<230, 0, 255) - name += "-" + Path(label.map_path).with_suffix("").name - # 未確定領域、走行可能領域、障害物を合成し、画像に変換 - merged_img = (unknown | free_area) & obstacles - merged_img = merged_img[:-offset, :-offset] # offset分を削除 - merged_img = Image.fromarray(merged_img, "L") - # 原点から合成後の画像の左下までの距離 - 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" - init_path = base_map.yaml_path.with_name(name).resolve() - return merged_img, merged_yaml, init_path - diff --git a/mylib/waypointlib.py b/mylib/waypointlib.py deleted file mode 100644 index 5174be1..0000000 --- a/mylib/waypointlib.py +++ /dev/null @@ -1,106 +0,0 @@ -import numpy as np -import quaternion - - -class WaypointList(): - def __init__(self, wp_yaml): - self.waypoints = [] - for point in wp_yaml["waypoints"]: - wp = {} - for key, val in point["point"].items(): - wp[key] = val - self.append(wp) - - self.point_keys = self.waypoints[0].keys() - self.number_dict = {} - self.waypoints_yaml = wp_yaml - return - - - def append(self, waypoint: dict, id=None): - self.waypoints.append(waypoint) - if id: self.set_id(len(self.waypoints), id) - return len(self.waypoints) - - - def insert(self, num, waypoint: dict, id=None): - self.waypoints.insert(num-1, waypoint) - for i, n in self.number_dict.items(): - self.number_dict[i] = n+1 if (n>=num) else n - if id: self.set_id(num, id) - return - - - def get_waypoint(self, id=None, num=None): - if id: return self.waypoints[self.get_num(str(id)) - 1] - if num: return self.waypoints[num-1] - return self.waypoints - - - def get_id_list(self): - return self.number_dict.keys() - - - def set_id(self, num, id): - self.number_dict[str(id)] = num - return - - - def get_num(self, id): - return self.number_dict[str(id)] - - - - -class FinishPose(): - def __init__(self, wp_yaml): - super().__init__() - - self.header = {} - for key, val in wp_yaml["finish_pose"]["header"].items(): - self.header[key] = val - - self.position = {} - for key, val in wp_yaml["finish_pose"]["pose"]["position"].items(): - self.position[key] = val - - self.orientation = {} - for key, val in wp_yaml["finish_pose"]["pose"]["orientation"].items(): - self.orientation[key] = val - - self.x = self.position["x"] - self.y = self.position["y"] - self.yaw = self.get_euler() - self.id = None - return - - - def get_euler(self): - o = self.orientation - q = np.quaternion(o["x"], o["y"], o["z"], o["w"]) - return quaternion.as_euler_angles(q)[1] - - - - -def get_waypoint_yaml(waypoints: WaypointList, finish_pose: FinishPose): - s = ["waypoints:" + "\n"] - for point in waypoints.get_waypoint(): - s.append("- point: {") - for i, (key, val) in enumerate(point.items()): - if (i != 0): s.append(", ") - s.append(key + ": " + str(val).lower()) - s.append("}" + "\n") - - s.append("finish_pose:" + "\n") - seq, stamp, frame = (finish_pose.header["seq"], finish_pose.header["stamp"], finish_pose.header["frame_id"]) - s.append(" header: {" + "seq: {}, stamp: {}, frame_id: {}".format(seq,stamp,frame) + "}" + "\n") - s.append(" pose:" + "\n") - x, y, z = finish_pose.x, finish_pose.y, finish_pose.position["z"] - s.append(" position: {" + "x: {}, y: {}, z: {}".format(x, y, z) + "}" + "\n") - q = quaternion.from_euler_angles([0, 0, finish_pose.yaw]) - s.append(" orientation: {" + "x: {}, y: {}, z: {}, w: {}".format(q.x, q.y, q.z, q.w) + "}") - return "".join(s) - - - \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..6c2e131 --- /dev/null +++ b/package.xml @@ -0,0 +1,50 @@ + + + multi_map_manager + 0.0.0 + The multi_map_manager package + + + + + ubuntu + + + + + + TODO + + + + + + + + + + + + + + + + + rospy + nav_msgs + std_msgs + + rospy + nav_msgs + std_msgs + + + catkin + + + + + + + + diff --git a/scripts/map_changer b/scripts/map_changer new file mode 100755 index 0000000..be0a809 --- /dev/null +++ b/scripts/map_changer @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +import rospy +import numpy as np +import ruamel.yaml +from pathlib import Path +from nav_msgs.srv import LoadMap +from std_msgs.msg import UInt16 + + + +class MultiMapServer(): + + def __init__(self): + yaml = ruamel.yaml.YAML() + ## Read multimaps directory + self.multimap_dir = rospy.get_param("map_changer/multi_map_dir") + self.multimap_dir = Path(self.multimap_dir).resolve() + self.current_map_num = 0 + ## Read waypoints file and change point number + self.change_point_num = [] + waypoints_path = rospy.get_param("map_changer/waypoints_file") + with open(waypoints_path) as file: + waypoints_yaml = yaml.load(file) + for i, data in enumerate(waypoints_yaml["waypoints"]): + if ("change_map" in data["point"]) and (data["point"]["change_map"]): + self.change_point_num.append(i+2) + self.change_point_num = np.array(self.change_point_num, dtype=np.uint16) + ## Subscribe current waypoint number + self.waypoint_num = 0 + self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback) + return + + + def waypoint_num_callback(self, msg): + if (self.waypoint_num == msg.data): return + if (np.any(self.change_point_num == msg.data)): + self.current_map_num += 1 + self.change_map_service_call() + self.waypoint_num = msg.data + return + + + def change_map_service_call(self): + rospy.wait_for_service("/change_map") + try: + change_map = rospy.ServiceProxy("/change_map", LoadMap) + res = change_map(str(self.multimap_dir / Path("map{}.yaml".format(self.current_map_num)))) + if res.result == 0: + rospy.loginfo("Successfully changed the map") + return True + else: + rospy.logerr("Failed to change the map: result=", res.result) + + except rospy.ServiceException: + rospy.logerr("Change map service call failed") + return False + + + +if __name__ == '__main__': + rospy.init_node("map_changer") + mms = MultiMapServer() + rospy.spin() +