Exemplo n.º 1
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()
Exemplo n.º 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()
Exemplo n.º 3
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()
Exemplo n.º 4
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()