예제 #1
0
        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(
                            "??????????????????////???????????????????")
예제 #2
0
    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)
예제 #3
0
        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)
예제 #4
0
 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("??????????????????////???????????????????")
예제 #5
0
        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)