diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index d15b0b6..73bf1c0 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -47,14 +47,17 @@ ## 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 self.status_bar = tk.Frame(self.master) - self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, text="(x, y, val) = ", - width=30, anchor=tk.W) + self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, text="(x, y, val) = ", + width=40, anchor=tk.W, font=("", 15)) self.mouse_position.pack(side=tk.LEFT, padx=1) + self.waypoint_num = tk.Label(self.status_bar, relief=tk.SUNKEN, text="Waypoint No. -----", + width=20, anchor=tk.W, font=("", 15)) + self.waypoint_num.pack(side=tk.LEFT, padx=1) self.status_bar.pack(side=tk.BOTTOM, fill=tk.X) ## 右クリックしたときに表示するポップアップメニューを作成 self.popup_menu = tk.Menu(self, tearoff=tk.OFF) - self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint) + self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint_here) self.right_click_coord = None # 右クリックしたときの座標を保持する変数 ## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止 @@ -183,10 +186,19 @@ """ +++++ 地図上にウェイポイントを示す円を描画する +++++ """ - def plot_waypoints(self): - print(self.canvas.find_all()) + def plot_waypoints(self, id=None): points = self.current_waypoints['waypoints'] # ウェイポイントのリスト r = self.point_rad + # 引数にidが指定された場合、そのポイントのみを再描画して終了 + if id: + wp_num = np.where(self.waypoints_id == id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 + img_x = float(points[wp_num-1]['point']['x']) / self.map_resolution + self.img_origin[0] + img_y = -float(points[wp_num-1]['point']['y']) / self.map_resolution + self.img_origin[1] + xy_affine = np.dot(self.mat_affine, [img_x, img_y, 1]) + x0 = xy_affine[0] - r + y0 = xy_affine[1] - r + self.canvas.moveto(id, x0, y0) + return # ウェイポイントの追加または削除が行なわれている場合、初期化 if len(self.waypoints_id) != len(points): for id in self.waypoints_id: @@ -202,8 +214,11 @@ x1 = xy_affine[0] + r + 1 y1 = xy_affine[1] + r + 1 if len(self.waypoints_id) < len(points): - id = self.canvas.create_oval(x0, y0, x1, y1, - fill='#FDD', outline='red', activefill='red') + id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FDD', outline='red', activefill='red') + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_clicked(event, wp_id)) + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_enter(event, wp_id)) + self.canvas.tag_bind(id, "", self.waypoint_leave) + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_click_move(event, wp_id)) self.waypoints_id = np.append(self.waypoints_id, id) else: id = self.waypoints_id[i] @@ -224,28 +239,81 @@ y1 = y0 - math.sin(self.finish_pose[2]) * r * 3 if self.finishpose_id is not None: self.canvas.delete(self.finishpose_id) - self.finishpose_id = self.canvas.create_line(x0, y0, x1, y1, width=10, + self.finishpose_id = self.canvas.create_line(x0, y0, x1, y1, width=10, tags="finish_pose", arrow=tk.LAST, arrowshape=(12,15,9), fill="#AAF") return """ + +++++ ウェイポイントが左クリックされたときのコールバック +++++ + """ + def waypoint_clicked(self, event, wp_id): + if wp_id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え + self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') + self.editing_waypoint_id = wp_id + self.canvas.itemconfig(wp_id, fill='red') + self.disp_waypoint_info(wp_id) + self.message("Show selected waypoint information") + return + + + + """ + +++++ ウェイポイントを左クリックしながら動かしたときのコールバック +++++ + """ + def waypoint_click_move(self, event, wp_id): + if not self.moving_waypoint: 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.canvas.move(self.editing_waypoint_id, delta_x, delta_y) + box = self.canvas.bbox(self.editing_waypoint_id) + px = (box[2] + box[0]) / 2 # ウィンドウ上の座標 + py = (box[3] + box[1]) / 2 + img_x, img_y = self.canvas2image(px, py) + # マップ画像上の座標を、実際の座標に変換 + x, y = self.image2real(img_x, img_y) + # 編集中のウェイポイント情報を更新 + wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + self.current_waypoints['waypoints'][wp_num]['point']['x'] = x + self.current_waypoints['waypoints'][wp_num]['point']['y'] = y + # 表示中のウェイポイント情報を更新 + txt_box = self.wp_info_win.grid_slaves(column=1, row=0)[0] + txt_box.delete(0, tk.END) + txt_box.insert(tk.END, x) + txt_box = self.wp_info_win.grid_slaves(column=1, row=1)[0] + txt_box.delete(0, tk.END) + txt_box.insert(tk.END, y) + self.old_click_point = [event.x, event.y] + return + + + """ + +++++ ウェイポイントを示す円にカーソルが入ったときと出たときのコールバック +++++ + """ + def waypoint_enter(self, event, wp_id): + wp_num = np.where(self.waypoints_id == wp_id)[0][0] + 1 # ウェイポイントの番号を取得 + self.waypoint_num["text"] = "Waypoint No. " + str(wp_num) + + def waypoint_leave(self, event): + self.waypoint_num["text"] = "Waypoint No. -----" + + + + """ +++++ キャンバス内でマウスを動かしたときのコールバック +++++ """ def mouse_move(self, event): - mat_inv = np.linalg.inv(self.mat_affine) - 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): + 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" return - x = (img_xy[0] - self.img_origin[0]) * self.map_resolution - y = (-img_xy[1] + self.img_origin[1]) * self.map_resolution - x = round(x, 3) - y = round(y, 3) - val = self.__map_img_pil.getpixel((img_xy[0], img_xy[1])) - self.mouse_position["text"] = " (x, y, val) = ({}, {}, {})".format(x,y,val) + x, y = self.image2real(img_x, img_y) + val = self.__map_img_pil.getpixel((img_x, img_y)) + self.mouse_position["text"] = " (x, y, val) = ({}, {}, {})".format(x, y, val) return @@ -255,20 +323,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 (not clicked_obj): return # オブジェクト以外の領域 - id = clicked_obj[0] - if (id == self.finishpose_id): return - tag = self.canvas.gettags(id) - if (len(tag) == 0) or (tag[0] == "origin"): return - # ↓ウェイポイントがクリックされた場合、情報を表示 - if id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え - self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') - self.editing_waypoint_id = id - self.canvas.itemconfig(id, fill='red') - self.disp_waypoint_info(id) - self.message("Show selected waypoint information") return @@ -284,10 +338,9 @@ self.wp_info_win = tk.Toplevel() self.wp_info_win.protocol("WM_DELETE_WINDOW", self.close_wp_info) self.wp_info_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 - # ウェイポイントファイルのキーを取得し、ラベルを配置 - label_w = 4 + # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置 for i, key in enumerate(point): - key_label = tk.Label(self.wp_info_win, text=key+":", width=label_w, font=("Consolas",15), anchor=tk.E) + key_label = tk.Label(self.wp_info_win, text=key+":", width=4, font=("Consolas",15), anchor=tk.E) 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]) @@ -298,8 +351,8 @@ apply_btn = tk.Button(canv, text="Apply", width=5, height=1, background="#FDD", command=self.apply_btn_callback) apply_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) - dnd_btn = tk.Button(canv, text="DnD", width=5, height=1, background="#EEE", - command=self.dnd_btn_callback) + dnd_btn = tk.Button(canv, text="DnD", width=5, height=1, background="#EEE") + dnd_btn["command"] = lambda obj=dnd_btn: self.dnd_btn_callback(dnd_btn) dnd_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) remove_btn = tk.Button(canv, text="Remove", width=7, height=1, background="#F00", command=self.remove_btn_callback) @@ -327,12 +380,12 @@ +++++ Applyボタンを押したときのコールバック +++++ """ def apply_btn_callback(self): - wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 + wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # 編集中のウェイポイントの番号を取得 point = self.current_waypoints['waypoints'][wp_num-1]['point'] for i, key in enumerate(point): txt_box = self.wp_info_win.grid_slaves(column=1, row=i)[0] self.current_waypoints['waypoints'][wp_num-1]['point'][key] = float(txt_box.get()) - self.plot_waypoints() + self.plot_waypoints(id=self.editing_waypoint_id) self.message("Apply changes of waypoint parameters") return @@ -341,17 +394,20 @@ """ +++++ ドラッグ&ドロップボタン(DnD)を押したときのコールバック +++++ """ - def dnd_btn_callback(self): - c = self.wp_info_win.grid_slaves(row=self.wp_info_win.grid_size()[1]-1, column=0)[0] - btn = c.pack_slaves()[1] + def dnd_btn_callback(self, obj=None): + if obj is None: return + btn = obj # 押された状態とそうでない状態を切り替える if btn["relief"] == tk.RAISED: btn["relief"] = tk.SUNKEN btn["background"] = "#AAA" + self.moving_waypoint = True + self.message("Drag & Drop to move waypoint") elif btn["relief"] == tk.SUNKEN: btn["relief"] = tk.RAISED btn["background"] = "#EEE" - self.message("Drag & Drop to move waypoint") + self.moving_waypoint = False + self.message("Show selected waypoint information") return @@ -362,7 +418,8 @@ def remove_btn_callback(self): wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 self.current_waypoints['waypoints'].pop(wp_num-1) # ウェイポイントを削除 - self.plot_waypoints() # ウェイポイントを再描画、waypoints_idを更新 + self.canvas.delete(self.editing_waypoint_id) # ウェイポイントを示す円を削除 + self.waypoints_id = np.delete(self.waypoints_id, wp_num-1) # waypoints_idから削除 self.close_wp_info() self.message("Removed waypoint" + str(wp_num)) return @@ -384,53 +441,21 @@ +++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++ """ def left_click_move(self, event): - if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()) and (not self.moving_waypoint): - # ウェイポイント情報が表示されていて、ウェイポイントを動かしていないとき - c = self.wp_info_win.grid_slaves(row=self.wp_info_win.grid_size()[1]-1, column=0)[0] - dnd_btn = c.pack_slaves()[1] # dndボタンの状態を取得 - clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) - if (clicked_obj) and (clicked_obj[0] == self.editing_waypoint_id) and (dnd_btn["relief"] == tk.SUNKEN): - # dndボタンが押された状態で、編集中のウェイポイントがクリックされたとき - self.moving_waypoint = True # ウェイポイントをドラッグで移動させるモードへ - self.old_click_point = [event.x, event.y] - return + if self.moving_waypoint: 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] - #ウェイポイントをドラッグで動かしているとき - if self.moving_waypoint: - self.canvas.move(self.editing_waypoint_id, delta_x, delta_y) - box = self.canvas.bbox(self.editing_waypoint_id) - px = (box[2] + box[0]) / 2 # ウィンドウ上の座標 - py = (box[3] + box[1]) / 2 - mat_inv = np.linalg.inv(self.mat_affine) - img_xy = np.dot(mat_inv, [px, py, 1]) # マップ画像上の座標 - # マップ画像上の座標を、実際の座標に変換 - x = (img_xy[0] - self.img_origin[0]) * self.map_resolution - y = (-img_xy[1] + self.img_origin[1]) * self.map_resolution - x = round(x, 6) - y = round(y, 6) - wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] - # 編集中のウェイポイント情報を更新 - self.current_waypoints['waypoints'][wp_num]['point']['x'] = x - self.current_waypoints['waypoints'][wp_num]['point']['y'] = y - # 表示中のウェイポイント情報を更新 - txt_box = self.wp_info_win.grid_slaves(column=1, row=0)[0] - txt_box.delete(0, tk.END) - txt_box.insert(tk.END, x) - txt_box = self.wp_info_win.grid_slaves(column=1, row=1)[0] - txt_box.delete(0, tk.END) - txt_box.insert(tk.END, y) - self.old_click_point = [event.x, event.y] - return # ウェイポイント移動モードでないとき、地図を平行移動 self.translate_mat(delta_x, delta_y) self.draw_image() - self.canvas.move("origin", delta_x, delta_y) #self.plot_origin() - self.plot_waypoints() + # origin, waypoints finish_pose を平行移動 + self.canvas.move("origin", delta_x, delta_y) + for id in self.waypoints_id: + self.canvas.move(id, delta_x, delta_y) + self.canvas.move("finish_pose", delta_x, delta_y) self.old_click_point = [event.x, event.y] return @@ -441,7 +466,6 @@ """ def left_click_release(self, event): self.old_click_point = None - self.moving_waypoint = False return @@ -455,14 +479,12 @@ if clicked_obj: # 何かオブジェクトがクリックされていた場合 return # クリックされた座標 => 元画像の座標 の変換 - mat_inv = np.linalg.inv(self.mat_affine) - img_xy = np.dot(mat_inv, [event.x, event.y, 1]) + img_x, img_y = self.canvas2image(event.x, event.y) # 変換後の元画像の座標がサイズから外れている場合（地図画像の外をクリックしている） - 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): + if (img_x < 0) or (img_y < 0) or (img_x > self.__map_img_pil.width) or (img_y > self.__map_img_pil.height): return self.popup_menu.post(event.x_root, event.y_root) # メニューをポップアップ - self.right_click_coord = img_xy[0:2] # クリックされた元画像上の座標を変数に格納 + self.right_click_coord = [img_x, img_y] # クリックされた元画像上の座標を変数に格納 return @@ -470,12 +492,12 @@ """ +++++ 右クリックしてポップアップメニューのadd waypoint hereをクリックしたときのコールバック関数 +++++ """ - def add_waypoint(self): + def add_waypoint_here(self): if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): self.close_wp_info() img_xy = self.right_click_coord if self.__map_img_pil.getpixel((img_xy[0], img_xy[1])) == 0: - print("There is obstacles") + self.message("There is obstacles") return # 何番目のウェイポイントの次に追加するか入力させる self.add_wp_win = tk.Toplevel() @@ -507,24 +529,21 @@ num_box = self.add_wp_win.grid_slaves(row=0, column=1)[0] num = num_box.get() if (num == ""): - print("Please enter number") + self.message("Please enter number") return self.add_wp_win.destroy() num = int(num) img_xy = self.right_click_coord # ウェイポイント座標を計算 - x = (img_xy[0] - self.img_origin[0]) * self.map_resolution - y = (-img_xy[1] + self.img_origin[1]) * self.map_resolution - x = round(x, 6) - y = round(y, 6) + x, y = self.image2real(img_xy[0], img_xy[1]) # ウェイポイントを追加 data = CommentedMap() data['point'] = CommentedMap() for key in self.current_waypoints['waypoints'][0]['point']: - if (key == 'x'): val = x - elif (key == 'y'): val = y - else: val = 0.0 - data['point'][key] = val + if (key == 'x'): v = x + elif (key == 'y'): v = y + else: v = 0.0 + data['point'][key] = v self.current_waypoints['waypoints'].insert(num, data) self.plot_waypoints() # ウェイポイントを再描画、waypoints_idも初期化 id = self.waypoints_id[num] @@ -604,12 +623,15 @@ title="Save As", initialdir=str(Path('..','..','waypoint_nav','param')), filetypes=[("YAML", ".yaml")], - defaultextension="yaml" + defaultextension=".yaml" ) if len(new_filepath) == 0: return # cancel self.save_waypoints(new_filepath) - self.waypoint_filepath = new_filepath - self.message("Save As", "\"", new_filepath, "\"") + current_title = self.master.title() + old_filename = self.waypoint_filepath.name + self.waypoint_filepath = Path(new_filepath) + self.master.title(current_title.replace(old_filename, self.waypoint_filepath.name)) + self.message("Save As" + "\"" + str(new_filepath) + "\"") return @@ -710,6 +732,26 @@ + """ + +++++ キャンバス上（ウィンドウ上）の座標から、元の地図画像上の座標に変換 +++++ + """ + def canvas2image(self, cx, cy): + mat_inv = np.linalg.inv(self.mat_affine) + img_xy = np.dot(mat_inv, [cx, cy, 1]) + return img_xy[0], img_xy[1] + + + + """ + +++++ 元の地図画像上の座標から、実際の座標に変換 +++++ + """ + def image2real(self, ix, iy): + x = (ix - self.img_origin[0]) * self.map_resolution + y = (-iy + self.img_origin[1]) * self.map_resolution + x = round(x, 6) + y = round(y, 6) + return x, y + #===== メイン処理　プログラムはここから実行される =====# if __name__ == "__main__": diff --git a/waypoint_nav/param/test.yaml b/waypoint_nav/param/test.yaml new file mode 100644 index 0000000..4adc38e --- /dev/null +++ b/waypoint_nav/param/test.yaml @@ -0,0 +1,27 @@ +waypoints: +- point: {x: 15.549, y: 1.99887, z: 0} +- point: {x: 27.7033, y: 0.740999, z: 0} +- point: {x: 45.603441, y: -10.556894, z: 0.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.025251, 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} \ No newline at end of file diff --git a/waypoint_nav/param/waypoints_gakunai.yaml b/waypoint_nav/param/waypoints_gakunai.yaml index f4c66b1..1b42abe 100644 --- a/waypoint_nav/param/waypoints_gakunai.yaml +++ b/waypoint_nav/param/waypoints_gakunai.yaml @@ -1,100 +1,27 @@ 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 +- point: {x: 15.549, y: 1.99887, z: 0} +- point: {x: 27.7033, y: 0.740999, z: 0} +- point: {x: 44.367489, y: -3.73285, z: 0.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 + header: {seq: 0, stamp: 1623386648.025251, 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} \ No newline at end of file diff --git a/waypoint_nav/param/waypoints_gakunai_edit.yaml b/waypoint_nav/param/waypoints_gakunai_edit.yaml new file mode 100644 index 0000000..5d13f3a --- /dev/null +++ b/waypoint_nav/param/waypoints_gakunai_edit.yaml @@ -0,0 +1,27 @@ +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.025251, 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} \ No newline at end of file