def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self.__last_request = {} self.people = set([]) self.predicate = rospy.get_param("~distance_predicate", "robot_distance") self.__update_srv_name = rospy.get_param( "~update_srv_name", "/kcl_rosplan/update_knowledge_base_array") self.__get_details_srv_name = rospy.get_param( "~get_details_srv_name", "/kcl_rosplan/get_domain_predicate_details") self.__get_instances_srv_name = rospy.get_param( "~get_instances_srv_name", "/kcl_rosplan/get_current_instances") with open(rospy.get_param("~config_file"), 'r') as f: config = yaml.load(f) rospy.loginfo("Inserting static instances.") self.update_knowledgebase( instances=self._create_instances(config["static_instances"])) self.subscribers = [] for inputs in config["inputs"]: rospy.loginfo("Subsribing to '%s'." % inputs["topic"]) self.__last_request[inputs["topic"]] = { 0: KnowledgeUpdateServiceArrayRequest(), 1: KnowledgeUpdateServiceArrayRequest() } self.subscribers.append( rospy.Subscriber(name=inputs["topic"], data_class=rostopic.get_topic_class( inputs["topic"], blocking=True)[0], callback=self.callback, callback_args=inputs)) rospy.loginfo("... done")
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_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 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 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 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 distance(msg, params): distance = float('inf') ret_value = [] # create KB update client rospy.wait_for_service('/strategic/rosplan_knowledge_base/update_array') rospy.wait_for_service( '/tactical_execution/rosplan_knowledge_base/update_array') rospy.wait_for_service( '/strategic/rosplan_interface_strategic_control/add_mission_goal') update_kb = rospy.ServiceProxy( '/strategic/rosplan_knowledge_base/update_array', KnowledgeUpdateServiceArray) update_kb_tactical = rospy.ServiceProxy( '/tactical_execution/rosplan_knowledge_base/update_array', KnowledgeUpdateServiceArray) update_strategic_goals = rospy.ServiceProxy( '/strategic/rosplan_interface_strategic_control/add_mission_goal', KnowledgeUpdateServiceArray) # add new ground_wp to the KB ground_wps = rospy.get_param("/ground_wp") try: pos = [float(msg.data[0]), float(msg.data[1]), 0.0, 0.0] for wp in ground_wps: # check if wp is the new one if close_point(pos, ground_wps[wp], 0.01): # upload new wp to KB kus = KnowledgeUpdateServiceArrayRequest() kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring() ki = KnowledgeItem() ki.instance_type = "ground" ki.instance_name = wp ki.knowledge_type = ki.INSTANCE kus.knowledge.append(ki) res = update_kb(kus) res = update_kb_tactical(kus) # Oberve added waypoint (if not original) if len(wp) >= 10 and int(wp[9:]) > 4: kus = KnowledgeUpdateServiceArrayRequest() kus.update_type += np.array(kus.ADD_GOAL).tostring() ki = KnowledgeItem() ki.knowledge_type = ki.FACT ki.attribute_name = 'observed' kv = KeyValue() kv.key = 'wp' kv.value = wp ki.values.append(kv) kus.knowledge.append(ki) try: res = update_strategic_goals(kus) except: print "Something Happened" # add new wp to the prm prm_nodes = rospy.get_param("/prm/wp") prm_edges = rospy.get_param("/prm/edge") rospy.set_param("/prm/wp/" + wp, ground_wps[wp]) prm_nodes[wp] = ground_wps[wp] prm_edges[wp] = {} for node in prm_nodes: if node == wp: continue d = sqrt((ground_wps[wp][0] - prm_nodes[node][0]) * (ground_wps[wp][0] - prm_nodes[node][0]) + (ground_wps[wp][1] - prm_nodes[node][1]) * (ground_wps[wp][1] - prm_nodes[node][1]) + (ground_wps[wp][2] - prm_nodes[node][2]) * (ground_wps[wp][2] - prm_nodes[node][2])) if d <= 4: rospy.set_param("/prm/edge/" + wp + "/" + node, d) rospy.set_param("/prm/edge/" + node + "/" + wp, d) prm_edges[wp][node] = d prm_edges[node][wp] = d # add new distances for each other wp kus = KnowledgeUpdateServiceArrayRequest() for other_wp in ground_wps: if other_wp == wp: continue # don't check frame_id if not type(ground_wps[other_wp]) is list: continue # run dijkstra distance = dijkstra(wp, other_wp, prm_nodes, prm_edges) if distance > 0: # upload distances to KB ki = KnowledgeItem() ki.knowledge_type = ki.FUNCTION ki.attribute_name = "distance" kv = KeyValue() kv.key = 'wp1' kv.value = other_wp ki.values.append(kv) kv = KeyValue() kv.key = 'wp2' kv.value = wp ki.values.append(kv) ki.function_value = distance kus.update_type += np.array( kus.ADD_KNOWLEDGE).tostring() kus.knowledge.append(ki) ki = KnowledgeItem() ki.knowledge_type = ki.FUNCTION ki.attribute_name = "distance" kv = KeyValue() kv.key = 'wp1' kv.value = wp ki.values.append(kv) kv = KeyValue() kv.key = 'wp2' kv.value = other_wp ki.values.append(kv) ki.function_value = distance kus.update_type += np.array( kus.ADD_KNOWLEDGE).tostring() kus.knowledge.append(ki) res = update_kb(kus) res = update_kb_tactical(kus) # add new visibility between sky and ground sky_wps = rospy.get_param("/sky_wp") added_visibility = False kus = KnowledgeUpdateServiceArrayRequest() for sky_wp in sky_wps: if check_visibility(ground_wps[wp], sky_wps[sky_wp]): ki = KnowledgeItem() ki.knowledge_type = ki.FACT ki.attribute_name = "can_observe" kv = KeyValue() kv.key = 'wp1' kv.value = sky_wp ki.values.append(kv) kv = KeyValue() kv.key = 'wp2' kv.value = wp ki.values.append(kv) kus.update_type += np.array( kus.ADD_KNOWLEDGE).tostring() kus.knowledge.append(ki) added_visibility = True if added_visibility: res = update_kb(kus) res = update_kb_tactical(kus) # create new sky_wp if no visibility was created if not added_visibility: new_sky_wp_name = "sky_above_" + wp # upload new wp to KB kus = KnowledgeUpdateServiceArrayRequest() kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring() ki = KnowledgeItem() ki.instance_type = "sky" ki.instance_name = new_sky_wp_name ki.knowledge_type = ki.INSTANCE kus.knowledge.append(ki) res = update_kb(kus) res = update_kb_tactical(kus) # create new sky_wp and add to param server new_sky_wp = list(ground_wps[wp]) new_sky_wp[2] = new_sky_wp[2] + 8 rospy.set_param("/sky_wp" + new_sky_wp_name, new_sky_wp) # add visibility of other ground wps for the new sky wp kus = KnowledgeUpdateServiceArrayRequest() for gwp in ground_wps: if check_visibility(ground_wps[gwp], new_sky_wp): ki = KnowledgeItem() ki.knowledge_type = ki.FACT ki.attribute_name = "can_observe" kv = KeyValue() kv.key = 'wp1' kv.value = new_sky_wp_name ki.values.append(kv) kv = KeyValue() kv.key = 'wp2' kv.value = wp ki.values.append(kv) kus.update_type += np.array( kus.ADD_KNOWLEDGE).tostring() kus.knowledge.append(ki) if len(kus.knowledge) > 0: res = update_kb(kus) res = update_kb_tactical(kus) break except rospy.ServiceException, e: rospy.logerr( "KCL (%s) Failed to update knowledge base: %s" % rospy.get_name(), e.message)
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)
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 # load and check KB for waypoints waypoints = [] if rospy.has_param(waypoint_namespace): waypoints = rospy.get_param(waypoint_namespace) if len(params[0]) != len(waypoints): del params[0][:] # waypoints not properly loaded yet, add to KB rospy.loginfo("KCL: (%s) Waypoints not loaded into KB yet, skipping visibility check" % rospy.get_name()) try: kus = KnowledgeUpdateServiceArrayRequest() for wp in waypoints: ki = KnowledgeItem() ki.instance_type = "waypoint" ki.instance_name = wp ki.knowledge_type = ki.INSTANCE kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring() kus.knowledge.append(ki) params[0].append(wp) 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 if skip_check: return ret_value