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)
