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 state_BALLDELIVERY():
global Ballbot_status
if(Ballbot_status == "goalreached"):
Ballbot_status = None
# activate ball delivery for 10 seconds
ballpickup_msg = BallPickup()
ballpickup_msg.direction = -1
pub_ballpickup.publish(ballpickup_msg)
rospy.sleep(10)
# deactivate ball pickup
ballpick_msg = BallPickup()
ballpickup_msg.direction = 0
pub_ballpickup.publish(ballpickup_msg)
return "delivered"
else:
return "notdelivered"
def state_BALLPICKUP():
global Ballbot_speed, Ballbot_steering
Ballbot_speed = 1.0
Steering_gain = 1.6
r = rospy.Rate(10)
while ((Ball_d >= 0.5) and not (rospy.is_shutdown())):
Ballbot_steering = Steering_gain * Ball_theta
if (Ballbot_steering >= math.radians(30)):
Ballbot_steering = math.radians(30)
elif (Ballbot_steering <= math.radians(-30)):
Ballbot_steering = -math.radians(30)
rospy.loginfo("Ball_d %f Ball_theta %f Ballbot_steering %f", Ball_d,
Ball_theta, Ballbot_steering)
pub_velcmd.publish(Ballbot_speed, Ballbot_steering)
r.sleep()
#--------------------------------------
Ballbot_speed = 0.5
Ballbot_steering = 0.0
pub_velcmd.publish(Ballbot_speed, Ballbot_steering)
# activate ball pickup for 5 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(5)
# deactivate ball pickup
ballpick_msg = BallPickup()
ballpickup_msg.direction = 1
pub_ballpickup.publish(ballpickup_msg)
Ballbot_speed = 0.0
pub_velcmd.publish(Ballbot_speed, Ballbot_steering)
#----------------------------------------
return "pickedup"
def state_BALLDELIVERY():
global Ballbot_speed, Ballbot_steering
Ballbot_speed = 1
Ballbot_steering = math.radians(30)
pub_velcmd.publish(Ballbot_speed, Ballbot_steering)
rospy.sleep(5)
Ballbot_speed = 0
Ballbot_steering = 0
pub_velcmd.publish(Ballbot_speed, Ballbot_steering)
# activate ball delivery for 5 seconds
ballpickup_msg = BallPickup()
ballpickup_msg.direction = -1
pub_ballpickup.publish(ballpickup_msg)
rospy.sleep(5)
# deactivate ball pickup
ballpick_msg = BallPickup()
ballpickup_msg.direction = 1
pub_ballpickup.publish(ballpickup_msg)
return "delivered"