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