def load_fact_message(self, attribute, uploaded_instance, item, domain):
        fact_msg = KnowledgeItem()
        fact_msg.knowledge_type = KnowledgeItem.FACT
        fact_msg.instance_type = ''
        fact_msg.instance_name = ''
        fact_msg.attribute_name = attribute
        fact_msg.function_value = 0.0
        output_value = KeyValue()
        output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
        output_value.value = uploaded_instance
        fact_msg.values.append(output_value)

        #write to knowledge base
        ## For insert domain, the attribute container needs to be added as a fact, without second term
        if (('in' == attribute) or ('on' == attribute)):
            output_value = KeyValue()
            output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
            output_value.value = self.msg_to_instance_name(item, domain)
            fact_msg.values.append(output_value)

            ##Robocup hack for finals 
            ## Removing instance of objects to be picked form rotation table CB02
            if "CB02" == output_value.value or "CB01" == output_value.value:
                print "Rotating table knowledge base not updateing for object",uploaded_instance
                self.object_to_delete_goals.append(uploaded_instance)
                return
            if "SH01" == output_value.value or "SH02" == output_value.value:
                print "Removing object tot pick from shelf ,", uploaded_instance
                self.object_to_delete_goals.append(uploaded_instance)
                return

        #print (fact_msg)
        self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_KNOWLEDGE'])
Exemplo n.º 2
0
    def update_kb(self):
        kus = KnowledgeUpdateServiceArrayRequest()
        # Get info from KB functions and propositions (to know type of variable)
        # Fill update
        self.dump_cache_mutex.acquire(True)
        self.mutex.acquire(True)
        sensed_predicates = self.sensed_topics.copy()
        self.mutex.release()
        self.srv_mutex.acquire(True)
        sensed_predicates.update(self.sensed_services.copy())
        self.srv_mutex.release()

        for predicate, (val, changed,
                        updated) in sensed_predicates.iteritems():
            if updated:
                continue

            if predicate in self.sensed_topics:
                self.mutex.acquire(True)
                self.sensed_topics[predicate] = (
                    self.sensed_topics[predicate][0], False, True
                )  # Set to not changed and updated KB
                self.mutex.release()
            else:
                self.srv_mutex.acquire(True)
                self.sensed_services[predicate] = (
                    self.sensed_services[predicate][0], False, True
                )  # Set to not changed and updated KB
                self.srv_mutex.release()
            predicate_info = predicate.split(':')
            ki = KnowledgeItem()
            ki.attribute_name = predicate_info.pop(0)
            if type(val) is bool:
                ki.knowledge_type = ki.FACT
                ki.is_negative = not val
            else:
                ki.knowledge_type = ki.FUNCTION
                ki.function_value = val

            # TODO what to do if no parameters specified? iterate over all the instantiated parameters and add them?
            for i, param in enumerate(predicate_info):
                kv = KeyValue()
                kv.key = 'p' + str(i)
                kv.value = param
                ki.values.append(kv)

            kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring()
            kus.knowledge.append(ki)
        self.dump_cache_mutex.release()

        # Update the KB with the full array
        if len(kus.update_type) > 0:
            try:
                self.update_kb_srv.call(kus)
            except Exception as e:
                rospy.logerr(
                    "KCL (SensingInterface) Failed to update knowledge base: %s"
                    % e.message)
Exemplo n.º 3
0
    def fill_order(self, order, attribute, first, second, count=1, container_name=None):
        fact_msg = KnowledgeItem()
        fact_msg.knowledge_type = KnowledgeItem.FACT
        fact_msg.instance_type = ""
        fact_msg.instance_name = ""
        fact_msg.attribute_name = attribute
        fact_msg.function_value = 0.0
        if 5 == order.destination.type.data:
            fact_msg.attribute_name = "in"
        output_value = KeyValue()
        output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[
            0
        ]
        name_of_object = self.msg_to_instance_name(order, first, count)
        output_value.value = name_of_object

        ##Robocup finals hack not adding goals for object to be picked from rotation table
        if output_value.value in self.object_to_delete_goals:
            print "Not adding goal for ", output_value.value
            return

        # if 5 == order.object.type.data or 12 == order.object.type.data:
        #    print "Not adding M20_100 and distance tube goal"
        #    return

        fact_msg.values.append(output_value)
        output_value = KeyValue()
        output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[
            1
        ]
        if not container_name:
            # in order the container or location doesnt have the count
            output_value.value = self.msg_to_instance_name(order, second)
        else:
            output_value.value = container_name
        # if 'SH01' == order.destination.type.data or 'SH02' == order.destination.type.data:
        #    print "not adding goal for shelfes"
        #    return
        ## For PP01 adding insert
        if 5 == order.destination.type.data:
            # Hack for PPT for Nagoya
            #################
            # print "Not adding Goals for PP01"
            # return
            #################
            output_value.value = "PP01_CAVITY"

        fact_msg.values.append(output_value)

        self.write_to_knowledge_base(
            fact_msg, self.knowledge_update_service["ADD_GOAL"]
        )

        return name_of_object
    def load_instance_message(self, domain, item, count=1 ):
        #Creating the message
        instance_msg = KnowledgeItem()
        #uploading the instance of object

        instance_msg.knowledge_type = KnowledgeItem.INSTANCE
        instance_msg.instance_type = self.domain_name[domain]
        instance_msg.instance_name = self.msg_to_instance_name(item, domain, count)
        instance_msg.attribute_name = ''
        instance_msg.function_value = 0.0

        self.write_to_knowledge_base(instance_msg, self.knowledge_update_service['ADD_KNOWLEDGE'])
        return instance_msg.instance_name
Exemplo n.º 5
0
    def load_instance_message_PP01(self, domain):
        # Creating the message
        instance_msg = KnowledgeItem()
        # uploading the instance of object

        instance_msg.knowledge_type = KnowledgeItem.INSTANCE
        instance_msg.instance_type = self.domain_name[domain]
        instance_msg.instance_name = "PP01_CAVITY"
        instance_msg.attribute_name = ""
        instance_msg.function_value = 0.0

        self.write_to_knowledge_base(
            instance_msg, self.knowledge_update_service["ADD_KNOWLEDGE"]
        )
        return instance_msg.instance_name
Exemplo n.º 6
0
def get_kb_update(instances, d):
    kua = KnowledgeUpdateServiceArrayRequest()
    n = len(instances)
    assert (n == len(d) and n == len(d[0]))
    for i in xrange(n):
        for j in xrange(n):
            ki = KnowledgeItem()
            ki.knowledge_type = ki.FUNCTION
            ki.attribute_name = 'distance'
            kv = KeyValue()
            kv.key = 'a'
            kv.value = instances[i]
            ki.values.append(kv)
            kv = KeyValue()
            kv.key = 'b'
            kv.value = instances[j]
            ki.values.append(kv)
            ki.function_value = d[i][j]
            kua.update_type += np.array(kua.ADD_KNOWLEDGE).tostring()
            kua.knowledge.append(ki)
    return kua
Exemplo n.º 7
0
    def fill_order(self, order, attribute, first, second, count=-1):
        #In transportationtask sometimes the destination is not there in inventory
        #Needs to add fact about the inventory
        if 'destination' == second:
            name = self.load_instance_message(KnowledgeItem.INSTANCE, 'destination', order)
        fact_msg = KnowledgeItem()
        fact_msg.knowledge_type = KnowledgeItem.FACT
        fact_msg.instance_type = ''
        fact_msg.instance_name = ''
        fact_msg.attribute_name = attribute
        fact_msg.function_value = 0.0
        output_value = KeyValue()
        output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
        output_value.value = self.msg_to_instance_name(order, first, count)

        fact_msg.values.append(output_value)
        output_value = KeyValue()
        output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
        #in order the container or location doesnt have the count
        output_value.value = self.msg_to_instance_name(order, second )
        fact_msg.values.append(output_value)
        self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_GOAL'] )
Exemplo n.º 8
0
    def rosplan_update_knowledge(self,
                                 knowledge_type,
                                 instance_type,
                                 instance_name,
                                 attribute_name,
                                 values,
                                 function_value=0.0,
                                 update_type='ADD_KNOWLEDGE'):
        '''
        This generic function receives knowledge parameters and does a service call
        to rosplan knowledge base (mongodb)

        rosplan_update_knowledge() = rosplan service call add knowledge

        Example 1:
        # upload instance : youbot has one dynamixel gripper (dynamixel - gripper)
        rosservice call /kcl_rosplan/update_knowledge_base "update_type: 0 # ADD_KNOWLEDGE = 0
            knowledge:
            knowledge_type: 0 # INSTANCE = 0
            instance_type: 'gripper'
            instance_name: 'dynamixel'
            attribute_name: ''
            values:
            -{}
            function_value: 0.0";
        # (on o4 S3)

        example 2: object4 (o4) is on location S3
        # upload fact :
        rosservice call /kcl_rosplan/update_knowledge_base "update_type: 0 # ADD_KNOWLEDGE = 0
            knowledge:
            knowledge_type: 1 # FACT = 1
            instance_type: ''
            instance_name: ''
            attribute_name: 'on'
            values:
            - {key: 'o', value: 'o4'}
            - {key: 'l', value: 'S3'}
            function_value: 0.0";

        '''
        msg = KnowledgeItem()
        if knowledge_type == 0:
            # instance
            msg.knowledge_type = KnowledgeItem.INSTANCE  # INSTANCE (rosplan_knowledge_msgs/msg/KnowledgeItem.msg)
            msg.instance_type = instance_type
            msg.instance_name = instance_name
            msg.attribute_name = ''
        elif knowledge_type == 1:
            # fact
            msg.knowledge_type = KnowledgeItem.FACT  # FACT (rosplan_knowledge_msgs/msg/KnowledgeItem.msg)
            msg.instance_type = ''
            msg.instance_name = ''
            msg.attribute_name = attribute_name
            for each in values:
                msg.values.append(KeyValue(each[0], each[1]))
        else:
            rospy.logerr(
                'error: no information will be uploaded, Knowledge type should be INSTANCE = 0, or FACT = 1'
            )
            return False
        msg.function_value = function_value
        rospy.logdebug('======msg=========')
        rospy.logdebug(msg)
        rospy.logdebug('================')
        rospy.loginfo(
            'Waiting for /kcl_rosplan/update_knowledge_base to become available...'
        )
        rospy.wait_for_service('/kcl_rosplan/update_knowledge_base')
        try:
            update_kb = rospy.ServiceProxy(
                '/kcl_rosplan/update_knowledge_base', KnowledgeUpdateService)
            # get response
            if update_type == 'ADD_KNOWLEDGE':
                response = update_kb(
                    KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE, msg)
            elif update_type == 'ADD_GOAL':
                response = update_kb(KnowledgeUpdateServiceRequest.ADD_GOAL,
                                     msg)
            elif update_type == 'REMOVE_KNOWLEDGE':
                response = update_kb(
                    KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE, msg)
            elif update_type == 'REMOVE_GOAL':
                response = update_kb(KnowledgeUpdateServiceRequest.REMOVE_GOAL,
                                     msg)
            else:
                rospy.logerr(
                    'Error : Unknown update_type, admisible values are ADD_KNOWLEDGE, ADD_GOAL, REMOVE_KNOWLEDGE and REMOVE_GOAL'
                )
                rospy.logwarn('Knowledge base not updated...!')
        except rospy.ServiceException, e:
            rospy.logerr('Service call failed: %s' % e)
            return False
Exemplo n.º 9
0
def distance(msg, params):

    distance = float('inf')
    ret_value = []

    # create KB update client
    rospy.wait_for_service('/strategic/rosplan_knowledge_base/update_array')
    rospy.wait_for_service(
        '/tactical_execution/rosplan_knowledge_base/update_array')
    rospy.wait_for_service(
        '/strategic/rosplan_interface_strategic_control/add_mission_goal')
    update_kb = rospy.ServiceProxy(
        '/strategic/rosplan_knowledge_base/update_array',
        KnowledgeUpdateServiceArray)
    update_kb_tactical = rospy.ServiceProxy(
        '/tactical_execution/rosplan_knowledge_base/update_array',
        KnowledgeUpdateServiceArray)
    update_strategic_goals = rospy.ServiceProxy(
        '/strategic/rosplan_interface_strategic_control/add_mission_goal',
        KnowledgeUpdateServiceArray)

    # add new ground_wp to the KB
    ground_wps = rospy.get_param("/ground_wp")
    try:
        pos = [float(msg.data[0]), float(msg.data[1]), 0.0, 0.0]
        for wp in ground_wps:

            # check if wp is the new one
            if close_point(pos, ground_wps[wp], 0.01):

                # upload new wp to KB
                kus = KnowledgeUpdateServiceArrayRequest()
                kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring()
                ki = KnowledgeItem()
                ki.instance_type = "ground"
                ki.instance_name = wp
                ki.knowledge_type = ki.INSTANCE
                kus.knowledge.append(ki)
                res = update_kb(kus)
                res = update_kb_tactical(kus)

                # Oberve added waypoint (if not original)
                if len(wp) >= 10 and int(wp[9:]) > 4:
                    kus = KnowledgeUpdateServiceArrayRequest()
                    kus.update_type += np.array(kus.ADD_GOAL).tostring()
                    ki = KnowledgeItem()
                    ki.knowledge_type = ki.FACT
                    ki.attribute_name = 'observed'
                    kv = KeyValue()
                    kv.key = 'wp'
                    kv.value = wp
                    ki.values.append(kv)
                    kus.knowledge.append(ki)
                    try:
                        res = update_strategic_goals(kus)
                    except:
                        print "Something Happened"

                # add new wp to the prm
                prm_nodes = rospy.get_param("/prm/wp")
                prm_edges = rospy.get_param("/prm/edge")

                rospy.set_param("/prm/wp/" + wp, ground_wps[wp])
                prm_nodes[wp] = ground_wps[wp]
                prm_edges[wp] = {}
                for node in prm_nodes:
                    if node == wp:
                        continue
                    d = sqrt((ground_wps[wp][0] - prm_nodes[node][0]) *
                             (ground_wps[wp][0] - prm_nodes[node][0]) +
                             (ground_wps[wp][1] - prm_nodes[node][1]) *
                             (ground_wps[wp][1] - prm_nodes[node][1]) +
                             (ground_wps[wp][2] - prm_nodes[node][2]) *
                             (ground_wps[wp][2] - prm_nodes[node][2]))
                    if d <= 4:
                        rospy.set_param("/prm/edge/" + wp + "/" + node, d)
                        rospy.set_param("/prm/edge/" + node + "/" + wp, d)
                        prm_edges[wp][node] = d
                        prm_edges[node][wp] = d

                # add new distances for each other wp
                kus = KnowledgeUpdateServiceArrayRequest()
                for other_wp in ground_wps:
                    if other_wp == wp:
                        continue

                    # don't check frame_id
                    if not type(ground_wps[other_wp]) is list:
                        continue

                    # run dijkstra
                    distance = dijkstra(wp, other_wp, prm_nodes, prm_edges)

                    if distance > 0:
                        # upload distances to KB
                        ki = KnowledgeItem()
                        ki.knowledge_type = ki.FUNCTION
                        ki.attribute_name = "distance"
                        kv = KeyValue()
                        kv.key = 'wp1'
                        kv.value = other_wp
                        ki.values.append(kv)
                        kv = KeyValue()
                        kv.key = 'wp2'
                        kv.value = wp
                        ki.values.append(kv)
                        ki.function_value = distance
                        kus.update_type += np.array(
                            kus.ADD_KNOWLEDGE).tostring()
                        kus.knowledge.append(ki)

                        ki = KnowledgeItem()
                        ki.knowledge_type = ki.FUNCTION
                        ki.attribute_name = "distance"
                        kv = KeyValue()
                        kv.key = 'wp1'
                        kv.value = wp
                        ki.values.append(kv)
                        kv = KeyValue()
                        kv.key = 'wp2'
                        kv.value = other_wp
                        ki.values.append(kv)
                        ki.function_value = distance
                        kus.update_type += np.array(
                            kus.ADD_KNOWLEDGE).tostring()
                        kus.knowledge.append(ki)
                res = update_kb(kus)
                res = update_kb_tactical(kus)

                # add new visibility between sky and ground
                sky_wps = rospy.get_param("/sky_wp")
                added_visibility = False

                kus = KnowledgeUpdateServiceArrayRequest()
                for sky_wp in sky_wps:
                    if check_visibility(ground_wps[wp], sky_wps[sky_wp]):

                        ki = KnowledgeItem()
                        ki.knowledge_type = ki.FACT
                        ki.attribute_name = "can_observe"
                        kv = KeyValue()
                        kv.key = 'wp1'
                        kv.value = sky_wp
                        ki.values.append(kv)
                        kv = KeyValue()
                        kv.key = 'wp2'
                        kv.value = wp
                        ki.values.append(kv)
                        kus.update_type += np.array(
                            kus.ADD_KNOWLEDGE).tostring()
                        kus.knowledge.append(ki)

                        added_visibility = True

                if added_visibility:
                    res = update_kb(kus)
                    res = update_kb_tactical(kus)

                # create new sky_wp if no visibility was created
                if not added_visibility:

                    new_sky_wp_name = "sky_above_" + wp

                    # upload new wp to KB
                    kus = KnowledgeUpdateServiceArrayRequest()
                    kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring()
                    ki = KnowledgeItem()
                    ki.instance_type = "sky"
                    ki.instance_name = new_sky_wp_name
                    ki.knowledge_type = ki.INSTANCE
                    kus.knowledge.append(ki)
                    res = update_kb(kus)
                    res = update_kb_tactical(kus)

                    # create new sky_wp and add to param server
                    new_sky_wp = list(ground_wps[wp])
                    new_sky_wp[2] = new_sky_wp[2] + 8
                    rospy.set_param("/sky_wp" + new_sky_wp_name, new_sky_wp)

                    # add visibility of other ground wps for the new sky wp
                    kus = KnowledgeUpdateServiceArrayRequest()
                    for gwp in ground_wps:

                        if check_visibility(ground_wps[gwp], new_sky_wp):

                            ki = KnowledgeItem()
                            ki.knowledge_type = ki.FACT
                            ki.attribute_name = "can_observe"
                            kv = KeyValue()
                            kv.key = 'wp1'
                            kv.value = new_sky_wp_name
                            ki.values.append(kv)
                            kv = KeyValue()
                            kv.key = 'wp2'
                            kv.value = wp
                            ki.values.append(kv)
                            kus.update_type += np.array(
                                kus.ADD_KNOWLEDGE).tostring()
                            kus.knowledge.append(ki)

                    if len(kus.knowledge) > 0:
                        res = update_kb(kus)
                        res = update_kb_tactical(kus)

                break
    except rospy.ServiceException, e:
        rospy.logerr(
            "KCL (%s) Failed to update knowledge base: %s" % rospy.get_name(),
            e.message)
Exemplo n.º 10
0
    def generate_knowledge_base(self):

        # Remove earlier entries from knowledge database
        self.clear_knowledge()

        empty_waypoints = []
        valid_occupants =  self.objects['drone'] + self.objects['turtlebot']
        for loc in self.objects['waypoint'] + self.objects['airwaypoint']:
            valid = False
            if loc in self.at:
                for o in self.at[loc]:
                    valid = True if (o in valid_occupants) else valid

            if not valid:
                empty_waypoints.append(loc)

       # Set up waypoint distances
        for wp1 in self.objects['waypoint']:
            for wp2 in self.objects['waypoint']:
                kitem = KnowledgeItem()
                kitem.knowledge_type = KnowledgeItem.FUNCTION
                kitem.attribute_name = "move-duration"
                kitem.values = [KeyValue('from', wp1), KeyValue('to', wp2)]
                kitem.function_value = self.waypoint_distance(wp1,wp2)
                self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)

        for wp1 in self.objects['airwaypoint']:
            for wp2 in self.objects['airwaypoint']:
                kitem = KnowledgeItem()
                kitem.knowledge_type = KnowledgeItem.FUNCTION
                kitem.attribute_name = "move-duration"
                kitem.values = [KeyValue('from', wp1), KeyValue('to', wp2)]
                kitem.function_value = self.waypoint_distance(wp1,wp2)
                self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)

        # Set empty for agents and waypoints
        for obj in self.objects['drone'] + self.objects['turtlebot'] + empty_waypoints:
            kitem = KnowledgeItem()
            kitem.knowledge_type = KnowledgeItem.FACT
            kitem.values = [KeyValue('aw_object', obj)]
            kitem.attribute_name = 'empty'
            self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)

        # Add agents
        for key, items in self.objects.iteritems():
            for item in items:
                kitem = KnowledgeItem()
                kitem.knowledge_type = KnowledgeItem.INSTANCE
                kitem.instance_type = key
                kitem.instance_name = item
                self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)

        # Add locations for agents and boxes
        for key, values in self.at.iteritems():
            for value in values:
                kitem = KnowledgeItem()
                kitem.knowledge_type = KnowledgeItem.FACT
                kitem.values = [KeyValue('object', value), KeyValue('waypoint', key)]
                kitem.attribute_name = 'at'
                self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
        
        # Add state airborne to drones at air waypoints
        for air_wp in self.objects['airwaypoint']:
            for drone in self.objects['drone']:
                if drone in self.at[air_wp]:
                    kitem = KnowledgeItem()
                    kitem.knowledge_type = KnowledgeItem.FACT
                    kitem.values = [KeyValue('drone', drone)]
                    kitem.attribute_name = 'airborne'
                    self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)

        # Set relation between air and ground waypoints
        for air, ground in zip(self.objects['airwaypoint'], self.objects['waypoint']):
            kitem = KnowledgeItem()
            kitem.knowledge_type = KnowledgeItem.FACT
            kitem.values = [KeyValue('airwaypoint', air), KeyValue('waypoint', ground)]
            kitem.attribute_name = 'over'
            self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)

        # Set rescued persons
        for person in self.rescued:
            kitem = KnowledgeItem()
            kitem.knowledge_type = KnowledgeItem.FACT
            kitem.values = [KeyValue('person', person)]
            kitem.attribute_name = 'handled'
            self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)

        # Set goals, each person should be handled
        for person in self.objects['person']:
            kitem = KnowledgeItem()
            kitem.knowledge_type = KnowledgeItem.FACT
            kitem.values = [KeyValue('person', person)]
            kitem.attribute_name = 'handled'
            self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_GOAL, knowledge=kitem)
Exemplo n.º 11
0
    def load_inventory_to_knowledge_base(self):

        if (None == self._inventory_message):
            return

        #Creating the message
        instance_msg = KnowledgeItem()


        for item in self._inventory_message.items:
            #Attribute on which the fact is written
            attribute = None
            object_instance_list = []

            #################################
            ## Uploading INSTANCE of Objects
            #################################
            #check if the instance_id is present
            if (0 != item.object.instance_id.data):
                #uploading the instance of object
                name = self.load_instance_message(KnowledgeItem.INSTANCE, 'object', item )
                object_instance_list.append(name)

            else:
                #if not then
                #check quantity and loop on quantity
                if (0 != item.quantity.data):
                    for count in range(item.quantity.data):
                        #uploading the instance of object
                        name = self.load_instance_message(KnowledgeItem.INSTANCE, 'object', item, count)
                        object_instance_list.append(name)
                else:
                    rospy.loginfo("Instance ID not provided as well as quantity ")
                    #uploading the instance of object without ID and quantity 
                    name = self.load_instance_message(KnowledgeItem.INSTANCE, 'object', item)
                    object_instance_list.append(name)
                    self.event_out.publish("e_failure")

            #Flag for identifying if location or container
            instance_type_identifyer = None

            #################################
            ## Uploading INSTANCE of locations
            #################################
            #check if location exist
            if (0 != item.location.type.data):
                name = self.load_instance_message(KnowledgeItem.INSTANCE, 'location', item)

                #fill fact "Object" ON "location"
                attribute = 'on'
                instance_type_identifyer = 'location'

            #################################
            ## Uploading INSTANCE of CONTAINER
            #################################
            elif (0 != item.container.type.data):
                name = self.load_instance_message(KnowledgeItem.INSTANCE, 'container', item)
                #upload fact "Object" IN "container"
                attribute = 'in'
                instance_type_identifyer = 'container'

            #################################
            ## Uploading FACTS
            #################################

            if (attribute):
                for uploaded_instance in object_instance_list:
                    fact_msg = KnowledgeItem()
                    fact_msg.knowledge_type = KnowledgeItem.FACT
                    fact_msg.instance_type = ''
                    fact_msg.instance_name = ''
                    fact_msg.attribute_name = attribute
                    fact_msg.function_value = 0.0
                    output_value = KeyValue()
                    output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
                    output_value.value = uploaded_instance
                    fact_msg.values.append(output_value)
                    output_value = KeyValue()
                    output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
                    output_value.value = self.msg_to_instance_name(item, \
                                                        instance_type_identifyer)
                    fact_msg.values.append(output_value)
                    self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_KNOWLEDGE'])

                attribute = None

            #Resetting the flag when the inventory contained some data
            rospy.loginfo('Received inventory data updated knowledge base')
            self.start_inventory_flag = False
            # Triggering the completion of the inventory update
            self.inventory_update_complete_flag = True

        #clearning inventory message
        self._inventory_message = None


        self.event_out.publish("e_done")