Fork: 0
tsukuba2022 / waypoint_navigation
Browse code
update
master
1 parent 7eaedac commit 79eca89dc5179d119618de0ea1b6b32c4b79b352
@koki koki authored on 25 Oct 2022
Patch
Unified Split
Showing 2 changed files
View
12
■■■■■
 waypoint_manager/scripts/lib/waypointlib.py
s.append("finish_pose:" + "\n")
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)
 
 
View
58
■■■■■
 waypoint_manager/scripts/manager_GUI.py
self.menu_bar.add_cascade(label="File", menu=self.file_menu) # Fileメニューとしてバーに追加
self.bind_all("<Control-s>", self.menu_save) #キーボードショートカットを設定
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)
self.finish_pose = None
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 # 画像上に示すポイントの半径ピクセル
self.footprint = [[0.25, 0.4], [0.25, -0.4], [-0.65, -0.4], [-0.65, 0.4]]
with open(path, 'w') as f:
f.write(get_waypoint_yaml(self.waypoints, self.finish_pose))
return
 
"""
+++++ 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 +++++
"""
self.mouse_position["text"] = " Out of map "
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
 
"""
+++++ 左クリックされたとき +++++
"""
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
 
"""
"""
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
# カーソルの移動量を計算
+++++ 右クリックしたとき +++++
"""
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: # 何かオブジェクトがクリックされていた場合
return
+++++ Ctrl押しながら左クリックしたとき +++++
"""
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()
self.plot_origin()
+++++ Ctrl押しながら右クリックしたとき +++++
"""
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()
self.plot_origin()