diff --git a/waypoint_manager/scripts/None b/waypoint_manager/scripts/None new file mode 100644 index 0000000..3463e52 --- /dev/null +++ b/waypoint_manager/scripts/None @@ -0,0 +1,11 @@ +waypoints: +- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.3} +- point: {x: -0.188321, y: -0.510442, z: 0.0, vel: 1.0, rad: 0.3} +- point: {x: 0.568284, y: -0.501916, z: 0.0, vel: 0.8, rad: 0.2} +- point: {x: 0.631463, y: 0.530001, z: 0.0, vel: 0.5, rad: 0.3} +- point: {x: 0.629175, y: 1.860692, z: 0.0, vel: 0.5, rad: 0.2} +finish_pose: + header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link} + pose: + position: {x: -0.550981, y: 1.85, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} \ No newline at end of file diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index 7d37f92..9912315 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -25,7 +25,6 @@ self.menu_bar = tk.Menu(self) # メニューバーを配置 self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) # バーに追加するメニューを作成 self.menu_bar.add_cascade(label="File", menu=self.file_menu) # Fileメニューとしてバーに追加 -<<<<<<< HEAD self.open_menu = tk.Menu(self.file_menu, tearoff=tk.OFF) self.open_menu.add_command(label="Map", command=self.menu_open_map) self.open_menu.add_command(label="Waypoints", command=self.menu_open_waypoints, state="disabled") @@ -39,16 +38,6 @@ command=self.menu_saveas_clicked, accelerator="Ctrl+Shift+S" ) -======= - self.file_menu.add_command(label="Save", # FileメニューにSaveコマンドを追加 - command=self.menu_save_clicked, #コールバック関数を設定 - accelerator="Ctrl+S"# 右側に表示するキーボードショートカット - ) - self.file_menu.add_command(label="Save As", # 同様にSave Asコマンドを追加 - command=self.menu_saveas_clicked, - accelerator="Ctrl+Shift+S" - ) ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa self.file_menu.add_separator() self.file_menu.add_command(label="Exit", command=self.menu_exit_clicked, accelerator="Ctrl+Q") self.bind_all("", self.menu_save_clicked) #キーボードショートカットを設定 @@ -75,14 +64,6 @@ self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint_here) self.right_click_coord = None # 右クリックしたときの座標を保持する変数 -<<<<<<< HEAD -======= - ## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止 - self.__map_img_pil, self.__map_yaml = self.get_map_info() - self.__waypoints, self.waypoint_filepath = self.get_waypoints() - self.current_waypoints = self.__waypoints # 編集中のウェイポイント情報を保持する - ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa ## canvasを配置 self.canvas = tk.Canvas(self.master, background="#AAA") # 画像を描画するcanvas self.canvas.pack(expand=True, fill=tk.BOTH) # canvasを配置 @@ -91,7 +72,6 @@ ## 画像をcanvasのサイズにフィッティングして描画 self.canv_w = self.canvas.winfo_width() # canvasの幅を取得 self.canv_h = self.canvas.winfo_height() # canvasの高さを取得 -<<<<<<< HEAD ## マウスイベントに対するコールバックを設定 self.master.bind("", self.mouse_move) @@ -143,9 +123,6 @@ # .pgmをplillowで読み込む self.__map_img_pil = Image.open(Path(map_path).with_name(self.__map_yaml["image"]).resolve()) # 画像を表示 -======= - self.mat_affine = np.eye(3) # 同次変換行列の初期化 ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa scale = 1 offset_x = 0 offset_y = 0 @@ -162,19 +139,12 @@ self.translate_mat(offset_x, offset_y) # 同次変換行列を更新 self.draw_img_tk = ImageTk.PhotoImage(image=self.__map_img_pil) # 描画する画像を保持する変数を初期化 self.draw_image() # 画像を描画 -<<<<<<< HEAD ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 -======= - - ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 - self.point_rad = 10 ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m) self.map_resolution = self.__map_yaml['resolution'] # 画像上の原点座標を保持（左上から横x, 縦y） self.img_origin = [-origin[0]/self.map_resolution, self.__map_img_pil.height+origin[1]/self.map_resolution, 1] self.plot_origin() -<<<<<<< HEAD self.master.title(Path(map_path).name + " - " + self.master.title()) self.open_menu.entryconfigure("Map", state=tk.DISABLED) self.open_menu.entryconfigure("Waypoints", state=tk.NORMAL) @@ -194,36 +164,9 @@ waypoints = ruamel.yaml.YAML().load(file) self.__waypoints = waypoints self.current_waypoints = waypoints + self.waypoint_filepath = Path(filepath) self.plot_waypoints() self.master.title(Path(filepath).name + " - " + self.master.title()) -======= - - ## waypoints.yamlからウェイポイント情報を取得し、画像にポイントを描画 - self.waypoints_id = np.array([], np.uint16) - self.wplabel_id = np.array([], np.uint16) - self.finish_pose = None - self.finishpose_id = None - self.plot_waypoints() - - ## マウスイベントに対するコールバックを設定 - self.master.bind("", self.mouse_move) - self.master.bind("", self.mouse_wheel) - self.master.bind("", self.left_click_move) - self.master.bind("", self.left_click) - self.master.bind("", self.left_click_release) - self.master.bind("", self.right_click) - self.master.bind("", self.ctrl_left_click) - self.master.bind("", self.ctrl_right_click) - ## ウィンドウに関するコールバック - self.master.bind("", self.window_resize_callback) - - ## その他必要になる変数の初期化 - self.old_click_point = None # 最後にカーソルのあった座標を保持 - self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ - self.editing_waypoint_id = None # 編集中のウェイポイントを示す図形のオブジェクトID - self.moving_waypoint = False # ウェイポイントをDnDで動かしている最中かどうか - self.add_wp_win = None # add waypoint hereをクリックしたときに表示するウィンドウ ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa return @@ -239,36 +182,6 @@ """ -<<<<<<< HEAD -======= - +++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++ - --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- - """ - def get_map_info(self): - map_path = Path('..','..','waypoint_nav','maps','map_gakunai') # .pgmと.yamlの手前までのパス - map_img_pil = Image.open(Path(str(map_path)+'.pgm')) # .pgmをplillowで読み込む - with open(str(map_path)+'.yaml') as file: # .yamlを読み込む - map_yaml = ruamel.yaml.YAML().load(file) - self.master.title(map_path.name + " - " + self.master.title()) - return map_img_pil, map_yaml # この2つの変数を戻り値とする - - - - """ - +++++ waypointsのパスを受け取り読み込んだデータを返す +++++ - --- これもget_param()でパスを受け取り、読み込めるようにする --- - """ - def get_waypoints(self): - file_path = Path('..','..','waypoint_nav','param','waypoints_gakunai.yaml') - with open(file_path) as file: - waypoints = ruamel.yaml.YAML().load(file) - self.master.title(file_path.name + " - " + self.master.title()) - return waypoints, file_path - - - - """ ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa +++++ 地図上の原点に円を描画する +++++ """ def plot_origin(self): @@ -290,10 +203,7 @@ +++++ 地図上にウェイポイントを示す円を描画する +++++ """ def plot_waypoints(self, id=None): -<<<<<<< HEAD if not self.__waypoints: return -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa points = self.current_waypoints['waypoints'] # ウェイポイントのリスト r = self.point_rad # 引数にidが指定された場合、そのポイントのみを再描画して終了 @@ -414,10 +324,7 @@ +++++ キャンバス内でマウスを動かしたときのコールバック +++++ """ def mouse_move(self, event): -<<<<<<< HEAD if not self.__map_img_pil: return -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa img_x, img_y = self.canvas2image(event.x, event.y) if (img_x < 0) or (img_y < 0) or (img_x > self.__map_img_pil.width) or (img_y > self.__map_img_pil.height): self.mouse_position["text"] = " Out of map" @@ -451,11 +358,7 @@ self.wp_info_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置 for i, key in enumerate(point): -<<<<<<< HEAD key_label = tk.Label(self.wp_info_win, text=key+":", width=6, font=("Consolas",15), anchor=tk.E) -======= - key_label = tk.Label(self.wp_info_win, text=key+":", width=4, font=("Consolas",15), anchor=tk.E) ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa key_label.grid(column=0, row=i, padx=2, pady=5) txt_box = tk.Entry(self.wp_info_win, width=20, font=("Consolas", 15)) txt_box.insert(tk.END, point[key]) @@ -556,10 +459,7 @@ +++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++ """ def left_click_move(self, event): -<<<<<<< HEAD if not self.__map_img_pil: return -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa if self.moving_waypoint: return if self.old_click_point is None: self.old_click_point = [event.x, event.y] @@ -593,10 +493,7 @@ +++++ 右クリックしたときのコールバック関数 +++++ """ def right_click(self, event): -<<<<<<< HEAD if not self.__map_img_pil: return -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa # クリックした座標の近くにあるオブジェクトを取得 clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) if clicked_obj: # 何かオブジェクトがクリックされていた場合 @@ -682,10 +579,7 @@ --- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも --- """ def mouse_wheel(self, event): -<<<<<<< HEAD if not self.__map_img_pil: return -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa self.translate_mat(-event.x, -event.y) if event.delta > 0: # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 @@ -705,10 +599,7 @@ +++++ Ctrl押しながら左クリックしたときのコールバック +++++ """ def ctrl_left_click(self, event): -<<<<<<< HEAD if not self.__map_img_pil: return -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa self.translate_mat(-event.x, -event.y) self.scale_mat(1.2) self.translate_mat(event.x, event.y) @@ -724,10 +615,7 @@ +++++ Ctrl押しながら右クリックしたときのコールバック +++++ """ def ctrl_right_click(self, event): -<<<<<<< HEAD if not self.__map_img_pil: return -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa self.translate_mat(-event.x, -event.y) self.scale_mat(0.8) self.translate_mat(event.x, event.y) @@ -743,7 +631,7 @@ +++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++ """ def menu_save_clicked(self, event=None): - self.save_waypoints(self.waypoint_filepath) + self.save_waypoints(str(self.waypoint_filepath)) self.message("Saved changes!") return @@ -754,21 +642,12 @@ """ def menu_saveas_clicked(self, event=None): new_filepath = tkinter.filedialog.asksaveasfilename( -<<<<<<< HEAD parent=self.master, title="Save As", initialdir=str(Path('..','..','waypoint_nav','param')), filetypes=[("YAML", ".yaml")], defaultextension=".yaml" ) -======= - parent=self.master, - title="Save As", - initialdir=str(Path('..','..','waypoint_nav','param')), - filetypes=[("YAML", ".yaml")], - defaultextension=".yaml" - ) ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa if len(new_filepath) == 0: return # cancel self.save_waypoints(new_filepath) current_title = self.master.title() @@ -849,19 +728,11 @@ def draw_image(self): mat_inv = np.linalg.inv(self.mat_affine) img = self.__map_img_pil.transform( -<<<<<<< HEAD (self.canv_w, self.canv_w), Image.Transform.AFFINE, tuple(mat_inv.flatten()), Image.Resampling.NEAREST, fillcolor = 160 ) -======= - (self.canv_w, self.canv_w), - Image.Transform.AFFINE, tuple(mat_inv.flatten()), - Image.Resampling.NEAREST, - fillcolor = 160 - ) ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa self.draw_img_tk = ImageTk.PhotoImage(image=img) # 描画する画像を変数に保持 if not self.canvas.find_withtag("map_image"): # 初めて画像を描画するとき self.canvas.create_image(0, 0, anchor='nw', image=self.draw_img_tk, tags="map_image") # 画像の描画 @@ -903,10 +774,6 @@ x = round(x, 6) y = round(y, 6) return x, y -<<<<<<< HEAD - -======= ->>>>>>> 918b81d1900aee8075d3b55124c65e454deb16fa #===== メイン処理　プログラムはここから実行される =====#