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 __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 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 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 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