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()
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 = 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()