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 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 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 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 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"))
