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 startPlanner(data): global startNode,goalNode,plan,path (x1,y1,th1,v) = util.point_to_lattice(Ballbot_x,Ballbot_y,Ballbot_theta,parameters.ROBOT_SPEED_MAX) v2 = util.ROBOT_SPEED_MAX (x2,y2,th2,v2) = util.point_to_lattice(data.pose.x,data.pose.y,data.pose.theta,parameters.ROBOT_SPEED_MAX) if(x1 < 0) or (x1 > parameters.COURT_WIDTH) or (y1 < 0) or (y1 > parameters.COURT_LENGTH): print "Invalid start!",(x1,y1,th1) return if(x2 < 0) or (x2 > parameters.COURT_WIDTH) or (y2 < 0) or (y2 > parameters.COURT_LENGTH): print "Invalid goal!",(x2,y2,th2) return util.goaltype = data.goaltype.data startNode = util.LatticeNode(stateparams = (x1,y1,th1,v)) if(util.goaltype == "gotopose"): goalNode = util.LatticeNode(stateparams = (x2,y2,th2,v2)) else: goalNode = util.LatticeNode(stateparams = (data.pose.x,data.pose.y,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() ########################################## DEBUGGING if (parameters.SEARCHALGORITHM == "straightline"): debugpaths.straightLine((x1,y1,th1,v1),(x2,y2,th2,v2)) return elif (parameters.SEARCHALGORITHM == "figureofeight"): debugpaths.figureofeight((x1,y1,th1,v1)) return ########################################################################################################### elif (parameters.SEARCHALGORITHM == "A*"): Astarsearch(startNode,goalNode) elif (parameters.SEARCHALGORITHM == "LPA*"): print "running LPA*" LPAstarsearch(startNode,goalNode) elif (parameters.SEARCHALGORITHM == "MT-AdaptiveA*"): rospy.loginfo("running MT-AdaptiveA* %s",(startNode.get_stateparams(), goalNode.get_stateparams())) util.goaltype = data.goaltype.data MTAdaptiveAstarsearch(startNode,goalNode) if plan == None: print "Plan of length 0" return path = [] path.append((x1/100.0,y1/100.0,th1,'s','f')) path = path + util.plan_to_path(plan) path[0] = (x1/100.0,y1/100.0,th1,path[1][3],path[1][4]) print "plan of length",len(plan) actionlist=[] for (Node,action) in plan: actionlist.append(action) #print Node.get_stateparams(),action,Node.get_g() rospy.loginfo(actionlist) raw_input("Hit any key to confirm path") path_to_send = Path() for point in path: pose = Pose() # plan uses center of car, so transform such that path has points to be traversed by rear axle center, # which is 17.41 cm away from the center pose.x = point[0] - 17.41*math.cos(point[2])/100.0 pose.y = point[1] - 17.41*math.sin(point[2])/100.0 pose.theta = point[2] path_element = PathElement() path_element.pose = pose path_element.type = point[3] path_element.direction = point[4] path_to_send.poses.append(path_element) #path[0] = (x1/100.0,y1/100.0,th1,path[1][3],path[1][4]) pub_path.publish(path_to_send) print "path published"