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)