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"
