Esempio n. 1
0
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"
Esempio n. 2
0
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"
Esempio n. 3
0
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"
Esempio n. 4
0
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"