diff --git a/waypoint_manager/scripts/lib/waypointlib.py b/waypoint_manager/scripts/lib/waypointlib.py
index ab54d33..d8c54ab 100644
--- a/waypoint_manager/scripts/lib/waypointlib.py
+++ b/waypoint_manager/scripts/lib/waypointlib.py
@@ -108,10 +108,12 @@
     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")
-    p = finish_pose.position
-    s.append("    position: {" + "x: {}, y: {}, z: {}".format(p["x"], p["y"], p["z"]) + "}" + "\n")
-    o = finish_pose.orientation
-    s.append("    orientation: {" + "x: {}, y: {}, z: {}, w: {}".format(o["x"],o["y"],o["z"],o["w"]) + "}")
+    x = finish_pose.x
+    y = finish_pose.y
+    z = 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)
 
 
diff --git a/waypoint_manager/scripts/manager_GUI.py b/waypoint_manager/scripts/manager_GUI.py
index 846476c..ecf0e80 100755
--- a/waypoint_manager/scripts/manager_GUI.py
+++ b/waypoint_manager/scripts/manager_GUI.py
@@ -35,6 +35,10 @@
         self.bind_all("<Control-Shift-S>", self.menu_saveas)
         self.bind_all("<Control-q>", self.menu_exit)
 
+        self.edit_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF)
+        self.edit_menu.add_command(label="Set Finish Pose", command=self.menu_move_finishpose)
+        self.menu_bar.add_cascade(label="Edit", menu=self.edit_menu)
+
         self.show_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF)
         self.fp_menu = tk.Menu(self.show_menu, tearoff=tk.OFF)
         self.fp_menu.add_command(label="Set model", command=self.menu_set_footprint)
@@ -94,6 +98,7 @@
         self.waypoints_filepath = None
         self.editing_waypoint_id = None   # 編集中のウェイポイントを示す図形のオブジェクトID
         self.moving_waypoint = False   # ウェイポイントをDnDで動かしている最中かどうか
+        self.setting_finish_pose = 0 # finish pose のセット中かどうか
         self.old_click_point = None   # 最後にカーソルのあった座標を保持
         self.wp_info_win = None   # ウェイポイント情報を表示するウィンドウ
         self.point_rad = 10   # 画像上に示すポイントの半径ピクセル
@@ -219,6 +224,15 @@
     
 
     """
+    +++++  Edit -> Set Finish Pose +++++
+    """
+    def menu_move_finishpose(self, event=None):
+        self.message("Click any point to set finsih pose")
+        self.setting_finish_pose = 1
+        return
+
+
+    """
     +++++  View -> Footprint -> Set +++++
     """
     def menu_set_footprint(self, event=None):
@@ -623,6 +637,17 @@
             return
         x, y = self.mymap.image2real(img_x, img_y)
         self.mouse_position["text"] = " ( x,  y ) = ( {},  {} ) ".format(x, y)
+
+        if self.setting_finish_pose == 2:
+            x0, y0, _, _ = self.canvas.coords("set_finish_pose")
+            x, y = event.x, event.y
+            theta = math.atan2((-y+y0), (x-x0))
+            x1 = x0 + math.cos(theta) * self.point_rad * 3
+            y1 = y0 - math.sin(theta) * self.point_rad * 3
+            self.canvas.delete("set_finish_pose")
+            self.canvas.create_line(x0, y0, x1, y1, tags="set_finish_pose",
+                width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#F88"
+            )
         return
     
 
@@ -649,6 +674,35 @@
     """
     def left_click(self, event):
         self.popup_menu.unpost()   # 右クリックで出るポップアップメニューを非表示
+
+        if self.setting_finish_pose == 1:
+            x0, y0 = event.x, event.y
+            x1 = x0 + math.cos(self.finish_pose.yaw) * self.point_rad * 3
+            y1 = y0 - math.sin(self.finish_pose.yaw) * self.point_rad * 3
+            self.canvas.create_line(x0, y0, x1, y1, tags="set_finish_pose",
+                width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#F88"
+            )
+            self.setting_finish_pose = 2
+            return
+        
+        if self.setting_finish_pose == 2:
+            x0, y0, _, _ = self.canvas.coords("set_finish_pose")
+            x, y = event.x, event.y
+            theta = math.atan2((-y+y0), (x-x0))
+            x1 = x0 + math.cos(theta) * self.point_rad * 3
+            y1 = y0 - math.sin(theta) * self.point_rad * 3
+            self.canvas.delete("set_finish_pose")
+            self.canvas.delete(self.finish_pose.id)
+            self.finish_pose.id = self.canvas.create_line(x0, y0, x1, y1, tags="finish_pose",
+                width=10, arrow=tk.LAST, arrowshape=(12,15,9), fill="#AAF"
+            )
+            img_x, img_y = self.mymap.inv_transform(x0, y0)
+            real_x, real_y = self.mymap.image2real(img_x, img_y)
+            self.finish_pose.x = real_x
+            self.finish_pose.y = real_y
+            self.finish_pose.yaw = theta
+            self.setting_finish_pose = 0
+            self.message("New finish pose is setted.")
         return
     
 
@@ -658,6 +712,7 @@
     def left_click_move(self, event):
         if not self.mymap: return
         if self.moving_waypoint: return
+        if self.setting_finish_pose != 0: return
         if self.old_click_point is None:
             self.old_click_point = [event.x, event.y]
             return
@@ -690,6 +745,7 @@
     """
     def right_click(self, event):
         if not self.mymap: return
+        if self.setting_finish_pose != 0: return
         # クリックした座標の近くにあるオブジェクトを取得
         clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20)
         if clicked_obj:   # 何かオブジェクトがクリックされていた場合
@@ -709,6 +765,7 @@
     """
     def ctrl_left_click(self, event):
         if not self.mymap: return
+        if self.setting_finish_pose != 0: return
         scale = 1.2
         self.mymap.scale_at(event.x, event.y, scale)
         self.draw_image()
@@ -725,6 +782,7 @@
     """
     def ctrl_right_click(self, event):
         if not self.mymap: return
+        if self.setting_finish_pose != 0: return
         scale = 0.8
         self.mymap.scale_at(event.x, event.y, scale)
         self.draw_image()