def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) self.type = type(self).__name__ self.command = None rospy.Subscriber("move_command", String, self.cmd_handler)
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) #define max/min coordinates for orchard space boundaries = locations.get_orchard_boundaries() self.maxX = boundaries["max_x"] self.maxY = boundaries["max_y"] self.minX = boundaries["min_x"] self.minY = boundaries["min_y"] self.has_bin = False self.type = type(self).__name__ # Unique variables for picker robots # self.picker_dict = dict() self.current_bin_x = 0 self.current_bin_y = 0 self.fruit_count = 0 self.has_finished = False empty_response_pub = rospy.Publisher('empty_response_topic', empty_response, queue_size=10) def callback(data): """Execute method in response to "bin_status" message.""" if data.is_empty: # Data used to calculate if it's the closest to the bin rospy.loginfo("Bin call: " + data.bin_id + " %.1f %.1f" % (data.x, data.y)) self.current_bin_x = data.x self.current_bin_y = data.y # rospy.loginfo(len(self.picker_dict)) if self.is_closest() and not self.has_bin: # and not self.slave and not data.is_carried: self.has_finished = False current_bin = robot_storage.getRobotWithId(data.bin_id) if current_bin.designated_picker == None: 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 rospy.loginfo(self.robot_id + msg.robot_id + msg.bin_id + data.bin_id) empty_response_pub.publish(msg) rospy.loginfo("??????????????????////???????????????????") def initiate_picking(data): if data.robot_id == self.robot_id: pickerx = robot_storage.getRobotWithId(data.robot_id) self.x_start = self.position['x'] self.y_start = self.position['y'] self.add_action(NavigatePickAction(pickerx.position["x"], self.maxY + 5)) rospy.Subscriber("bin_status_topic", bin_status, callback) # rospy.Subscriber("statuses", robot_status, picker_locations) rospy.Subscriber("latched_to_picker", empty_response, initiate_picking)
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) self.counter = 0 boundaries = locations.get_wall_boundaries() self.min_x = int(boundaries["min_x"]) self.max_x = int(boundaries["max_x"]) self.min_y = int(boundaries["min_y"]) self.max_y = int(boundaries["max_y"])
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) self.type = type(self).__name__ # Unique booleans for Bin instance self.slow_down_counter = 0 self.is_publishing = True self.is_empty = True self.is_carried = False self.designated_picker = None self.designated_carrier = None self.master = None self.should_face = None self.empty_response_msg = empty_response() self.bin_latch = rospy.Publisher('latched_to_picker', empty_response, queue_size=1) # self.bin_full_latch = rospy.Publisher('latched_to_carrier', full_response, queue_size=1) def id_response(data): """Execute method in response to an empty_response message.""" # rospy.loginfo("SDAFDFDSFDSAFDSAFDSAFDSAFDSAFSADFADSFSADF") if data.bin_id == self.robot_id: self.is_publishing = False robot = robot_storage.getRobotWithId(data.robot_id) if robot.type == "PickerRobot": self.designated_picker = data.robot_id elif robot.type == "CarrierRobot": self.designated_carrier = data.robot_id # rospy.loginfo(self.robot_id + " " + data.robot_id) # self.is_carried = True def mimic_now(data): """Execute method in response to a robot_status message""" if not self.should_face and data.robot_id == self.designated_picker and not self.master: if (data.x - 0.4) <= self.position['x'] <= (data.x + 0.4): if (data.y - 0.4) <= self.position['y'] <= (data.y + 0.4): picker = robot_storage.getRobotWithId(data.robot_id) # rospy.loginfo(data.robot_id) self.latch(picker) self.empty_response_msg.robot_id = data.robot_id self.empty_response_msg.bin_id = self.robot_id if not self.should_face and data.robot_id == self.designated_carrier and not self.master: if (data.x - 0.4) <= self.position['x'] <= (data.x + 0.4): if (data.y - 0.4) <= self.position['y'] <= (data.y + 0.4): carrier = robot_storage.getRobotWithId(data.robot_id) # rospy.loginfo(data.robot_id) self.latch(carrier) self.empty_response_msg.robot_id = data.robot_id self.empty_response_msg.bin_id = self.robot_id # Suscribe to topic to recieve response from pickers. rospy.Subscriber("empty_response_topic", empty_response, id_response) rospy.Subscriber("statuses", robot_status, mimic_now)
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) self.counter = 0 boundaries = locations.get_wall_boundaries() self.min_x = int(boundaries["min_x"]) self.max_x = int(boundaries["max_x"]) self.min_y = int(boundaries["min_y"]) self.max_y = int(boundaries["max_y"])
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) self.type = type(self).__name__ # Unique variables for carrier robots self.carrier_dict = dict() self.current_bin_x = 0 self.current_bin_y = 0 self.has_bin = False self.counter = 0 self.going_towards = None empty_response_pub = rospy.Publisher('empty_response_topic', empty_response, queue_size=10) 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 bin_carrying(data): if data.robot_id == self.robot_id: rospy.loginfo("carrier " + self.robot_id + " going up to driveway") carrierx = robot_storage.getRobotWithId(data.robot_id) for i in range(0,3): if not bin_locations[i]['occupied']: bin_locations[i]['occupied'] = True self.add_action(NavigateAction(bin_locations[i]['x'], bin_locations[i]['y'])) self.add_action(UnlatchAction(self.slave)) break if i is 3: # Locations should never be all full, but who knows? bin_locations[i]['occupied'] = True self.add_action(NavigateAction(bin_locations[i]['x'], bin_locations[i]['y'])) rospy.Subscriber("latched_to_picker", empty_response, bin_carrying) rospy.Subscriber("bin_status_topic", bin_status, callback)
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) # Set boundaries boundaries = locations.get_wall_boundaries() self.min_x = boundaries["min_x"] self.max_x = boundaries["max_x"] self.min_y = boundaries["min_y"] self.max_y = boundaries["max_y"] # Instance variables self.was_blocked = False self.old_queue = [] self.d = 7
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) # Instance variables self.type = type(self).__name__ self.robot_dict = dict() self.currently_targeting = False self.dict_index = -1 self.counter = 0 self.retreat = False def robot_locations(data): """Stores picker and carrier data from the statuses topic""" if data.robot_type == "PickerRobot" or data.robot_type == "CarrierRobot": self.robot_dict[data.robot_id] = data rospy.Subscriber("statuses", robot_status, robot_locations)
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) # Set boundaries boundaries = locations.get_wall_boundaries() self.min_x = boundaries["min_x"] self.max_x = boundaries["max_x"] self.min_y = boundaries["min_y"] self.max_y = boundaries["max_y"] # Instance variables self.was_blocked = False self.old_queue = [] self.d = 7
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) self.type = type(self).__name__ self.command = None rospy.Subscriber("move_command", String, self.cmd_handler)
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) #define max/min coordinates for orchard space boundaries = locations.get_orchard_boundaries() self.maxX = boundaries["max_x"] self.maxY = boundaries["max_y"] self.minX = boundaries["min_x"] self.minY = boundaries["min_y"] self.has_bin = False self.type = type(self).__name__ # Unique variables for picker robots # self.picker_dict = dict() self.current_bin_x = 0 self.current_bin_y = 0 self.fruit_count = 0 self.has_finished = False empty_response_pub = rospy.Publisher('empty_response_topic', empty_response, queue_size=10) def callback(data): """Execute method in response to "bin_status" message.""" if data.is_empty: # Data used to calculate if it's the closest to the bin rospy.loginfo("Bin call: " + data.bin_id + " %.1f %.1f" % (data.x, data.y)) self.current_bin_x = data.x self.current_bin_y = data.y # rospy.loginfo(len(self.picker_dict)) if self.is_closest( ) and not self.has_bin: # and not self.slave and not data.is_carried: self.has_finished = False current_bin = robot_storage.getRobotWithId(data.bin_id) if current_bin.designated_picker == None: 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 rospy.loginfo(self.robot_id + msg.robot_id + msg.bin_id + data.bin_id) empty_response_pub.publish(msg) rospy.loginfo( "??????????????????////???????????????????") def initiate_picking(data): if data.robot_id == self.robot_id: pickerx = robot_storage.getRobotWithId(data.robot_id) self.x_start = self.position['x'] self.y_start = self.position['y'] self.add_action( NavigatePickAction(pickerx.position["x"], self.maxY + 5)) rospy.Subscriber("bin_status_topic", bin_status, callback) # rospy.Subscriber("statuses", robot_status, picker_locations) rospy.Subscriber("latched_to_picker", empty_response, initiate_picking)
def __init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset): Robot.__init__(self, robot_id, top_speed, angular_top_speed, x_offset, y_offset, theta_offset) self.type = type(self).__name__ # Unique variables for carrier robots self.carrier_dict = dict() self.current_bin_x = 0 self.current_bin_y = 0 self.has_bin = False self.counter = 0 self.going_towards = None empty_response_pub = rospy.Publisher('empty_response_topic', empty_response, queue_size=10) 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 bin_carrying(data): if data.robot_id == self.robot_id: rospy.loginfo("carrier " + self.robot_id + " going up to driveway") carrierx = robot_storage.getRobotWithId(data.robot_id) for i in range(0, 3): if not bin_locations[i]['occupied']: bin_locations[i]['occupied'] = True self.add_action( NavigateAction(bin_locations[i]['x'], bin_locations[i]['y'])) self.add_action(UnlatchAction(self.slave)) break if i is 3: # Locations should never be all full, but who knows? bin_locations[i]['occupied'] = True self.add_action( NavigateAction(bin_locations[i]['x'], bin_locations[i]['y'])) rospy.Subscriber("latched_to_picker", empty_response, bin_carrying) rospy.Subscriber("bin_status_topic", bin_status, callback)