diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..18ad7b6
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,14 @@
+.vscode/
+# !.vscode/settings.json
+# !.vscode/tasks.json
+# !.vscode/launch.json
+# !.vscode/extensions.json
+# !.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+*.pptx
+.mylib/__pycache__/
+
+# Built Visual Studio Code Extensions
+*.vsix
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/README.md
diff --git a/icon/folder.png b/icon/folder.png
new file mode 100644
index 0000000..397b8cf
--- /dev/null
+++ b/icon/folder.png
Binary files differ
diff --git a/icon/invisible.png b/icon/invisible.png
new file mode 100644
index 0000000..de968b3
--- /dev/null
+++ b/icon/invisible.png
Binary files differ
diff --git a/icon/layer.png b/icon/layer.png
new file mode 100644
index 0000000..2ccd413
--- /dev/null
+++ b/icon/layer.png
Binary files differ
diff --git a/icon/move_button.png b/icon/move_button.png
new file mode 100644
index 0000000..8229622
--- /dev/null
+++ b/icon/move_button.png
Binary files differ
diff --git a/icon/rotate_button.png b/icon/rotate_button.png
new file mode 100644
index 0000000..3a3a019
--- /dev/null
+++ b/icon/rotate_button.png
Binary files differ
diff --git a/icon/visible.png b/icon/visible.png
new file mode 100644
index 0000000..9fa24fc
--- /dev/null
+++ b/icon/visible.png
Binary files differ
diff --git a/main.py b/main.py
new file mode 100644
index 0000000..dfbc869
--- /dev/null
+++ b/main.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/map_trimmer.py b/map_trimmer.py
new file mode 100644
index 0000000..24b689a
--- /dev/null
+++ b/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("<Control-t>", self.menu_trim)
+        self.bind_all("<Control-Shift-S>", self.menu_saveas)
+        self.bind_all("<Control-o>", self.menu_open)
+        self.bind_all("<Control-q>", 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("<B1-Motion>",        self.left_click_move)
+        self.canvas.bind("<Button-1>",         self.left_click)
+        self.canvas.bind("<ButtonRelease-1>",  self.left_click_release)
+        self.canvas.bind("<Control-Button-1>", lambda event, scale=1.1: self.ctrl_click(event, scale))
+        self.canvas.bind("<Control-Button-3>", lambda event, scale=0.9: self.ctrl_click(event, scale))
+        self.canvas.bind("<Configure>",        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, "<B1-Motion>", 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/merged-nakaniwa_sim_1-2.pgm b/maps/merged-nakaniwa_sim_1-2.pgm
new file mode 100644
index 0000000..2cfe467
--- /dev/null
+++ b/maps/merged-nakaniwa_sim_1-2.pgm
Binary files differ
diff --git a/maps/merged-nakaniwa_sim_1-2.yaml b/maps/merged-nakaniwa_sim_1-2.yaml
new file mode 100644
index 0000000..2e8d27d
--- /dev/null
+++ b/maps/merged-nakaniwa_sim_1-2.yaml
@@ -0,0 +1,6 @@
+image: /home/ubuntu/catkin_ws/src/multi_map_manager/maps/merged-nakaniwa_sim_1-2.pgm
+resolution: 0.05
+origin: [-60.0, -60.0, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
\ No newline at end of file
diff --git a/maps/merged-nakaniwa_sim_1-2_trim.pgm b/maps/merged-nakaniwa_sim_1-2_trim.pgm
new file mode 100644
index 0000000..795e0b5
--- /dev/null
+++ b/maps/merged-nakaniwa_sim_1-2_trim.pgm
Binary files differ
diff --git a/maps/merged-nakaniwa_sim_1-2_trim.yaml b/maps/merged-nakaniwa_sim_1-2_trim.yaml
new file mode 100644
index 0000000..0c1419c
--- /dev/null
+++ b/maps/merged-nakaniwa_sim_1-2_trim.yaml
@@ -0,0 +1,6 @@
+image: ./merged-nakaniwa_sim_1-2_trim.pgm
+resolution: 0.05
+origin: [-2.985562713745958, -1.9343252142634129, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/maps/multi-nakaniwa_sim_1-2.yaml b/maps/multi-nakaniwa_sim_1-2.yaml
new file mode 100644
index 0000000..f54ac69
--- /dev/null
+++ b/maps/multi-nakaniwa_sim_1-2.yaml
@@ -0,0 +1,17 @@
+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/maps/mymap_nakaniwa_1.pgm b/maps/mymap_nakaniwa_1.pgm
new file mode 100644
index 0000000..ae968dc
--- /dev/null
+++ b/maps/mymap_nakaniwa_1.pgm
Binary files differ
diff --git a/maps/mymap_nakaniwa_1.yaml b/maps/mymap_nakaniwa_1.yaml
new file mode 100644
index 0000000..38ebbd7
--- /dev/null
+++ b/maps/mymap_nakaniwa_1.yaml
@@ -0,0 +1,7 @@
+image: ./mymap_nakaniwa_1.pgm
+resolution: 0.050000
+origin: [-50.000000, -50.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
diff --git a/maps/mymap_nakaniwa_2.pgm b/maps/mymap_nakaniwa_2.pgm
new file mode 100644
index 0000000..1f906d8
--- /dev/null
+++ b/maps/mymap_nakaniwa_2.pgm
Binary files differ
diff --git a/maps/mymap_nakaniwa_2.yaml b/maps/mymap_nakaniwa_2.yaml
new file mode 100644
index 0000000..49d3856
--- /dev/null
+++ b/maps/mymap_nakaniwa_2.yaml
@@ -0,0 +1,8 @@
+image: ./mymap_nakaniwa_2.pgm
+resolution: 0.050000
+origin: [-50.000000, -50.000000, 0.000000]
+# origin: [97.886772, 105.743201, 3.1149172860865844]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
diff --git a/maps/mymap_nakaniwa_sim_1.pgm b/maps/mymap_nakaniwa_sim_1.pgm
new file mode 100644
index 0000000..78798d3
--- /dev/null
+++ b/maps/mymap_nakaniwa_sim_1.pgm
Binary files differ
diff --git a/maps/mymap_nakaniwa_sim_1.yaml b/maps/mymap_nakaniwa_sim_1.yaml
new file mode 100644
index 0000000..565c916
--- /dev/null
+++ b/maps/mymap_nakaniwa_sim_1.yaml
@@ -0,0 +1,7 @@
+image: ./mymap_nakaniwa_sim_1.pgm
+resolution: 0.050000
+origin: [-60.000000, -60.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
diff --git a/maps/mymap_nakaniwa_sim_2.pgm b/maps/mymap_nakaniwa_sim_2.pgm
new file mode 100644
index 0000000..e897525
--- /dev/null
+++ b/maps/mymap_nakaniwa_sim_2.pgm
Binary files differ
diff --git a/maps/mymap_nakaniwa_sim_2.yaml b/maps/mymap_nakaniwa_sim_2.yaml
new file mode 100644
index 0000000..901cd59
--- /dev/null
+++ b/maps/mymap_nakaniwa_sim_2.yaml
@@ -0,0 +1,7 @@
+image: ./mymap_nakaniwa_sim_2.pgm
+resolution: 0.050000
+origin: [-60.000000, -60.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
diff --git a/waypoints/waypoints_nakaniwa_1.yaml b/waypoints/waypoints_nakaniwa_1.yaml
new file mode 100644
index 0000000..7e768bb
--- /dev/null
+++ b/waypoints/waypoints_nakaniwa_1.yaml
@@ -0,0 +1,19 @@
+waypoints:
+- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 11.025331, y: -0.023422, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 18.50064, y: -0.394513, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 26.176818, y: -0.880348, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 34.8965, y: -2.35386, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 42.74376, y: -4.864187, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 50.031271, y: -5.398604, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 53.505717, y: -1.933611, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 53.2674, y: 7.98859, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 53.0977, y: 20.0885, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 52.298734, y: 41.727943, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 52.033145, y: 52.371588, z: 0.0, vel: 1, rad: 1, stop: false}
+finish_pose:
+  header: {seq: 0, stamp: 1663209268.6905353, frame_id: map}
+  pose:
+    position: {x: 50, y: 54.5, z: 0}
+    orientation: {x: 0, y: 0, z: 0.999995, w: 0.00303693}
\ No newline at end of file
diff --git a/waypoints/waypoints_nakaniwa_2.yaml b/waypoints/waypoints_nakaniwa_2.yaml
new file mode 100644
index 0000000..314326b
--- /dev/null
+++ b/waypoints/waypoints_nakaniwa_2.yaml
@@ -0,0 +1,19 @@
+waypoints:
+- point: {x: 4.8992, y: 0.0763934, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 11.025331, y: -0.023422, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 18.787577, y: -0.169002, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 27.171158, y: -0.322829, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 34.862517, y: -0.322829, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 43.015357, y: -0.476656, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 50.091407, y: -0.55357, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 53.091037, y: 2.061492, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 52.93721, y: 9.52211, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 52.629555, y: 19.751617, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 52.5512, y: 29.5179, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 52.895084, y: 41.365471, z: 0.0, vel: 1, rad: 1, stop: false}
+- point: {x: 53.259834, y: 51.961747, z: 0.0, vel: 1, rad: 1, stop: false}
+finish_pose:
+  header: {seq: 0, stamp: 1663209268.6905353, frame_id: map}
+  pose:
+    position: {x: 50, y: 54.5, z: 0}
+    orientation: {x: 0, y: 0, z: 0.999995, w: 0.00303693}
\ No newline at end of file
diff --git a/waypoints/waypoints_nakaniwa_sim_1-2.yaml b/waypoints/waypoints_nakaniwa_sim_1-2.yaml
new file mode 100644
index 0000000..c9be1e0
--- /dev/null
+++ b/waypoints/waypoints_nakaniwa_sim_1-2.yaml
@@ -0,0 +1,25 @@
+waypoints:
+- point: {x: 4.91548, y: -0.0550663, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 9.79794, y: 0.02867, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 13.9369, y: 0.0678324, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.700148, y: 1.386101, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.837027, y: 5.446853, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.882653, y: 9.416352, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.973906, y: 13.477104, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.6479, y: 16.8488, z: 0, vel: 1, rad: 1, stop: true, change_map: true}
+- point: {x: 12.649069, y: 16.968348, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 9.385235, y: 16.95799, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 5.662998, y: 16.946178, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 3.134879, y: 16.223039, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 4.868725, y: 13.373368, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 6.50457, y: 11.50272, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 7.484275, y: 9.79152, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 8.585662, y: 7.884585, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 9.694664, y: 5.982185, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 11.19084, y: 3.638886, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 11.307752, y: 1.466401, z: 0, vel: 1, rad: 1, stop: false}
+finish_pose:
+  header: {seq: 0, stamp: 85.975, frame_id: map}
+  pose:
+    position: {x: 10.569600461613387, y: -0.02156870555503687, z: 0}
+    orientation: {x: -0.0, y: 0.0, z: -0.9998723389797014, w: 0.01597828974768534}
\ No newline at end of file
diff --git a/waypoints/waypoints_nakaniwa_sim_1.yaml b/waypoints/waypoints_nakaniwa_sim_1.yaml
new file mode 100644
index 0000000..3141008
--- /dev/null
+++ b/waypoints/waypoints_nakaniwa_sim_1.yaml
@@ -0,0 +1,13 @@
+waypoints:
+- point: {x: 4.91548, y: -0.0550663, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 9.79794, y: 0.02867, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 13.9369, y: 0.0678324, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.700148, y: 1.386101, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.837027, y: 5.446853, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.882653, y: 9.416352, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 15.973906, y: 13.477104, z: 0, vel: 1, rad: 1, stop: false}
+finish_pose:
+  header: {seq: 0, stamp: 59.85, frame_id: map}
+  pose:
+    position: {x: 15.6479, y: 16.8488, z: 0}
+    orientation: {x: 0, y: 0, z: 0.999942, w: 0.010754}
\ No newline at end of file
diff --git a/waypoints/waypoints_nakaniwa_sim_2.yaml b/waypoints/waypoints_nakaniwa_sim_2.yaml
new file mode 100644
index 0000000..af7e887
--- /dev/null
+++ b/waypoints/waypoints_nakaniwa_sim_2.yaml
@@ -0,0 +1,17 @@
+waypoints:
+- point: {x: 2.583187, y: -0.004932, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 5.847038, y: -0.004932, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 9.569294, y: -0.004932, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 12.099695, y: 0.710181, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 10.3749, y: 3.56534, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 8.745, y: 5.44117, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 7.77073, y: 7.15547, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 6.6754, y: 9.06589, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 5.57244, y: 10.9718, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 4.083708, y: 13.319835, z: 0, vel: 1, rad: 1, stop: false}
+- point: {x: 3.97369, y: 15.49268, z: 0, vel: 1, rad: 1, stop: false}
+finish_pose:
+  header: {seq: 0, stamp: 85.975, frame_id: map}
+  pose:
+    position: {x: 4.71656, y: 16.9783, z: 0}
+    orientation: {x: 0, y: 0, z: 0.0143918, w: 0.999896}
\ No newline at end of file