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 cb_found_turtles(msg):
global closest, turtles
if len(msg.turtles) == 0:
closest = ''
return
closest_tmp = ''
closest_dist = MIN_DIST_ASK
for turtle in msg.turtles:
pose = utils.transformPose(turtle.position, frame=base_frame)
dist = utils.dist_vec(pose.pose.position, Vector3())
if dist < closest_dist:
closest_dist = dist
closest_tmp = turtle.name
turtles[turtle.name] = turtle.position
closest = closest_tmp
def cb_found_turtles(msg):
global closest, turtles
if len(msg.turtles) == 0:
closest = ''
return
closest_tmp = ''
closest_dist = MIN_DIST_ASK
for turtle in msg.turtles:
pose = utils.transformPose(turtle.position, frame = base_frame)
dist = utils.dist_vec(pose.pose.position, Vector3())
if dist < closest_dist:
closest_dist = dist
closest_tmp = turtle.name
turtles[turtle.name] = turtle.position
closest = closest_tmp
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 = 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()
