def update_knowledgebase(self, predicate, truth_value):
"""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)
"""
srv_name = "/kcl_rosplan/update_knowledge_base_array"
req = KnowledgeUpdateServiceArrayRequest()
req.update_type = req.ADD_KNOWLEDGE if truth_value else req.REMOVE_KNOWLEDGE
predicate = [predicate] if not isinstance(predicate,list) else predicate
for p in predicate:
cond = p.split("__")
rospy.loginfo("Updating %s %s" % (str(p), str(truth_value)))
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 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 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 kb_apply_action_effects(self, pddl_action_msg, effect_time):
# request object for update
kus = KnowledgeUpdateServiceArrayRequest()
# fetch operator details
operator_name = pddl_action_msg.name.split(" ")[0]
operator_details = self.get_kb_operator_details(operator_name)
if not operator_details:
rospy.logerr(
'KCL: ({}) error fetching operator details from KB: {}'.format(
rospy.get_name(), operator_name))
return
# set up operator parameters for use in effects
bound_parameters = {}
for op_parameter in operator_details.formula.typed_parameters:
found = False
for i in range(0, len(pddl_action_msg.parameters)):
if op_parameter.key == pddl_action_msg.parameters[i].key:
bound_parameters[pddl_action_msg.parameters[i].
key] = pddl_action_msg.parameters[i].value
found = True
break
if not found:
rospy.logerr(
'KCL: ({}) aborting applying action effect due to missing parameter, missing {}'
.format(rospy.get_name(), op_parameter.key))
return
kus.knowledge = []
kus.update_type = []
if effect_time == self.AT_START:
# At start add effects
for eff in operator_details.at_start_add_effects:
self.pack_simple_effect(eff,
bound_parameters,
kus,
add_effect=True)
# At start del effects
for eff in operator_details.at_start_del_effects:
self.pack_simple_effect(eff,
bound_parameters,
kus,
add_effect=False)
if effect_time == self.AT_END:
# At end add effects
for eff in operator_details.at_end_add_effects:
self.pack_simple_effect(eff,
bound_parameters,
kus,
add_effect=True)
# At end del effects
for eff in operator_details.at_end_del_effects:
self.pack_simple_effect(eff,
bound_parameters,
kus,
add_effect=False)
self.update_kb(kus)
