diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index ef4d82f..bc9ef8a 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -70,12 +70,14 @@ self.draw_image() # 画像を描画 ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 + self.point_rad = 10 origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m) 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からウェイポイント情報を取得し、画像にポイントを描画 + self.waypoints_id = np.array([], np.uint8) self.plot_waypoints() ## マウスイベントに対するコールバックを設定 @@ -123,7 +125,7 @@ """ def plot_origin(self): origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換 - r = 10 # 円の半径(ピクセル) + r = self.point_rad # 円の半径(ピクセル) x1 = origin_affine[0] - r y1 = origin_affine[1] - r x2 = origin_affine[0] + r + 1 @@ -140,6 +142,23 @@ +++++ 地図上にウェイポイントを示す円を描画する +++++ """ def plot_waypoints(self): + points = self.current_waypoints['waypoints'] # ウェイポイントのリスト + r = self.point_rad + for i in range(len(points)): + x = points[i]['point']['x'] / self.map_resolution + self.img_origin[0] + y = -points[i]['point']['y'] / self.map_resolution + self.img_origin[1] + xy_affine = np.dot(self.mat_affine, [x, y, 1]) + x1 = xy_affine[0] - r + y1 = xy_affine[1] - r + x2 = xy_affine[0] + r + 1 + y2 = xy_affine[1] + r + 1 + if len(self.waypoints_id) < len(points): + id = self.canvas.create_oval(x1, y1, x2, y2, fill='magenta', outline='red', + activefill='red') + self.waypoints_id = np.append(self.waypoints_id, id) + else: + id = self.waypoints_id[i] + self.canvas.moveto(id, x1, y1) return @@ -153,8 +172,8 @@ # クリックした座標の近くにあるオブジェクトを取得 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) + print("id=",clicked_obj[0]) + print("tag=", self.canvas.gettags(clicked_obj[0])) return diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml index e061bb1..f8d6b5b 100644 --- a/waypoint_nav/param/waypoints.yaml +++ b/waypoint_nav/param/waypoints.yaml @@ -2,7 +2,7 @@ - point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.5} - point: {x: 0.203745, y: -0.496466, z: 0.0, vel: 0.5, rad: 0.2} - point: {x: 0.666450, y: 0.504773, z: 0.0, vel: 0.5, rad: 0.3} -- point: {x: 0.515219, y: 0.515219, z: 0.0, vel: 1.0, rad: 0.5} +- point: {x: 0.515219, y: 2.015219, z: 0.0, vel: 1.0, rad: 0.5} finish_pose: header: {seq: 0.0, stamp: 1.6203674303803937E9, frame_id: base_link} pose: