def state_BALLPICKUP():
global Goal_current,Ballbot_status
while((Ballbot_status != "goalreached") and not(rospy.is_shutdown())):
if(Ball_x >= 0) and (Ball_y >= 0): # if ball is visible
dist = math.sqrt(math.pow((Goal_current.pose.x/100.0 - Ball_x),2) + math.pow((Goal_current.pose.y/100.0 - Ball_y),2))
goal_msg = Goal()
goal_msg.pose = Pose(Ball_x*100.0,Ball_y*100.0,Ball_theta)
if (dist >= 0.10) and (dist <= 0.5):
goal_msg.goaltype = String("updategoal")
pub_goal.publish(goal_msg)
Goal_current = goal_msg
#print (Ball_x,Ball_y),goal_msg.goaltype
elif(dist >= 0.5):
goal_msg.goaltype = String("newball")
pub_goal.publish(goal_msg)
Goal_current = goal_msg
#print (Ball_x,Ball_y),goal_msg.goaltype
rospy.sleep(1.0) # update goal position every 1 s for now
Ballbot_status = None
#--------------------------------------
# activate ball pickup for 10 seconds
ballpickup_msg = BallPickup()
ballpickup_msg.direction = -1
pub_ballpickup.publish(ballpickup_msg)
ballpickup_msg = BallPickup()
ballpickup_msg.direction = 0
pub_ballpickup.publish(ballpickup_msg)
rospy.sleep(10)
# deactivate ball pickup
ballpick_msg = BallPickup()
ballpickup_msg.direction = 1
pub_ballpickup.publish(ballpickup_msg)
#----------------------------------------
goal_msg = Goal()
goal_msg.pose = Pose(10.0 * 100.0,10.0 * 100.0,0.0)
goal_msg.goaltype = String("gotopose")
print "Go to (10.0 10.0 0); hit any key to drive"
raw_input()
pub_goal.publish(goal_msg)
Goal_current = goal_msg
return "pickedup"
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 goalsender():
argv = rospy.myargv(argv=sys.argv)
while (Ballbot_x == -10) and not (rospy.is_shutdown()):
rospy.sleep(1)
if (len(argv) == 3):
d_goal = float(argv[1]) * 100
th_goal = float(argv[2])
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]
th_goal = th_goal % (2 * math.pi)
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)
pub_goal.publish(goal_msg)
print "pose", (Ballbot_x, Ballbot_y, Ballbot_theta)
print "sent goal", (x2, y2, th2)
else:
rospy.loginfo(
"Usage: rosrun lattice_planner goalsender.py d_dist[m] d_theta[deg]"
)
rospy.signal_shutdown("Exiting")
def goalsender():
argv = rospy.myargv(argv=sys.argv)
while (Ballbot_x == -10) and not (rospy.is_shutdown()):
rospy.sleep(1)
if(len(argv) == 3):
d_goal = float(argv[1])*100
th_goal = float(argv[2])
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]
th_goal = th_goal%(2*math.pi)
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)
pub_goal.publish(goal_msg)
print "pose",(Ballbot_x,Ballbot_y,Ballbot_theta)
print "sent goal",(x2,y2,th2)
else:
rospy.loginfo("Usage: rosrun lattice_planner goalsender.py d_dist[m] d_theta[deg]")
rospy.signal_shutdown("Exiting")
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 state_IDLE():
global Goal_current,Ball_x,Ball_y,Ball_theta
if (Ball_x >= 0) and (Ball_y >= 0):
print "new ball seen. Hit a key to pickup",Ball_x,Ball_y,Ball_theta
raw_input()
goal_msg = Goal()
goal_msg.goaltype = String("newball")
goal_msg.pose = Pose(Ball_x*100.0,Ball_y*100.0,Ball_theta) pub_goal.publish(goal_msg)
Goal_current = goal_msg
return "newballseen"
else:
return "noball"