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?
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):
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'
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'
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?
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()