示例#1
0
    def execute(self, userdata):
        global move_action_server

        target = hive_loc
        goal = utils.create_goal_message(target)
        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()
                if at_hive():
                    print 'standing still too long, but close to hive'
                    return 'success'
                else:
                    print 'standing still to long'
                    move_action_server.cancel_all_goals()
                    move_action_server = actionlib.SimpleActionClient(
                        'move_to_goal', MoveBaseAction)
                    move_action_server.wait_for_server()

                    if self.retry < MAX_RETRY:
                        self.retry += 1
                        move_action_server.send_goal(goal)
                    else:
                        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 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 < 2 * MAX_RETRY:
                    self.retry += 1
                    move_action_server.send_goal(goal)
                else:
                    move_action_server.cancel_all_goals()
                    return 'failed'
            rate.sleep()
    def execute(self, userdata):
        global move_action_server

        target = hive_loc
        goal = utils.create_goal_message(target)
        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()
                if at_hive():
                    print 'standing still too long, but close to hive'
                    return 'success'
                else:
                    print 'standing still to long'
                    move_action_server.cancel_all_goals()
                    move_action_server = actionlib.SimpleActionClient('move_to_goal', MoveBaseAction)
                    move_action_server.wait_for_server()

                    if self.retry < MAX_RETRY:
                        self.retry +=1
                        move_action_server.send_goal(goal)
                    else:
                        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 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 < 2*MAX_RETRY:
                    self.retry +=1
                    move_action_server.send_goal(goal)
                else:
                    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()
    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()
示例#5
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()
示例#6
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()
    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()
    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()