def service_update_fact_item(update_type, name, keys=[], values=[]): update_item = KnowledgeItem() update_item.knowledge_type = KnowledgeItem.FACT update_item.attribute_name = name assert (len(keys) == len(values)) for i in range(len(keys)): key_val = KeyValue(key=keys[i], value=values[i]) update_item.values.append(key_val) result = update_service(update_type, update_item) update_type_names = { KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE: 'ADD_KNOWLEDGE', KnowledgeUpdateServiceRequest.ADD_GOAL: 'ADD_GOAL', KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE: 'REMOVE_KNOWLEDGE', KnowledgeUpdateServiceRequest.REMOVE_GOAL: 'REMOVE_GOAL', KnowledgeUpdateServiceRequest.ADD_METRIC: 'ADD_METRIC', KnowledgeUpdateServiceRequest.REMOVE_METRIC: 'REMOVE_METRIC' } print 'result of {} for \'{}'.format(update_type_names[update_type], name), for value in values: print ' {}'.format(value), print '\'' print result
def _make_instance(type_name, item_name): kb_item = KnowledgeItem() kb_item.knowledge_type = KB_ITEM_INSTANCE kb_item.instance_name = item_name kb_item.instance_type = type_name return kb_item
def update_knowledgebase(self, key=None, predicates=None, instances=None, truth_value=1): """update the knowledge base. :param predicate: The condition as a string taken from the PNP. :param truth_value: (int) -1 (unknown), 0 (false), 1 (true) """ req = KnowledgeUpdateServiceArrayRequest() req.update_type = req.ADD_KNOWLEDGE if truth_value else req.REMOVE_KNOWLEDGE if instances != None: instances = [instances ] if not isinstance(instances, list) else instances for i in instances: rospy.logdebug("Updating %s %s" % (str(i), str(truth_value))) for n in i[1]: req.knowledge.append( KnowledgeItem(knowledge_type=KnowledgeItem.INSTANCE, instance_type=i[0], instance_name=n)) if predicates != None: predicates = [predicates ] if not isinstance(predicates, list) else predicates for p in predicates: rospy.logdebug("Updating %s %s" % (str(p), str(truth_value))) if not p[0] in self.__tp: self.__tp[p[0]] = self._get_predicate_details( p[0]).predicate.typed_parameters if len(self.__tp[p[0]]) != len(p[1]): rospy.logerr( "Fact '%s' should have %s parameters but has only %s as parsed from: '%s'" % (p[0], len(self.__tp[p[0]]), len(p[1]))) return req.knowledge.append( KnowledgeItem(knowledge_type=KnowledgeItem.FACT, attribute_name=p[0], values=[ KeyValue(key=str(k.key), value=str(v)) for k, v in zip(self.__tp[p[0]], p[1]) ])) if req.knowledge: try: if not self.__compare_knowledge_items( self.__last_request[key][truth_value].knowledge, req.knowledge): self.__call_service(self.__update_srv_name, KnowledgeUpdateServiceArray, req) self.__last_request[key][truth_value] = req except KeyError: self.__call_service(self.__update_srv_name, KnowledgeUpdateServiceArray, req)
def changeKMSFact(self, name, paramsValues, changeType): predicateDetails = self.predicate_details_client.call(name) knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.FACT knowledge.attribute_name = name for param, paramValue in zip(predicateDetails.predicate.typed_parameters, paramsValues): pair = KeyValue() pair.key = param.key pair.value = paramValue knowledge.values.append(pair) update_response = self.update_knowledge_client(changeType, knowledge)
def changeKMSFact(self, name, paramsValues, changeType): predicateDetails = self.predicate_details_client.call(name) knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.FACT knowledge.attribute_name = name for param, paramValue in zip( predicateDetails.predicate.typed_parameters, paramsValues): pair = KeyValue() pair.key = param.key pair.value = paramValue knowledge.values.append(pair) update_response = self.update_knowledge_client(changeType, knowledge)
def publish_instances(self, object_instances): """publish instances. input is dictionary where name is key and type is value""" print "adding: ", object_instances ka = [] for obj_name in object_instances: k = KnowledgeItem() k.knowledge_type = k.INSTANCE k.instance_name = obj_name.lower() k.instance_type = object_instances[obj_name].lower() print k ka.append(k) self.publish(ka)
def changeKMSFact(self, name, params, changeType): knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.FACT knowledge.attribute_name = name for param in params: pair = KeyValue() pair.key = param[0] pair.value = param[1] knowledge.values.append(pair) update_response = self.update_knowledge_client(changeType, knowledge) if (update_response.success is not True): rospy.loginfo("Failed updating KMS with attribute: %s", knowledge.attribute_name) else: rospy.loginfo("Updated KMS with attribute: %s", knowledge.attribute_name)
def publish_predicates(self, preds): ka = [] for p in preds: k = KnowledgeItem() k.knowledge_type = k.FACT k.attribute_name = p.head.lower() for i in xrange(len(p.args)): kv = KeyValue() kv.key = self.predicate_key_name[(k.attribute_name, i)] kv.value = p.args[i].lower() k.values.append(kv) ka.append(k) k.is_negative = not p.isTrue self.publish(ka)
def add_instance(instance_type, instance_name): instance_item = KnowledgeItem() instance_item.knowledge_type = KnowledgeItem.INSTANCE instance_item.instance_type = instance_type instance_item.instance_name = instance_name result = update_service(KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE, instance_item) print 'result of adding instance \'{}\' of type \'{}\''.format( instance_name, instance_type) print result return result.success
def addFact(name, params, client): add_knowledge_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.FACT knowledge.attribute_name = name for param in params: pair = KeyValue() pair.key = param[0] pair.value = param[1] knowledge.values.append(pair) update_response = client(add_knowledge_type, knowledge) if (update_response.success is not True): rospy.loginfo("Failed updating KMS with attribute: %s", knowledge.attribute_name) else: rospy.loginfo("Updated KMS with attribute: %s", knowledge.attribute_name)
def rm_instance(type_name, item_name): # db[type_name].remove({'name': item_name}) return services['update_knowledge_base'](KB_UPDATE_RM_KNOWLEDGE, KnowledgeItem(KB_ITEM_INSTANCE, type_name, item_name, "", [], 0.0, False))
def load_order_to_knowledge_base(self): rospy.loginfo("Loading task info to knowledge base") #Creating the message instance_msg = KnowledgeItem() for task in self._tasks.tasks: #################################################################### # Transportation task #################################################################### if atwork_ros_msgs.msg.Task.TRANSPORTATION == task.type.data: # The task is transportation # find the object, its destination if (0 != task.transportation_task.container.type_id.data): #container is available self.fill_order(task.transportation_task, 'in', 'object', 'container') elif (0 != task.transportation_task.destination.instance_id.data): #container not available but destination available self.fill_order(task.transportation_task, 'on', 'object', 'destination') else: #both container and destination not available rospy.logerr("No Goal set") if (0 != task.transportation_task.container.type_id.data) and \ (0 != task.transportation_task.destination.instance_id.data): #Both container and destination available self.fill_order(task.transportation_task, 'on', 'container', 'destination' ) rospy.loginfo("Task info upload completed")
def add_goal(self, goal): srv_name = "/kcl_rosplan/update_knowledge_base_array" req = KnowledgeUpdateServiceArrayRequest() req.update_type = req.ADD_GOAL goal = [goal] if not isinstance(goal, list) else goal for p in goal: cond = p.split("__") rospy.loginfo("Adding %s" % str(p)) tp = self._get_predicate_details( cond[0]).predicate.typed_parameters if len(tp) != len(cond[1:]): rospy.logerr( "Fact '%s' should have %s parameters but has only %s as parsed from: '%s'" % (cond[0], len(tp), len(cond[1:]))) return req.knowledge.append( KnowledgeItem(knowledge_type=KnowledgeItem.FACT, attribute_name=cond[0], values=[ KeyValue(key=str(k.key), value=str(v)) for k, v in zip(tp, cond[1:]) ])) while not rospy.is_shutdown(): try: self.__call_service(srv_name, KnowledgeUpdateServiceArray, req) except rospy.ROSInterruptException: rospy.logerr("Communication with '%s' interrupted. Retrying." % srv_name) rospy.sleep(1.) else: return
def ground_simple_effect(self, effect, bound_parameters): # TODO fetch predicate labels # bind objects from action message to effect predicate item = KnowledgeItem() item.knowledge_type = KnowledgeItem.FACT item.attribute_name = effect.name for j in range(0, len(effect.typed_parameters)): # set key (parameter label in predicate) and value (object bound to operator) pair = KeyValue() pair.key = "TODO THE LABEL" # self.predicates[self.op.at_start_del_effects[i].name].typed_parameters[j].key pair.value = bound_parameters[effect.typed_parameters[j].key] item.values.append(pair) return item
def query_knowledgbase(self, predicate): """querry the knowledge base. :param predicate: The condition as a string taken from the PNP. :return (int) -1 (unknown), 0 (false), or 1 (true) """ cond = predicate.split("__") srv_name = "/kcl_rosplan/query_knowledge_base" tp = self._get_predicate_details(cond[0]).predicate.typed_parameters if len(tp) != len(cond[1:]): rospy.logerr( "Fact '%s' should have %s parameters but has only %s as parsed from: '%s'" % (cond[0], len(tp), len(cond[1:]))) return req = KnowledgeQueryServiceRequest() req.knowledge.append( KnowledgeItem(knowledge_type=KnowledgeItem.FACT, attribute_name=cond[0], values=[ KeyValue(key=str(k.key), value=str(v)) for k, v in zip(tp, cond[1:]) ])) while not rospy.is_shutdown(): try: r = self.__call_service(srv_name, KnowledgeQueryService, req) except rospy.ROSInterruptException: rospy.logerr("Communication with '%s' interrupted. Retrying." % srv_name) rospy.sleep(1.) else: return 1 if r.all_true else 0
def gen_predicate(type_name, **kwargs): new_type_name, is_negative = is_predicate_negative(type_name) return KnowledgeItem(KB_ITEM_FACT, "", "", new_type_name, dict_to_keyval(kwargs), 0.0, is_negative)
def load_order_to_knowledge_base(self): rospy.loginfo("Loading task info to knowledge base") #Creating the message instance_msg = KnowledgeItem() for task in self._tasks.tasks: #################################################################### # Transportation task #################################################################### if atwork_ros_msgs.msg.Task.TRANSPORTATION == task.type.data: # The task is transportation # find the object, its destination if (0 != task.transportation_task.container.type_id.data): #container is available #some order contain container in order but not in inventory, so need to add fact explicitly name = self.load_instance_message('destination', task.transportation_task) container_name = self.load_instance_message('container', task.transportation_task) if (container_name): #same the knowledge about the initial location of the container is missin, adding it self.load_fact_message('on', container_name, task.transportation_task, 'destination') self.load_fact_message('container', container_name, task.transportation_task, 'destination') self.load_fact_message('heavy', container_name, task.transportation_task, 'destination') object_name = self.fill_order(task.transportation_task, 'in', 'object', 'container', container_name=container_name) if object_name: self.load_fact_message('insertable', object_name, task.transportation_task, 'destination') elif (0 != task.transportation_task.destination.instance_id.data): #if (0 != task.transportation_task.destination.instance_id.data): //Add when 'in' domain is not present #In transportationtask sometimes the destination is not there in inventory #Needs to add fact about the inventory name = self.load_instance_message('destination', task.transportation_task) #container not available but destination available insertable_object_name = self.fill_order(task.transportation_task, 'on', 'object', 'destination') ## IF PP01 then we need to load some facts if 5 == task.transportation_task.destination.type.data : self.load_fact_message('on', 'PP01_CAVITY', task.transportation_task, 'destination') self.load_fact_message('container', 'PP01_CAVITY', task.transportation_task, 'destination') self.load_fact_message('heavy', 'PP01_CAVITY', task.transportation_task, 'destination') if(insertable_object_name): self.load_fact_message('insertable', insertable_object_name, task.transportation_task, 'destination') self.load_instance_message_PP01('object') else: #both container and destination not available rospy.logerr("No Goal set") if (0 != task.transportation_task.container.type_id.data) and \ (0 != task.transportation_task.destination.instance_id.data): ##Both container and destination available ###ERL2016 the containers need not moved so we dont need to add goal for the containers ###Add the below line when the container needs to be transported #self.fill_order(task.transportation_task, 'on', 'container', 'destination' ) ###The Fact about the initial location of the container is missing pass rospy.loginfo("Task info upload completed")
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'])
def createInstances(): update_knowledge_client = rospy.ServiceProxy("/kcl_rosplan/update_knowledge_base", KnowledgeUpdateService) add_knowledge_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.INSTANCE knowledge.instance_type = "obj" instance_list = ["door1","zone1","door_zone_1","door1_button","room_2","zone2","door_zone_2","room_4"] #"E1","zone1","E1_entrance_f1","E1_keypad_f1","room_2","room_4", \ #"coke_stand","zone2","E1_entrance_f2","E1_keypad_f2","room3","room4"] for instance in instance_list: knowledge.instance_name = instance update_response = update_knowledge_client(add_knowledge_type, knowledge) if (update_response.success is not True): rospy.loginfo("Failed updating KMS with instance: %s", knowledge.instance_name) else: rospy.loginfo("Updated KMS with instance: %s", knowledge.instance_name)
def remove_instance(item_name, type_name): if not isinstance(item_name, str) or not isinstance(type_name, str): raise TypeError return _services["update_knowledge_base"]( KB_UPDATE_RM_KNOWLEDGE, KnowledgeItem(KB_ITEM_INSTANCE, type_name, item_name, "", [], 0.0, False)).success
def doughnut_visible_from(msg, params): global _static_map rospy.wait_for_service('/rosplan_knowledge_base/update_array') update_kb = rospy.ServiceProxy('/rosplan_knowledge_base/update_array', KnowledgeUpdateServiceArray) # Wait for map sub_once = rospy.Subscriber("map", OccupancyGrid, set_map) while not rospy.is_shutdown() and not _static_map: rospy.loginfo("KCL: (%s) Static map not received, waiting..." % rospy.get_name()) rospy.sleep(0.5) sub_once.unregister() assert(len(params) == 2) ret_value = [] skip_check = False waypoint_namespace = "/task_planning_waypoints" if rospy.has_param('~waypoint_namespace'): waypoint_namespace = rospy.get_param('~waypoint_namespace') # load and check KB for objects doughnuts = [] if rospy.has_param('~doughnuts'): doughnuts = rospy.get_param('~doughnuts') if len(params[1]) != len(doughnuts): del params[1][:] # objects not properly loaded yet, add to KB rospy.loginfo("KCL: (%s) Objects not loaded into KB yet, skipping visibility check" % rospy.get_name()) try: kus = KnowledgeUpdateServiceArrayRequest() for d in doughnuts: ki = KnowledgeItem() ki.instance_type = d['type'] ki.instance_name = d['name'] ki.knowledge_type = ki.INSTANCE kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring() kus.knowledge.append(ki) params[1].append(d['name']) res = update_kb(kus) except rospy.ServiceException, e: rospy.logerr("KCL (%s) Failed to update knowledge base: %s" % rospy.get_name(), e.message) skip_check = True
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_initial_state_new_assumptions(self): self.clear_knowledge_client.call() for instance in self.initial_instances.instances: knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.INSTANCE knowledge.instance_type = "object" knowledge.instance_name = instance self.update_knowledge_client(KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE, knowledge) for goal in self.initial_goal.attributes: self.update_knowledge_client(KnowledgeUpdateServiceRequest.ADD_GOAL, goal) for fact in self.initial_facts.attributes: self.update_knowledge_client(KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE, fact) rospy.loginfo("Initial state loaded successfuly. Now loading assumptions:") for i in range(0,len(self.current_assumptions)): self.update_assumption(i,self.current_assumptions[i])
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)
def add_instance(type_name, item_name, value=None): if value is not None: db.insert_named('%s__%s' % (type_name, item_name), value) types[type_name] = value.__class__._type # print value.__class__._type return services['update_knowledge_base'](KB_UPDATE_ADD_KNOWLEDGE, KnowledgeItem(KB_ITEM_INSTANCE, type_name, item_name, "", [], 0.0, False))
def _removeEdgefromKnowledgeBase(self, first, second, ret_knowledge_only=False): knowledge = [ KnowledgeItem( knowledge_type=KnowledgeItem.FACT, # first -> second attribute_name="connected", values=[ KeyValue(key="from", value=first), KeyValue(key="to", value=second) ]), KnowledgeItem( knowledge_type=KnowledgeItem.FACT, # second -> first attribute_name="connected", values=[ KeyValue(key="from", value=second), KeyValue(key="to", value=first) ]), KnowledgeItem( knowledge_type=KnowledgeItem.FUNCTION, # second -> first attribute_name="distance", values=[ KeyValue(key="from", value=first), KeyValue(key="to", value=second) ], function_value=0), KnowledgeItem( knowledge_type=KnowledgeItem.FUNCTION, # second -> first attribute_name="distance", values=[ KeyValue(key="from", value=second), KeyValue(key="to", value=first) ], function_value=0) ] if ret_knowledge_only: return knowledge else: return self.knowledge_service(update_type=self.knowledge_service. request_class.REMOVE_KNOWLEDGE, knowledge=knowledge)
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
def createGoal(): rosplan_pub = rospy.Publisher("/kcl_rosplan/planning_commands", String, queue_size=10) speech_pub = rospy.Publisher("/KomodoSpeech/rec_command", KomodoSpeechRecCommand, queue_size=10) command = KomodoSpeechRecCommand() command.header = Header() command.cmd = 'start' command.cat = 'cmd' rospy.sleep(2.) speech_pub.publish(command) result = rospy.wait_for_message("/KomodoSpeech/rec_result", KomodoSpeechRecResult, timeout=30) if result and result.cat == "cmd" and result.success is True and result.phrase_id == 8: attribute_query_client = rospy.ServiceProxy("/kcl_rosplan/get_current_knowledge", GetAttributeService) knowledge_items = attribute_query_client.call("robot_at") if len(knowledge_items.attributes) > 1: rospy.loginfo("Failed to add goal. Robot in two places (robot_at)") return current_location = knowledge_items.attributes[0].values[0].value update_knowledge_client = rospy.ServiceProxy("/kcl_rosplan/update_knowledge_base", KnowledgeUpdateService) add_knowledge_type = KnowledgeUpdateServiceRequest.ADD_GOAL knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.FACT knowledge.attribute_name = "coke_at" pair = KeyValue() pair.key = "loc" pair.value = current_location #room_2 knowledge.values.append(pair) update_response = update_knowledge_client(add_knowledge_type, knowledge) if (update_response.success is not True): rospy.loginfo("Failed to add goal of attribute: %s", knowledge.attribute_name) else: rospy.loginfo("Added goal of attribute: %s", knowledge.attribute_name) rosplan_pub.publish(String("plan")) else: rospy.loginfo("Couldn't get command. Closing")
def _addEdgeToKnowledgeBase(self, first, second): firstpos = self.server.get(first).pose.position secondpos = self.server.get(second).pose.position dist = euclidean_distance(firstpos, secondpos) return self.knowledge_service( update_type=self.knowledge_service.request_class.ADD_KNOWLEDGE, knowledge=[ KnowledgeItem( knowledge_type=KnowledgeItem.FACT, # first -> second attribute_name="connected", values=[ KeyValue(key="from", value=first), KeyValue(key="to", value=second) ]), KnowledgeItem( knowledge_type=KnowledgeItem.FACT, # second -> first attribute_name="connected", values=[ KeyValue(key="from", value=second), KeyValue(key="to", value=first) ]), KnowledgeItem( knowledge_type=KnowledgeItem.FUNCTION, # second -> first attribute_name="distance", values=[ KeyValue(key="from", value=first), KeyValue(key="to", value=second) ], function_value=dist), KnowledgeItem( knowledge_type=KnowledgeItem.FUNCTION, # second -> first attribute_name="distance", values=[ KeyValue(key="from", value=second), KeyValue(key="to", value=first) ], function_value=dist) ])
def load_initial_state_new_assumptions(self): self.clear_knowledge_client.call() for instance in self.initial_instances.instances: knowledge = KnowledgeItem() knowledge.knowledge_type = KnowledgeItem.INSTANCE knowledge.instance_type = "object" knowledge.instance_name = instance self.update_knowledge_client( KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE, knowledge) for goal in self.initial_goal.attributes: self.update_knowledge_client( KnowledgeUpdateServiceRequest.ADD_GOAL, goal) for fact in self.initial_facts.attributes: self.update_knowledge_client( KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE, fact) rospy.loginfo( "Initial state loaded successfuly. Now loading assumptions:") for i in range(0, len(self.current_assumptions)): self.update_assumption(i, self.current_assumptions[i])
def _make_predicate(type_name, parameters): new_type_name, is_negative = _is_predicate_negative(type_name) kb_item = KnowledgeItem() kb_item.knowledge_type = KB_ITEM_FACT kb_item.is_negative = is_negative kb_item.attribute_name = new_type_name kb_item.values = dict_to_keyval(parameters) return kb_item
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
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
def get_fact_knowledge_item_list(facts): """ Create a list of KnowledgeItem from facts :param facts: facts :type facts: list of tuple ( each tuple is (str, [(str, str), ...]) ) :rtype: list of rosplan_knowledge_msgs.msg.KnowledgeItem """ ki_list = [] for attr_name, kv_list in facts: ki_list.append( KnowledgeItem( knowledge_type=KnowledgeItem.FACT, attribute_name=attr_name, values=[KeyValue(k, v.upper()) for k, v in kv_list], )) return ki_list
def get_instance_knowledge_item_list(instances): """ Create a list of KnowledgeItem from instances :param instances: instances :type instances: dict {str: [str, str, ...], ...} :rtype: list of rosplan_knowledge_msgs.KnowledgeItem """ ki_list = [] for instance_type, instance_names in instances.iteritems(): for instance_name in instance_names: ki_list.append( KnowledgeItem( knowledge_type=KnowledgeItem.INSTANCE, instance_type=instance_type, instance_name=instance_name.upper(), )) return ki_list
def apply_effects_to_KMS(self, block_name, other_block_name, action_name): fname = "{}::{}".format(self.__class__.__name__, self.apply_effects_to_KMS.__name__) emptyhand_update_type = KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE if \ (action_name == "pick_up") else KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE not_emptyhand_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \ (action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE onblock_update_type = KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE if \ (action_name == "pick_up") else KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE clear_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \ (action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE inhand_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \ (action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE # not_emptyhand predicate updated_knowledge = KnowledgeItem() updated_knowledge.knowledge_type = KnowledgeItem.FACT updated_knowledge.attribute_name = "not_emptyhand" update_response = self.update_knowledge_client(not_emptyhand_update_type, updated_knowledge) rospy.logdebug( "{}: Updated KMS with {} {}".format(fname, updated_knowledge.attribute_name, not_emptyhand_update_type)) if (update_response.success is not True): rospy.logerr("{}: Could not update KMS with action effect ({} {})". format(fname, not_emptyhand_update_type, "not_emptyhand")) # emptyhand predicate updated_knowledge = KnowledgeItem() updated_knowledge.knowledge_type = KnowledgeItem.FACT updated_knowledge.attribute_name = "emptyhand" update_response = self.update_knowledge_client(emptyhand_update_type, updated_knowledge) rospy.logdebug( "{}: Updated KMS with {} {}".format(fname, updated_knowledge.attribute_name, emptyhand_update_type)) if (update_response.success is not True): rospy.logerr("{}: Could not update KMS with action effect ({} {})". format(fname, emptyhand_update_type, "emptyhand")) # (on ?block ?from_block) predicate updated_knowledge = KnowledgeItem() updated_knowledge.knowledge_type = KnowledgeItem.FACT updated_knowledge.attribute_name = "on" pair = KeyValue() pair.key = "block" pair.value = block_name updated_knowledge.values.append(pair) pair = KeyValue() pair.key = "on_block" pair.value = other_block_name updated_knowledge.values.append(pair) update_response = self.update_knowledge_client(onblock_update_type, updated_knowledge) rospy.logdebug("{}: Updated KMS with {} ({}, {}) {}". format(fname, updated_knowledge.attribute_name, updated_knowledge.values[0].value, updated_knowledge.values[1].value, onblock_update_type)) if (update_response.success is not True): rospy.logerr("{}: Could not update KMS with action effect ({} {})". format(fname, onblock_update_type, "on")) # (clear ?from_block) predicate updated_knowledge = KnowledgeItem() updated_knowledge.knowledge_type = KnowledgeItem.FACT updated_knowledge.attribute_name = "clear" pair = KeyValue() pair.key = "block" pair.value = other_block_name updated_knowledge.values.append(pair) update_response = self.update_knowledge_client(clear_update_type, updated_knowledge) rospy.logdebug("{}: Updated KMS with {} ({}) {}". format(fname, updated_knowledge.attribute_name, updated_knowledge.values[0].value, clear_update_type)) if (update_response.success is not True): rospy.logerr("{}: Could not update KMS with action effect ({} {})". format(fname, clear_update_type, "clear")) # (inhand ?block) predicate updated_knowledge = KnowledgeItem() updated_knowledge.knowledge_type = KnowledgeItem.FACT updated_knowledge.attribute_name = "inhand" pair = KeyValue() pair.key = "block" pair.value = block_name updated_knowledge.values.append(pair) update_response = self.update_knowledge_client(inhand_update_type, updated_knowledge) rospy.logdebug("{}: Updated KMS with {} ({}) {}". format(fname, updated_knowledge.attribute_name, updated_knowledge.values[0].value, inhand_update_type)) if (update_response.success is not True): rospy.logerr("{}: Could not update KMS with action effect ({} {})". format(fname, inhand_update_type, "inhand"))