def main():
util.controlset = controlset.ControlSet()
util.costmap = util.CostMap()
root = graphics.root
root.title("Lattice Planner")
root.configure(background = 'grey')
graphics.canvas.pack()
# x initial
entryLabel_x1 = Label(root)
entryLabel_x1["text"] = "init: X"
entryLabel_x1.pack(side = LEFT)
entryWidget_x1 = Entry(root)
entryWidget_x1["width"] = 5
entryWidget_x1.pack(side = LEFT)
# y initial
entryLabel_y1 = Label(root)
entryLabel_y1["text"] = "Y"
entryLabel_y1.pack(side = LEFT)
entryWidget_y1 = Entry(root)
entryWidget_y1["width"] = 5
entryWidget_y1.pack(side = LEFT)
# theta initial
entryLabel_th1 = Label(root)
entryLabel_th1["text"] = "TH"
entryLabel_th1.pack(side = LEFT)
entryWidget_th1 = Entry(root)
entryWidget_th1["width"] = 5
entryWidget_th1.pack(side = LEFT)
# d goal (cm)
entryLabel_d = Label(root)
entryLabel_d["text"] = " goal: d"
entryLabel_d.pack(side = LEFT)
entryWidget_d = Entry(root)
entryWidget_d["width"] = 5
entryWidget_d.pack(side = LEFT)
# theta goal
entryLabel_th = Label(root)
entryLabel_th["text"] = "TH"
entryLabel_th.pack(side = LEFT)
entryWidget_th = Entry(root)
entryWidget_th["width"] = 5
entryWidget_th.pack(side = LEFT)
b = Button(root,text = "Go",command = lambda: startPlanner(float(entryWidget_x1.get()),float(entryWidget_y1.get()),float(entryWidget_th1.get()),float(entryWidget_d.get()),float(entryWidget_th.get())))
g = Button(root,text = "Show lattice",command = lambda:graphics.draw_lattice())
g.pack(side = RIGHT)
b.pack(side = RIGHT)
util.costmap.draw_costmap()
graphics.draw_court()
root.mainloop()
def startPlanner(d_goal=0.0,
th_goal=0.0,
x_goal=0.0,
y_goal=0.0,
th_goal_abs=0.0,
goaltype="None"):
"""
Send a goal message to topic 'goal'
"""
"""
Draw court
"""
global Goal_x, Goal_y
graphics.canvas.delete(ALL)
graphics.draw_court()
if (goaltype == "newball"):
th_goal = Ballbot_theta - math.radians(
th_goal) # angle to goal in global coord
# = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg]
x2 = Ballbot_x * 100.0 + d_goal * math.cos(th_goal)
y2 = Ballbot_y * 100.0 + d_goal * math.sin(th_goal)
th2 = 0 # doesn't matter for goal test
# publish goal
goal_msg = Goal()
goal_msg.goaltype = String("newball")
goal_msg.pose = Pose(x2, y2, th2)
graphics.draw_point(x2, y2, th2, color='red')
graphics.canvas.update_idletasks()
pub_goal.publish(goal_msg)
print "sent goal"
Goal_x = x2
Goal_y = y2
elif (goaltype == "gotopose"):
x2 = x_goal
y2 = y_goal
th2 = math.radians(th_goal_abs)
# publish goal
goal_msg = Goal()
goal_msg.goaltype = String("gotopose")
goal_msg.pose = Pose(x2, y2, th2)
graphics.draw_point(x2, y2, th2, color='red')
graphics.canvas.update_idletasks()
pub_goal.publish(goal_msg)
print "sent goal"
Goal_x = x2
Goal_y = y2
"""
def startPlanner(d_goal=0.0,th_goal=0.0,x_goal=0.0,y_goal=0.0,th_goal_abs=0.0,goaltype = "None"):
"""
Send a goal message to topic 'goal'
"""
"""
Draw court
"""
global Goal_x,Goal_y
graphics.canvas.delete(ALL)
graphics.draw_court()
if(goaltype == "newball"):
th_goal = Ballbot_theta - math.radians(th_goal) # angle to goal in global coord
# = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg]
x2 = Ballbot_x*100.0 + d_goal*math.cos(th_goal)
y2 = Ballbot_y*100.0 + d_goal*math.sin(th_goal)
th2 = 0 # doesn't matter for goal test
# publish goal
goal_msg = Goal()
goal_msg.goaltype = String("newball")
goal_msg.pose = Pose(x2,y2,th2)
graphics.draw_point(x2,y2,th2,color='red')
graphics.canvas.update_idletasks()
pub_goal.publish(goal_msg)
print "sent goal"
Goal_x = x2
Goal_y = y2
elif(goaltype == "gotopose"):
x2 = x_goal
y2 = y_goal
th2 = math.radians(th_goal_abs)
# publish goal
goal_msg = Goal()
goal_msg.goaltype = String("gotopose")
goal_msg.pose = Pose(x2,y2,th2)
graphics.draw_point(x2,y2,th2,color='red')
graphics.canvas.update_idletasks()
pub_goal.publish(goal_msg)
print "sent goal"
Goal_x = x2
Goal_y = y2
"""
def received_path(data):
"""
draw the new path
"""
graphics.canvas.delete(ALL)
graphics.draw_court()
graphics.draw_point(Goal_x,Goal_y,0,color='red')
for element in data.poses:
pose = element.pose
graphics.draw_point(pose.x*100.0,pose.y*100.0,pose.theta,color='blue')
#print "drew",pose.x*100.0,pose.y*100.0,pose.theta
graphics.draw_point_directed(data.poses[0].pose.x*100.0,data.poses[0].pose.y*100.0,data.poses[0].pose.theta)
graphics.draw_point_directed(data.poses[-1].pose.x*100.0,data.poses[-1].pose.y*100.0,data.poses[-1].pose.theta)
graphics.canvas.update_idletasks()
print "drew path"
def received_path(data):
"""
draw the new path
"""
graphics.canvas.delete(ALL)
graphics.draw_court()
graphics.draw_point(Goal_x, Goal_y, 0, color='red')
for element in data.poses:
pose = element.pose
graphics.draw_point(pose.x * 100.0,
pose.y * 100.0,
pose.theta,
color='blue')
#print "drew",pose.x*100.0,pose.y*100.0,pose.theta
graphics.draw_point_directed(data.poses[0].pose.x * 100.0,
data.poses[0].pose.y * 100.0,
data.poses[0].pose.theta)
graphics.draw_point_directed(data.poses[-1].pose.x * 100.0,
data.poses[-1].pose.y * 100.0,
data.poses[-1].pose.theta)
graphics.canvas.update_idletasks()
print "drew path"
def startPlanner(x1,y1,th1,d_goal,th_goal):
"""
Start A* search to connect start to goal :)
"""
"""
Draw court
"""
graphics.canvas.delete(ALL)
util.costmap.draw_costmap()
graphics.draw_court()
th1 = math.radians(th1)
"""
state = util.LatticeNode(stateparams = (787.5,1715.0,5*math.pi/4,util.ROBOT_SPEED_MAX))
nextstate = util.LatticeNode(stateparams = (682.5,1820.0,math.pi,util.ROBOT_SPEED_MAX))
print "cost of action",util.cost(state,"RS_f",nextstate)
print "cost of cell at 682.5,1820.0",util.costmap.cost_cell(682.5,1820.0)
return
"""
(x1,y1,th1,v) = util.point_to_lattice(x1,y1,th1,util.ROBOT_SPEED_MAX)
th_goal = th1 - math.radians(th_goal) # angle to goal in global coord
# = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg]
x2 = x1 + d_goal*math.cos(th_goal)
y2 = y1 + d_goal*math.sin(th_goal)
th2 = 0 # doesn't matter for goal test
v2 = util.ROBOT_SPEED_MAX
(x2,y2,th2,v2) = util.point_to_lattice(x2,y2,th2,util.ROBOT_SPEED_MAX)
if(x1 < 0) or (x1 > util.COURT_WIDTH) or (y1 < 0) or (y1 > util.COURT_LENGTH):
print "Invalid start!"
return
if(x2 < 0) or (x2 > util.COURT_WIDTH) or (y2 < 0) or (y2 > util.COURT_LENGTH):
print "Invalid goal!"
return
startNode = util.LatticeNode(stateparams = (x1,y1,th1,v))
goalNode = util.LatticeNode(stateparams = (x2,y2,th2,v2))
print "start ",startNode.get_stateparams()," goal ",goalNode.get_stateparams()
(x1,y1,th1,v1) = startNode.get_stateparams()
(x2,y2,th2,v2) = goalNode.get_stateparams()
graphics.draw_point(x1,y1,th1,'red')
graphics.draw_point(x2,y2,th2,'red')
graphics.canvas.update_idletasks()
#print "right turn ",util.turn_Right(startNode.get_stateparams(),'f')
"""
Print allowed car motions from startNode
graphics.draw_segment(startNode,"F")
graphics.draw_segment(startNode,"F3")
graphics.draw_segment(startNode,"B")
graphics.draw_segment(startNode,"R_f")
graphics.draw_segment(startNode,"R_b")
graphics.draw_segment(startNode,"L_f")
graphics.draw_segment(startNode,"L_b")
graphics.draw_segment(startNode,"SR_f")
graphics.draw_segment(startNode,"SL_f")
startNode2 = util.LatticeNode(stateparams = (490,490,math.pi/4,v))
graphics.draw_segment(startNode2,"RS_f")
graphics.draw_segment(startNode2,"LS_f")
graphics.draw_segment(startNode2,"F_diag")
graphics.draw_segment(startNode2,"F_diag3")
graphics.draw_segment(startNode2,"B_diag")
return
"""
time_exec = time.time()
goalNode = util.Astarsearch(startNode,goalNode)
print "found goal! in",time.time() - time_exec,"s"
path = []
if(goalNode != None):
node = goalNode
while(node.getParent()!= None):
parent = node.getParent()
print parent.get_stateparams(),node.getAction(),node.get_stateparams()
path.append(node.getAction())
graphics.draw_segment(parent,node.getAction())
node = parent
print ""
path.reverse()
graphics.draw_point(x1,y1,th1,'red')
graphics.draw_point(x2,y2,th2,'red')
#simulator.init_simulator(startNode,path)
else:
print "No path to goal!"
def initialize_gui():
global root
rospy.init_node('GUI')
while not rospy.is_shutdown():
root = graphics.root
root.protocol("WM_DELETE_WINDOW", windowclosed)
root.title("Lattice Planner")
root.configure(background = 'grey')
graphics.canvas.pack()
# d goal (cm)
entryLabel_d = Label(root)
entryLabel_d["text"] = "goal: d"
entryLabel_d.pack(side = LEFT)
entryWidget_d = Entry(root)
entryWidget_d["width"] = 5
entryWidget_d.pack(side = LEFT)
# theta goal
entryLabel_th = Label(root)
entryLabel_th["text"] = "TH"
entryLabel_th.pack(side = LEFT)
entryWidget_th = Entry(root)
entryWidget_th["width"] = 5
entryWidget_th.pack(side = LEFT)
# Buttons
b = Button(root,text = "Newball",command = lambda: startPlanner(d_goal = float(entryWidget_d.get()),th_goal = float(entryWidget_th.get()),
goaltype = "newball"))
b.pack(side = LEFT)
g = Button(root,text = "Show lattice",command = lambda:graphics.draw_lattice())
g.pack(side = RIGHT)
# goal_x (cm)
entryLabel_x = Label(root)
entryLabel_x["text"] = "goal: x"
entryLabel_x.pack(side = LEFT)
entryWidget_x = Entry(root)
entryWidget_x["width"] = 5
entryWidget_x.pack(side = LEFT)
# goal_y (cm)
entryLabel_y = Label(root)
entryLabel_y["text"] = "goal: y"
entryLabel_y.pack(side = LEFT)
entryWidget_y = Entry(root)
entryWidget_y["width"] = 5
entryWidget_y.pack(side = LEFT)
# goal_theta (cm)
entryLabel_th2 = Label(root)
entryLabel_th2["text"] = "goal: theta"
entryLabel_th2.pack(side = LEFT)
entryWidget_th2 = Entry(root)
entryWidget_th2["width"] = 5
entryWidget_th2.pack(side = LEFT)
b2 = Button(root,text = "GoToPose",command = lambda: startPlanner(x_goal = float(entryWidget_x.get()),y_goal = float(entryWidget_y.get()),
th_goal_abs = float(entryWidget_th2.get()),
goaltype = "gotopose"))
b2.pack(side = RIGHT)
graphics.draw_court()
graphics.draw_landmarks()
graphics.canvas.configure(cursor = "crosshair")
graphics.canvas.bind("<Button-1>", onclick)
rospy.Subscriber("pose",Pose, received_odometry)
rospy.Subscriber("path",Path,received_path)
root.mainloop()
def initialize_gui():
global root
rospy.init_node('GUI')
while not rospy.is_shutdown():
root = graphics.root
root.protocol("WM_DELETE_WINDOW", windowclosed)
root.title("Lattice Planner")
root.configure(background='grey')
graphics.canvas.pack()
# d goal (cm)
entryLabel_d = Label(root)
entryLabel_d["text"] = "goal: d"
entryLabel_d.pack(side=LEFT)
entryWidget_d = Entry(root)
entryWidget_d["width"] = 5
entryWidget_d.pack(side=LEFT)
# theta goal
entryLabel_th = Label(root)
entryLabel_th["text"] = "TH"
entryLabel_th.pack(side=LEFT)
entryWidget_th = Entry(root)
entryWidget_th["width"] = 5
entryWidget_th.pack(side=LEFT)
# Buttons
b = Button(
root,
text="Newball",
command=lambda: startPlanner(d_goal=float(entryWidget_d.get()),
th_goal=float(entryWidget_th.get()),
goaltype="newball"))
b.pack(side=LEFT)
g = Button(root,
text="Show lattice",
command=lambda: graphics.draw_lattice())
g.pack(side=RIGHT)
# goal_x (cm)
entryLabel_x = Label(root)
entryLabel_x["text"] = "goal: x"
entryLabel_x.pack(side=LEFT)
entryWidget_x = Entry(root)
entryWidget_x["width"] = 5
entryWidget_x.pack(side=LEFT)
# goal_y (cm)
entryLabel_y = Label(root)
entryLabel_y["text"] = "goal: y"
entryLabel_y.pack(side=LEFT)
entryWidget_y = Entry(root)
entryWidget_y["width"] = 5
entryWidget_y.pack(side=LEFT)
# goal_theta (cm)
entryLabel_th2 = Label(root)
entryLabel_th2["text"] = "goal: theta"
entryLabel_th2.pack(side=LEFT)
entryWidget_th2 = Entry(root)
entryWidget_th2["width"] = 5
entryWidget_th2.pack(side=LEFT)
b2 = Button(root,
text="GoToPose",
command=lambda: startPlanner(
x_goal=float(entryWidget_x.get()),
y_goal=float(entryWidget_y.get()),
th_goal_abs=float(entryWidget_th2.get()),
goaltype="gotopose"))
b2.pack(side=RIGHT)
graphics.draw_court()
graphics.draw_landmarks()
graphics.canvas.configure(cursor="crosshair")
graphics.canvas.bind("<Button-1>", onclick)
rospy.Subscriber("pose", Pose, received_odometry)
rospy.Subscriber("path", Path, received_path)
root.mainloop()
logger.info("Mean Squared Error: {}".format(mse))
Xs = [sf['x'] for i, sf in shot_features.iterrows()]
Ys = [-1.0 * sf['y'] for i, sf in shot_features.iterrows()]
# Plot the court with the shots
plt.figure(figsize=(15, 11.5))
# Plot the shots as scatter plot and change color based on expected
# value of shot
plt.title("{}".format(sys.argv[3]))
plt.scatter(Xs,
Ys,
c=shot_predicted_vals,
cmap=plt.cm.Reds,
s=250,
zorder=1)
# Darker colors represent moments earlier on in the game
cbar = plt.colorbar(orientation="horizontal")
# Invert the colorbar to have higher numbers on the left
cbar.ax.invert_xaxis()
ax = plt.gca()
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
draw_court(ax=ax)
plt.xlim(0, 94)
plt.ylim(-50, 0)
plt.show()
