예제 #1
0
def init_globals():
    global name, hive, hive_loc, move_random_stop, move_random_start, get_food_srv, get_hive_srv, move_action_server
    utils.init_globals()

    move_action_server = actionlib.SimpleActionClient('move_to_goal',
                                                      MoveBaseAction)
    move_action_server.wait_for_server()

    hive_loc = PoseStamped()
    hive_loc.header.frame_id = hive
    hive_loc.pose.orientation.w = 1
    hive_loc = utils.move_location_inwards(hive_loc, INWARDS)

    get_food_srv = rospy.ServiceProxy('get_location',
                                      GetLocation,
                                      persistent=True)
    get_hive_srv = rospy.ServiceProxy('get_hive', GetLocation, persistent=True)

    move_random_start = rospy.ServiceProxy('move_random_start', Empty)
    move_random_stop = rospy.ServiceProxy('move_random_stop', Empty)

    name = gethostname()

    rospy.Subscriber('/found_turtles', Turtles,
                     cb_found_turtles)  #which turtles are near?
예제 #2
0
    def execute(self, userdata):
        target = None
        if self.loc == 'hive':
            target = hive_loc
        else:
            target = get_food()
            if target is None:
                return 'failed'
            target = utils.move_location_inwards(target, INWARDS)

        goal = utils.move_location(target, x=-0.2, y=Y_OFFSET)
        goal = utils.create_goal_message(goal)

        move_action_server.send_goal(goal)

        rate = rospy.Rate(RATE)

        start = rospy.Time.now()

        stand_still = 0
        old_pose = utils.get_own_pose()

        while True:
            if stand_still > STAND_STILL_TIMES:
                move_action_server.cancel_all_goals()
                return 'failed'
            if utils.standing_still(old_pose):
                stand_still += 1
            else:
                stand_still = 0
                old_pose = utils.get_own_pose()

            if (rospy.Time.now() - start).to_sec() > self.TIME_OUT:
                move_action_server.cancel_all_goals()
                return 'failed'
            if move_action_server.get_state() == GoalStatus.SUCCEEDED:
                move_action_server.cancel_all_goals()
                return 'success'
            if move_action_server.get_state() == GoalStatus.PREEMPTED:
                move_action_server.cancel_all_goals()
                return 'failed'
            rate.sleep()
예제 #3
0
    def execute(self, userdata):
        target = None
        if self.loc =='hive':
            target = hive_loc
        else:
            target = get_food()
            if target is None:
                return 'failed'
            target = utils.move_location_inwards(target, INWARDS)
            
        goal = utils.move_location(target, x = -0.2, y = Y_OFFSET)
        goal = utils.create_goal_message(goal)

        move_action_server.send_goal(goal)

        rate = rospy.Rate(RATE)

        start = rospy.Time.now()
        
        stand_still = 0
        old_pose = utils.get_own_pose()
        
        while True:
            if stand_still > STAND_STILL_TIMES:
                move_action_server.cancel_all_goals()
                return 'failed'
            if utils.standing_still(old_pose):
                stand_still += 1
            else:
                stand_still = 0
                old_pose = utils.get_own_pose()
        
            if (rospy.Time.now()-start).to_sec() > self.TIME_OUT:
                move_action_server.cancel_all_goals()
                return 'failed' 
            if move_action_server.get_state() == GoalStatus.SUCCEEDED:
                move_action_server.cancel_all_goals()
                return 'success'
            if move_action_server.get_state() == GoalStatus.PREEMPTED:
                move_action_server.cancel_all_goals()
                return 'failed'
            rate.sleep()
예제 #4
0
    def execute(self, userdata):
        target = None
        found = None
        if self.loc =='hive':
            target = hive_loc
            found = at_hive
        else: #food
            target = get_food()
                        
            if target is None and userdata.pose_in is not None:
                target = userdata.pose_in
                
            if target is None:
                try:
                    self.forget_food()
                except:
                    print "forget_food failed"
                return 'failed'
            found = at_food
            target = utils.move_location_inwards(target, INWARDS)
        

        ang = utils.get_jaw(target.pose.orientation)
        rate = rospy.Rate(RATE)

        while not found():
            if utils.rotation_aligned(ang):
                if self.loc == 'food':
                    try:
                        self.forget_food()
                    except:
                        print "forget_food failed"

                return 'failed'
            utils.rotate_to_ang(ang)
            rate.sleep()
        return 'success'
예제 #5
0
    def execute(self, userdata):
        target = None
        found = None
        if self.loc == 'hive':
            target = hive_loc
            found = at_hive
        else:  #food
            target = get_food()

            if target is None and userdata.pose_in is not None:
                target = userdata.pose_in

            if target is None:
                try:
                    self.forget_food()
                except:
                    print "forget_food failed"
                return 'failed'
            found = at_food
            target = utils.move_location_inwards(target, INWARDS)

        ang = utils.get_jaw(target.pose.orientation)
        rate = rospy.Rate(RATE)

        while not found():
            if utils.rotation_aligned(ang):
                if self.loc == 'food':
                    try:
                        self.forget_food()
                    except:
                        print "forget_food failed"

                return 'failed'
            utils.rotate_to_ang(ang)
            rate.sleep()
        return 'success'
예제 #6
0
def init_globals():
    global name, hive, hive_loc, move_random_stop, move_random_start, get_food_srv, get_hive_srv, move_action_server
    utils.init_globals()


    move_action_server = actionlib.SimpleActionClient('move_to_goal', MoveBaseAction)
    move_action_server.wait_for_server()


    hive_loc = PoseStamped()
    hive_loc.header.frame_id = hive
    hive_loc.pose.orientation.w = 1
    hive_loc = utils.move_location_inwards(hive_loc, INWARDS)
    
    get_food_srv = rospy.ServiceProxy('get_location', GetLocation, persistent = True)
    get_hive_srv = rospy.ServiceProxy('get_hive', GetLocation, persistent = True)

    move_random_start = rospy.ServiceProxy('move_random_start', Empty)
    move_random_stop = rospy.ServiceProxy('move_random_stop', Empty)

    
    name = gethostname()

    rospy.Subscriber('/found_turtles', Turtles, cb_found_turtles) #which turtles are near?
예제 #7
0
    def execute(self, userdata):
        global move_action_server

        target = get_food()

        if target is None and userdata.pose_in is not None:
            target = userdata.pose_in
            userdata.pose_out = target

        if target is None:
            try:
                self.forget_food(location="")
            except:
                print "forget_food failed"
            print "target is None"
            return 'failed'
        goal = utils.create_goal_message(
            utils.move_location_inwards(target, INWARDS))
        move_action_server.send_goal(goal)

        rate = rospy.Rate(RATE)

        self.retry = 0
        stand_still = 0
        old_pose = utils.get_own_pose()

        while True:
            if stand_still > STAND_STILL_TIMES:
                move_action_server.cancel_all_goals()
                move_action_server = actionlib.SimpleActionClient(
                    'move_to_goal', MoveBaseAction)
                move_action_server.wait_for_server()

                if at_food():
                    print "standing still too long, but close to food"
                    return 'success'
                else:
                    print "standing still too long, failed to go to food"

                    try:
                        self.forget_food(location="")
                    except:
                        print "forget_food failed"
                    return 'failed'
            if utils.standing_still(old_pose):
                stand_still += 1
            else:
                stand_still = 0
                old_pose = utils.get_own_pose()
            pose = get_food()
            if pose is not None and (utils.dist_vec(
                    pose.pose.position, target.pose.position) > EPS_TARGETS):
                print "resending goal"
                move_action_server.cancel_all_goals()
                target = pose
                goal = utils.create_goal_message(
                    utils.move_location_inwards(target, INWARDS))
                move_action_server.send_goal(goal)

            if move_action_server.get_state() == GoalStatus.SUCCEEDED:
                move_action_server.cancel_all_goals()
                return 'success'
            if move_action_server.get_state() == GoalStatus.PREEMPTED:
                move_action_server.cancel_all_goals()
                if self.retry < MAX_RETRY:
                    self.retry += 1
                    move_action_server.send_goal(goal)
                else:
                    try:
                        self.forget_food(location="")
                    except:
                        print "forget_food failed"
                    return 'failed'
            rate.sleep()
예제 #8
0
    def execute(self, userdata):
        global move_action_server

        target = None

        if self.loc == 'hive':
            target = hive_loc
            at_loc = at_hive
        else:
            target = get_food()

            if target is None and userdata.pose_in is not None:
                target = userdata.pose_in
                userdata.pose_out = target

            if target is None:
                return 'failed'
            at_loc = at_food
            target = utils.move_location_inwards(target, INWARDS)

        if at_loc():
            return 'success'

        goal = utils.move_location(target, x=-1.0, y=-0.5)
        goal = utils.create_goal_message(goal)

        move_action_server.send_goal(goal)

        rate = rospy.Rate(RATE)

        start = rospy.Time.now()
        stand_still = 0
        old_pose = utils.get_own_pose()

        retry = MAX_RETRY - 2
        while True:
            if stand_still > STAND_STILL_TIMES:
                move_action_server.cancel_all_goals()
                print "standing still too long for moviing to in location"
                move_action_server = actionlib.SimpleActionClient(
                    'move_to_goal', MoveBaseAction)
                move_action_server.wait_for_server()

                return 'failed'
            if utils.standing_still(old_pose):
                stand_still += 1
            else:
                stand_still = 0
                old_pose = utils.get_own_pose()

            if move_action_server.get_state() == GoalStatus.SUCCEEDED:
                move_action_server.cancel_all_goals()
                return 'success'
            if move_action_server.get_state() == GoalStatus.PREEMPTED:
                move_action_server.cancel_all_goals()
                if retry > MAX_RETRY:
                    return 'failed'
                else:
                    retry += 1
                    move_random_start()
                    rospy.sleep(MOVE_RANDOM_TIME)
                    move_random_stop()
                    move_action_server.send_goal(goal)
            rate.sleep()
예제 #9
0
    def execute(self, userdata):
        global move_action_server

        target = get_food()

        if target is None and userdata.pose_in is not None:
            target = userdata.pose_in
            userdata.pose_out = target

        if target is None:
            try:
                self.forget_food(location = "")
            except:
                print "forget_food failed"
            print "target is None"
            return 'failed'
        goal = utils.create_goal_message(utils.move_location_inwards(target, INWARDS))
        move_action_server.send_goal(goal)

        rate = rospy.Rate(RATE)

        self.retry = 0
        stand_still = 0
        old_pose = utils.get_own_pose()
        
        while True:
            if stand_still > STAND_STILL_TIMES:
                move_action_server.cancel_all_goals()
                move_action_server = actionlib.SimpleActionClient('move_to_goal', MoveBaseAction)
                move_action_server.wait_for_server()

                if at_food():
                    print "standing still too long, but close to food"
                    return 'success'
                else:
                    print "standing still too long, failed to go to food"

                    try:
                        self.forget_food(location = "")
                    except:
                        print "forget_food failed"
                    return 'failed'
            if utils.standing_still(old_pose):
                stand_still += 1
            else:
                stand_still = 0
                old_pose = utils.get_own_pose()
            pose = get_food()
            if pose is not None and (utils.dist_vec(pose.pose.position, target.pose.position) > EPS_TARGETS):
                print "resending goal"
                move_action_server.cancel_all_goals()
                target = pose
                goal = utils.create_goal_message(utils.move_location_inwards(target, INWARDS))
                move_action_server.send_goal(goal)
                
            if move_action_server.get_state() == GoalStatus.SUCCEEDED:
                move_action_server.cancel_all_goals()
                return 'success'
            if move_action_server.get_state() == GoalStatus.PREEMPTED:
                move_action_server.cancel_all_goals()
                if self.retry < MAX_RETRY:
                    self.retry +=1
                    move_action_server.send_goal(goal)
                else:
                    try:
                        self.forget_food(location = "")
                    except:
                        print "forget_food failed"
                    return 'failed'
            rate.sleep()
예제 #10
0
    def execute(self, userdata):
        global move_action_server

        target = None

        if self.loc =='hive':
            target = hive_loc
            at_loc = at_hive
        else:
            target = get_food()
            
            if target is None and userdata.pose_in is not None:
                target = userdata.pose_in
                userdata.pose_out = target

            if target is None:
                return 'failed'
            at_loc = at_food
            target = utils.move_location_inwards(target, INWARDS)

        if at_loc():
            return 'success'
        
        goal = utils.move_location(target, x = -1.0, y = -0.5)
        goal = utils.create_goal_message(goal)

        move_action_server.send_goal(goal)

        rate = rospy.Rate(RATE)

        start = rospy.Time.now()
        stand_still = 0
        old_pose = utils.get_own_pose()

        retry = MAX_RETRY - 2
        while True:
            if stand_still > STAND_STILL_TIMES:
                move_action_server.cancel_all_goals()
                print "standing still too long for moviing to in location"
                move_action_server = actionlib.SimpleActionClient('move_to_goal', MoveBaseAction)
                move_action_server.wait_for_server()

                return 'failed'
            if utils.standing_still(old_pose):
                stand_still += 1
            else:
                stand_still = 0
                old_pose = utils.get_own_pose()
                
            if move_action_server.get_state() == GoalStatus.SUCCEEDED:
                move_action_server.cancel_all_goals()
                return 'success'
            if move_action_server.get_state() == GoalStatus.PREEMPTED:
                move_action_server.cancel_all_goals()
                if retry > MAX_RETRY:
                    return 'failed'
                else:
                    retry += 1
                    move_random_start()
                    rospy.sleep(MOVE_RANDOM_TIME)
                    move_random_stop()
                    move_action_server.send_goal(goal)
            rate.sleep()