diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index 8bf650c..ef4d82f 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -71,8 +71,8 @@ ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m) - resol = self.__map_yaml['resolution'] - self.img_origin = [-origin[0]/resol, self.__map_img_pil.height+origin[1]/resol, 1] + self.map_resolution = self.__map_yaml['resolution'] + self.img_origin = [-origin[0]/self.map_resolution, self.__map_img_pil.height+origin[1]/self.map_resolution, 1] self.plot_origin() ## waypoints.yamlからウェイポイント情報を取得し、画像にポイントを描画 @@ -82,9 +82,12 @@ 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.window_resize_callback) + + self.old_click_point = None return @@ -94,7 +97,7 @@ --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- """ def get_map_info(self): - map_path = Path('..','..','waypoint_nav','maps','map') # .pgmと.yamlの手前までのパス + 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) @@ -108,7 +111,7 @@ --- これもget_param()でパスを受け取り、読み込めるようにする --- """ def get_waypoints(self): - file_path = Path('..','..','waypoint_nav','param','waypoints.yaml') + file_path = Path('..','..','waypoint_nav','param','waypoints_gakunai.yaml') with open(file_path) as file: waypoints = ruamel.yaml.YAML().load(file) return waypoints @@ -120,19 +123,15 @@ """ def plot_origin(self): origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換 - origin_affine = origin_affine.astype(np.int32) # 符号あり整数に変換 r = 10 # 円の半径（ピクセル） - x0 = origin_affine[0] - r - y0 = origin_affine[1] - r - x1 = origin_affine[0] + r + 1 - y1 = origin_affine[1] + r + 1 - # 円を描画できる位置かどうか判別 - if (x0 < 0) or (y0 < 0) or (x1 > self.canv_w) or (y1 > self.canv_h): - return - elif self.canvas.find_withtag("origin"): # 既に円を描画済みの場合、位置を変える - self.canvas.itemconfig("origin", x0=x0, y0=y0, x1=x1, y1=y1) - else: # 初めて円を描画する - self.canvas.create_oval(x0, y0, x1, y1, tags="origin", fill='cyan', outline='blue') + x1 = origin_affine[0] - r + y1 = origin_affine[1] - r + x2 = origin_affine[0] + r + 1 + y2 = origin_affine[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') return @@ -145,32 +144,64 @@ + + """ + +++++ 左クリックされたときに実行されるコールバック関数 +++++ + """ + def left_click(self, event): + self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 + # クリックした座標の近くにあるオブジェクトを取得 + clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) + if clicked_obj: # オブジェクトがクリックされた場合 + tag = self.canvas.gettags(clicked_obj[0])[0] + print("Clicked " + tag) + return + + + """ +++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++ """ def left_click_move(self, event): - print("x=" + str(event.x) + " y=" + str(event.y)) + if not self.old_click_point is None: + delta_x = event.x-self.old_click_point[0] + delta_y = event.y-self.old_click_point[1] + if (np.linalg.norm([delta_x, delta_y]) < 300): + self.translate_mat(event.x-self.old_click_point[0], event.y-self.old_click_point[1]) + self.draw_image() + self.canvas.move("origin", delta_x, delta_y) # self.plot_origin() + self.plot_waypoints() + self.old_click_point = [event.x, event.y] return """ + +++++ マウスの左クリックを離したときのコールバック関数 +++++ + """ + def left_click_release(self, event): + self.old_click_point = None + return + + + + """ +++++ 右クリックしたときのコールバック関数 +++++ """ def right_click(self, event): # クリックした座標の近くにあるオブジェクトを取得 clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) - if clicked_obj: # 何かオブジェクトがクリックされていた場合、何もしない + if clicked_obj: # 何かオブジェクトがクリックされていた場合 return # クリックされた座標 => 元画像の座標 の変換 mat_inv = np.linalg.inv(self.mat_affine) - img_xy = np.dot(mat_inv, np.array([[event.x], [event.y], [1]])) - # 変換後の元画像の座標がサイズから外れている場合何もしない（地図画像の外をクリックしている） + img_xy = np.dot(mat_inv, [event.x, event.y, 1]) + # 変換後の元画像の座標がサイズから外れている場合（地図画像の外をクリックしている） if (img_xy[0] < 0) or (img_xy[1] < 0) or \ (img_xy[0] > self.__map_img_pil.width) or (img_xy[1] > self.__map_img_pil.height): return self.popup_menu.post(event.x_root, event.y_root) # メニューをポップアップ - self.right_click_coord = img_xy # クリックされた元画像上の座標を変数に格納 + self.right_click_coord = img_xy[0:2] # クリックされた元画像上の座標を変数に格納 return @@ -179,7 +210,7 @@ +++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++ """ def add_waypoint(self): - print("Clicked \"add waypoint\"", self.right_click_coord[0], self.right_click_coord[1]) + print("Clicked \"add waypoint\"", self.right_click_coord) pix_xy = np.array(self.right_click_coord, np.uint32) print("value=",self.__map_img_pil.getpixel((pix_xy[0], pix_xy[1]))) return @@ -191,13 +222,17 @@ --- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも --- """ def mouse_wheel(self, event): + self.translate_mat(-event.x, -event.y) if event.delta > 0: # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 - print(event.x, event.y, event.delta) + self.scale_mat(1.1) else: # 下に回転（タッチパッドなら上にドラッグ）=> 縮小 - print(event.x, event.y, event.delta) - # 回転の向きに対するevent.deltaの正負はプラットフォームにより異なる可能性あり + self.scale_mat(0.9) + self.translate_mat(event.x, event.y) + self.draw_image() + self.plot_origin() + self.plot_waypoints() return @@ -220,19 +255,6 @@ - """ - +++++ 左クリックされたときに実行されるコールバック関数 +++++ - """ - def left_click(self, event): - self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 - # クリックした座標の近くにあるオブジェクトを取得 - clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) - if clicked_obj: # オブジェクトがクリックされた場合 - tag = self.canvas.gettags(clicked_obj[0])[0] - print("Clicked " + tag) - return - - """ +++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++ diff --git a/waypoint_nav/maps/map_gakunai.pgm b/waypoint_nav/maps/map_gakunai.pgm new file mode 100644 index 0000000..8a70604 --- /dev/null +++ b/waypoint_nav/maps/map_gakunai.pgm Binary files differ diff --git a/waypoint_nav/maps/map_gakunai.yaml b/waypoint_nav/maps/map_gakunai.yaml new file mode 100644 index 0000000..51664a5 --- /dev/null +++ b/waypoint_nav/maps/map_gakunai.yaml @@ -0,0 +1,7 @@ +image: /home/ubuntu/catkin_ws/src/orne_navigation/orne_navigation_executor/maps/mymap.pgm +resolution: 0.050000 +origin: [-100.000000, -100.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/waypoint_nav/param/waypoints_gakunai.yaml b/waypoint_nav/param/waypoints_gakunai.yaml new file mode 100644 index 0000000..f4c66b1 --- /dev/null +++ b/waypoint_nav/param/waypoints_gakunai.yaml @@ -0,0 +1,100 @@ +waypoints: + - point: + x: 15.549 + y: 1.99887 + z: 0 + - point: + x: 27.7033 + y: 0.740999 + z: 0 + - point: + x: 44.5033 + y: -3.88193 + z: 0 + - point: + x: 53.4473 + y: -1.98026 + z: 0 + - point: + x: 53.5381 + y: 13.9875 + z: 0 + - point: + x: 53.0202 + y: 25.7033 + z: 0 + - point: + x: 52.624 + y: 40.8326 + z: 0 + - point: + x: 52.4651 + y: 56.1678 + z: 0 + - point: + x: 47.4651 + y: 55.9678 + z: 0 + - point: + x: 41.1833 + y: 55.7508 + z: 0 + - point: + x: 30.9773 + y: 56.3972 + z: 0 + - point: + x: 19.605 + y: 55.9349 + z: 0 + - point: + x: 9.74244 + y: 57.775 + z: 0 + - point: + x: 9.04664 + y: 53.1814 + z: 0 + - point: + x: 12.2474 + y: 45.1187 + z: 0 + - point: + x: 17.5568 + y: 32.2592 + z: 0 + - point: + x: 19.6331 + y: 27.0299 + z: 0 + - point: + x: 23.7635 + y: 17.3945 + z: 0 + - point: + x: 27.6797 + y: 6.92171 + z: 0 + - point: + x: 29.8247 + y: 1.30448 + z: 0 + - point: + x: 12.7818 + y: 1.13019 + z: 0 +finish_pose: + header: + seq: 0 + stamp: 1623386648.025251015 + frame_id: base_link + pose: + position: + x: -0.8863756 + y: 0.5020624 + z: 0 + orientation: + x: 0 + y: 0 + z: 0.999797 + w: -0.0201511