def load_params(self, predicate_name, predicate_info):
# Check if predicate
kb_info = None
try:
kb_info = self.get_predicates_srv.call(
GetDomainPredicateDetailsServiceRequest(
predicate_name)).predicate
except Exception as e:
kb_info = self.get_function_params(predicate_name)
if not kb_info:
rospy.logerr(
"KCL: (RosplanSensing) Could not find predicate or function %s"
% predicate_name)
return (False, False)
if predicate_name not in self.params: # Prepare dictionary
self.params[predicate_name] = {}
# Obtain all the instances for each parameter
kb_params = []
for p in kb_info.typed_parameters:
instances = self.get_instances_srv.call(
GetInstanceServiceRequest(p.value)).instances
kb_params.append(instances)
if 'params' in predicate_info:
params = predicate_info['params']
if len(kb_params) != len(params):
rospy.logerr(
"KCL: (RosplanSensing) Parameters defined for predicate %s don't match the knowledge base"
% predicate_name)
rospy.signal_shutdown('Wrong cfg file parameters definition')
return (False, True)
# Check params
wildcard = False
for i, p in enumerate(params):
if p != '*' and p != '_':
if p in kb_params[i]:
kb_params[i] = [p]
else:
rospy.logerr(
'KCL: (RosplanSensing) Unknown parameter instance "%s" of type "%s" for predicate "%s"',
p, kb_info.typed_parameters[i].value,
predicate_name)
rospy.signal_shutdown(
'Wrong cfg file parameters definition')
return (False, True)
else:
wildcard = True
# If the params are fully instantiated we store them as a list, else it will be a matrix with a list of
# instances per parameter
self.params[predicate_name][
predicate_info['sub_idx']] = kb_params if wildcard else params
return (True, True)
else:
self.params[predicate_name][predicate_info['sub_idx']] = kb_params
return (True, False)
def get_kb_instances(self, type_name):
request = GetInstanceServiceRequest(type_name)
try:
# call service
ret = self.get_instances_srv.call()
except rospy.ServiceException as exc:
rospy.logerr(
'KCL: ({}) error fetching instances from KB: {}'.format(
rospy.get_name(), str(exc)))
return None
return ret.instances