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()
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 at_food():
global get_food_srv
try:
resp = get_food_srv()
own_pose = utils.get_own_pose()
target = resp.pose
dist = utils.dist_vec(own_pose.pose.position, target.pose.position)
return not resp.res == '' and (rospy.Time.now()-resp.pose.header.stamp).to_sec() < FIND_TIMEOUT and dist < MAX_DIST
except:
rospy.logerr("service call to get food failed")
get_food_srv.close()
get_food_srv = rospy.ServiceProxy('get_location', GetLocation, persistent = True)
return False
def at_food():
global get_food_srv
try:
resp = get_food_srv()
own_pose = utils.get_own_pose()
target = resp.pose
dist = utils.dist_vec(own_pose.pose.position, target.pose.position)
return not resp.res == '' and (
rospy.Time.now() -
resp.pose.header.stamp).to_sec() < FIND_TIMEOUT and dist < MAX_DIST
except:
rospy.logerr("service call to get food failed")
get_food_srv.close()
get_food_srv = rospy.ServiceProxy('get_location',
GetLocation,
persistent=True)
return False
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()
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()