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()