def is_closest(self):
"""Check if this picker is the closest to the specified bin."""
def dist(x, y):
"""Calculate distance from this robot to the current bin."""
d = math.sqrt((float(x) - float(self.current_bin_x))**2 +
(float(y) - float(self.current_bin_y))**2)
# rospy.loginfo("Returning distance: %d", d)
return d
robot_list = robot_storage.get_robot_list()
for p in robot_list:
if robot_list[
p].type == "PickerRobot" and not robot_list[p].has_bin:
if not robot_list[p].robot_id == self.robot_id:
# rospy.loginfo("I'm picker robot: " + robot_list[p].robot_id + "My Distance from bin %.1f is: %.1f" % (self.current_bin_x, dist(robot_list[p].position['x'], robot_list[p].position['y']))) # hi picker robot: " + robot_list[p].robot_id, I'm Dad!
if dist(self.position['x'], self.position['y']) > dist(
robot_list[p].position['x'],
robot_list[p].position['y']):
# rospy.loginfo(robot_list[p].robot_id + "Wasn't the closest to %.1f" % self.current_bin_x)
return False
elif dist(self.position['x'], self.position['y']) == dist(
robot_list[p].position['x'],
robot_list[p].position['y']):
if int(self.robot_id[6:]) > int(
robot_list[p].robot_id[6:]):
# rospy.loginfo("I wasn't the closest =")
return False
# rospy.loginfo(self.robot_id + "was the closest! %.1f" % self.current_bin_x)
return True
def is_closest(self):
"""Check if this carrier is the closest to the specified bin."""
def dist(x, y):
d = math.sqrt((float(x) - float(self.current_bin_x))**2 +
(float(y) - float(self.current_bin_y))**2)
# rospy.loginfo("Returning distance: %d", d)
return d
robot_list = robot_storage.get_robot_list()
for p in robot_list:
if robot_list[
p].type == "CarrierRobot" and not robot_list[p].has_bin:
if not robot_list[p].robot_id == self.robot_id:
if dist(self.position['x'], self.position['y']) > dist(
robot_list[p].position['x'],
robot_list[p].position['y']):
# rospy.loginfo(robot_list[p].robot_id + "Wasn't the closest to %.1f" % self.current_bin_x)
return False
elif dist(self.position['x'], self.position['y']) == dist(
robot_list[p].position['x'],
robot_list[p].position['y']):
if int(self.robot_id[6:]) > int(
robot_list[p].robot_id[6:]):
# rospy.loginfo("I wasn't the closest =")
return False
# rospy.loginfo(self.robot_id + "was the closest! %.1f" % self.current_bin_x)
return True
def is_closest(self):
"""Check if this picker is the closest to the specified bin."""
def dist(x, y):
"""Calculate distance from this robot to the current bin."""
d = math.sqrt((float(x)-float(self.current_bin_x))**2 + (float(y)-float(self.current_bin_y))**2)
# rospy.loginfo("Returning distance: %d", d)
return d
robot_list = robot_storage.get_robot_list()
for p in robot_list:
if robot_list[p].type == "PickerRobot" and not robot_list[p].has_bin:
if not robot_list[p].robot_id == self.robot_id:
# rospy.loginfo("I'm picker robot: " + robot_list[p].robot_id + "My Distance from bin %.1f is: %.1f" % (self.current_bin_x, dist(robot_list[p].position['x'], robot_list[p].position['y']))) # hi picker robot: " + robot_list[p].robot_id, I'm Dad!
if dist(self.position['x'], self.position['y']) > dist(robot_list[p].position['x'], robot_list[p].position['y']):
# rospy.loginfo(robot_list[p].robot_id + "Wasn't the closest to %.1f" % self.current_bin_x)
return False
elif dist(self.position['x'], self.position['y']) == dist(robot_list[p].position['x'], robot_list[p].position['y']):
if int(self.robot_id[6:]) > int(robot_list[p].robot_id[6:]):
# rospy.loginfo("I wasn't the closest =")
return False
# rospy.loginfo(self.robot_id + "was the closest! %.1f" % self.current_bin_x)
return True
def is_closest(self):
"""Check if this carrier is the closest to the specified bin."""
def dist(x, y):
d = math.sqrt((float(x)-float(self.current_bin_x))**2 + (float(y)-float(self.current_bin_y))**2)
# rospy.loginfo("Returning distance: %d", d)
return d
robot_list = robot_storage.get_robot_list()
for p in robot_list:
if robot_list[p].type == "CarrierRobot" and not robot_list[p].has_bin:
if not robot_list[p].robot_id == self.robot_id:
if dist(self.position['x'], self.position['y']) > dist(robot_list[p].position['x'], robot_list[p].position['y']):
# rospy.loginfo(robot_list[p].robot_id + "Wasn't the closest to %.1f" % self.current_bin_x)
return False
elif dist(self.position['x'], self.position['y']) == dist(robot_list[p].position['x'], robot_list[p].position['y']):
if int(self.robot_id[6:]) > int(robot_list[p].robot_id[6:]):
# rospy.loginfo("I wasn't the closest =")
return False
# rospy.loginfo(self.robot_id + "was the closest! %.1f" % self.current_bin_x)
return True
def test_add_robot(self):
"""test add robot method"""
robot_storage.addRobot(self.robot_obj, self.robot_obj.robot_id)
test_list = robot_storage.get_robot_list()
self.assertIn(self.robot_obj.robot_id, test_list)
def test_add_robot(self):
"""test add robot method"""
robot_storage.addRobot(self.robot_obj, self.robot_obj.robot_id)
test_list = robot_storage.get_robot_list()
self.assertIn(self.robot_obj.robot_id, test_list)
