def test_is_not_finished(self): """Checks if MoveAction correctly detects that it's not finished.""" action = NavigateAction(15, 16) self.robot.odometry.pose.pose.position.x = 15 self.robot.odometry.pose.pose.position.y = 15.49 self.assertFalse(action.is_finished(self.robot))
def test_start(self): """Checks if starting positions are set from robot.""" action = NavigateAction(15, 16) action.start(self.robot) self.assertEqual(action.x_start, 1) self.assertEqual(action.y_start, 1)
def acquire_target(self): """Find a new target for the animal to travel to""" target_x = self.robot_dict.values()[self.dict_index].x target_y = self.robot_dict.values()[self.dict_index].y target_id = self.robot_dict.values()[self.dict_index].robot_id rospy.loginfo("Animal:" + self.robot_id + " targeting " + str(target_id) + " at " + str(target_x) + "," + str(target_y)) # If there is something in a the queue, finish the current action if bool(self._action_queue): self._action_queue[0].finish(self) self._action_queue.pop() # Navigate to the target x, y self.add_action(NavigateAction(target_x, target_y))
def execute_callback(self): """Movement logic for person""" # Safety check for counter value if self.counter == sys.maxint: self.counter = 0 # Every 100 execution loops, generate random set of coordinates. if self.counter % 100 == 0: # rospy.loginfo("Counter:" + str(self.counter)) if self._action_queue: self._action_queue[0].finish(self) self._action_queue.pop() x_target = random.randint(self.min_x, self.max_x) y_target = random.randint(self.min_y, self.max_y) # Navigate to new random set of coordinates self.add_action(NavigateAction(x_target, y_target)) # rospy.loginfo("Uneducate person is going towards: " + str(x_target) + ", " + str(y_target)) self.counter += 1
def execute_callback(self): """Logic for the animal""" if self.retreat: # Move back to original spawned position current_x = self.position['x'] current_y = self.position['y'] target_x = self.x_offset target_y = self.y_offset distance = math.sqrt((float(current_x)-float(target_x))**2 + (float(current_y)-float(target_y))**2) if distance >= 1: return else: rospy.loginfo("The dog has arrived in the kennel") self.retreat = False # If not currently targeting, then get a new one from stored dictionary if (self.currently_targeting is False and bool(self.robot_dict)): self.dict_index = random.randint(0, len(self.robot_dict) - 1) self.currently_targeting = True return # Every 100 cycles, acquire new target location of target robot. if (self.dict_index >= 0 and self.counter >= 100): self.acquire_target() self.counter = 0 self.counter += 1 if (self.dict_index >= 0 and self.get_distance_from_target() <= 4): if bool(self._action_queue): rospy.loginfo("The dog barks") self._action_queue[0].finish(self) self._action_queue.pop() self.add_action(NavigateAction(self.x_offset, self.y_offset)) rospy.loginfo("The dog is running back to kennel") self.retreat = True self.currently_targeting = False
def callback(data): if not data.is_empty: self.current_bin_x = data.x self.current_bin_y = data.y current_bin = robot_storage.getRobotWithId(data.bin_id) if self.is_closest( ) and not self.has_bin and not current_bin.designated_carrier: self.going_towards = data.bin_id current_bin.designated_carrier = self.robot_id rospy.loginfo("Carrier bot coming towards bin " + data.bin_id + " at " + str(self.current_bin_x) + ", " + str(self.current_bin_y)) self.has_bin = True self.add_action( NavigateAction(self.current_bin_x, self.current_bin_y)) rospy.loginfo("P Robot: " + self.robot_id + " " + "Bin closest: " + data.bin_id) msg = empty_response() msg.robot_id = self.robot_id msg.bin_id = data.bin_id empty_response_pub.publish(msg)
def test_init(self): """Checks if target is set correctly after initialising.""" action = NavigateAction(15, 16) self.assertEqual(action.x_target, 15) self.assertEqual(action.y_target, 16)