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