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)
