def fill_order(self, order, attribute, first, second, count=1, container_name=None):
fact_msg = KnowledgeItem()
fact_msg.knowledge_type = KnowledgeItem.FACT
fact_msg.instance_type = ""
fact_msg.instance_name = ""
fact_msg.attribute_name = attribute
fact_msg.function_value = 0.0
if 5 == order.destination.type.data:
fact_msg.attribute_name = "in"
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[
0
]
name_of_object = self.msg_to_instance_name(order, first, count)
output_value.value = name_of_object
##Robocup finals hack not adding goals for object to be picked from rotation table
if output_value.value in self.object_to_delete_goals:
print "Not adding goal for ", output_value.value
return
# if 5 == order.object.type.data or 12 == order.object.type.data:
# print "Not adding M20_100 and distance tube goal"
# return
fact_msg.values.append(output_value)
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[
1
]
if not container_name:
# in order the container or location doesnt have the count
output_value.value = self.msg_to_instance_name(order, second)
else:
output_value.value = container_name
# if 'SH01' == order.destination.type.data or 'SH02' == order.destination.type.data:
# print "not adding goal for shelfes"
# return
## For PP01 adding insert
if 5 == order.destination.type.data:
# Hack for PPT for Nagoya
#################
# print "Not adding Goals for PP01"
# return
#################
output_value.value = "PP01_CAVITY"
fact_msg.values.append(output_value)
self.write_to_knowledge_base(
fact_msg, self.knowledge_update_service["ADD_GOAL"]
)
return name_of_object
def load_fact_message(self, attribute, uploaded_instance, item, domain):
fact_msg = KnowledgeItem()
fact_msg.knowledge_type = KnowledgeItem.FACT
fact_msg.instance_type = ''
fact_msg.instance_name = ''
fact_msg.attribute_name = attribute
fact_msg.function_value = 0.0
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
output_value.value = uploaded_instance
fact_msg.values.append(output_value)
#write to knowledge base
## For insert domain, the attribute container needs to be added as a fact, without second term
if (('in' == attribute) or ('on' == attribute)):
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
output_value.value = self.msg_to_instance_name(item, domain)
fact_msg.values.append(output_value)
##Robocup hack for finals
## Removing instance of objects to be picked form rotation table CB02
if "CB02" == output_value.value or "CB01" == output_value.value:
print "Rotating table knowledge base not updateing for object",uploaded_instance
self.object_to_delete_goals.append(uploaded_instance)
return
if "SH01" == output_value.value or "SH02" == output_value.value:
print "Removing object tot pick from shelf ,", uploaded_instance
self.object_to_delete_goals.append(uploaded_instance)
return
#print (fact_msg)
self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_KNOWLEDGE'])
def service_update_fact_item(update_type, name, keys=[], values=[]):
update_item = KnowledgeItem()
update_item.knowledge_type = KnowledgeItem.FACT
update_item.attribute_name = name
assert (len(keys) == len(values))
for i in range(len(keys)):
key_val = KeyValue(key=keys[i], value=values[i])
update_item.values.append(key_val)
result = update_service(update_type, update_item)
update_type_names = {
KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE: 'ADD_KNOWLEDGE',
KnowledgeUpdateServiceRequest.ADD_GOAL: 'ADD_GOAL',
KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE: 'REMOVE_KNOWLEDGE',
KnowledgeUpdateServiceRequest.REMOVE_GOAL: 'REMOVE_GOAL',
KnowledgeUpdateServiceRequest.ADD_METRIC: 'ADD_METRIC',
KnowledgeUpdateServiceRequest.REMOVE_METRIC: 'REMOVE_METRIC'
}
print 'result of {} for \'{}'.format(update_type_names[update_type], name),
for value in values:
print ' {}'.format(value),
print '\''
print result
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 _make_predicate(type_name, parameters):
new_type_name, is_negative = _is_predicate_negative(type_name)
kb_item = KnowledgeItem()
kb_item.knowledge_type = KB_ITEM_FACT
kb_item.is_negative = is_negative
kb_item.attribute_name = new_type_name
kb_item.values = dict_to_keyval(parameters)
return kb_item
def changeKMSFact(self, name, paramsValues, changeType):
predicateDetails = self.predicate_details_client.call(name)
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param, paramValue in zip(predicateDetails.predicate.typed_parameters, paramsValues):
pair = KeyValue()
pair.key = param.key
pair.value = paramValue
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
def changeKMSFact(self, name, paramsValues, changeType):
predicateDetails = self.predicate_details_client.call(name)
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param, paramValue in zip(
predicateDetails.predicate.typed_parameters, paramsValues):
pair = KeyValue()
pair.key = param.key
pair.value = paramValue
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
def load_instance_message(self, domain, item, count=1 ):
#Creating the message
instance_msg = KnowledgeItem()
#uploading the instance of object
instance_msg.knowledge_type = KnowledgeItem.INSTANCE
instance_msg.instance_type = self.domain_name[domain]
instance_msg.instance_name = self.msg_to_instance_name(item, domain, count)
instance_msg.attribute_name = ''
instance_msg.function_value = 0.0
self.write_to_knowledge_base(instance_msg, self.knowledge_update_service['ADD_KNOWLEDGE'])
return instance_msg.instance_name
def publish_predicates(self, preds):
ka = []
for p in preds:
k = KnowledgeItem()
k.knowledge_type = k.FACT
k.attribute_name = p.head.lower()
for i in xrange(len(p.args)):
kv = KeyValue()
kv.key = self.predicate_key_name[(k.attribute_name, i)]
kv.value = p.args[i].lower()
k.values.append(kv)
ka.append(k)
k.is_negative = not p.isTrue
self.publish(ka)
def changeKMSFact(self, name, params, changeType):
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param in params:
pair = KeyValue()
pair.key = param[0]
pair.value = param[1]
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed updating KMS with attribute: %s", knowledge.attribute_name)
else:
rospy.loginfo("Updated KMS with attribute: %s", knowledge.attribute_name)
def publish_predicates(self, preds): ka = [] for p in preds: k = KnowledgeItem() k.knowledge_type = k.FACT k.attribute_name = p.head.lower() for i in xrange(len(p.args)): kv = KeyValue() kv.key = self.predicate_key_name[(k.attribute_name, i)] kv.value = p.args[i].lower() k.values.append(kv) ka.append(k) k.is_negative = not p.isTrue self.publish(ka)
def load_instance_message_PP01(self, domain):
# Creating the message
instance_msg = KnowledgeItem()
# uploading the instance of object
instance_msg.knowledge_type = KnowledgeItem.INSTANCE
instance_msg.instance_type = self.domain_name[domain]
instance_msg.instance_name = "PP01_CAVITY"
instance_msg.attribute_name = ""
instance_msg.function_value = 0.0
self.write_to_knowledge_base(
instance_msg, self.knowledge_update_service["ADD_KNOWLEDGE"]
)
return instance_msg.instance_name
def changeKMSFact(self, name, params, changeType):
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param in params:
pair = KeyValue()
pair.key = param[0]
pair.value = param[1]
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed updating KMS with attribute: %s",
knowledge.attribute_name)
else:
rospy.loginfo("Updated KMS with attribute: %s",
knowledge.attribute_name)
def addFact(name, params, client):
add_knowledge_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param in params:
pair = KeyValue()
pair.key = param[0]
pair.value = param[1]
knowledge.values.append(pair)
update_response = client(add_knowledge_type, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed updating KMS with attribute: %s", knowledge.attribute_name)
else:
rospy.loginfo("Updated KMS with attribute: %s", knowledge.attribute_name)
def ground_simple_effect(self, effect, bound_parameters):
# TODO fetch predicate labels
# bind objects from action message to effect predicate
item = KnowledgeItem()
item.knowledge_type = KnowledgeItem.FACT
item.attribute_name = effect.name
for j in range(0, len(effect.typed_parameters)):
# set key (parameter label in predicate) and value (object bound to operator)
pair = KeyValue()
pair.key = "TODO THE LABEL" # self.predicates[self.op.at_start_del_effects[i].name].typed_parameters[j].key
pair.value = bound_parameters[effect.typed_parameters[j].key]
item.values.append(pair)
return item
def predicate_maker(attr_name, attr_key, attr_value, is_negative=None):
ki = KnowledgeItem()
ki.knowledge_type = 1
ki.attribute_name = attr_name
ki_values = []
if type(attr_key) == list:
for key, value in zip(attr_key, attr_value):
kv = KeyValue()
kv.key = key
kv.value = value
ki_values.append(kv)
else:
kv = KeyValue()
kv.key = attr_key
kv.value = attr_value
ki_values.append(kv)
ki.values = ki_values
if is_negative:
ki.is_negative = is_negative
return ki
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 createGoal():
rosplan_pub = rospy.Publisher("/kcl_rosplan/planning_commands", String, queue_size=10)
speech_pub = rospy.Publisher("/KomodoSpeech/rec_command", KomodoSpeechRecCommand, queue_size=10)
command = KomodoSpeechRecCommand()
command.header = Header()
command.cmd = 'start'
command.cat = 'cmd'
rospy.sleep(2.)
speech_pub.publish(command)
result = rospy.wait_for_message("/KomodoSpeech/rec_result", KomodoSpeechRecResult, timeout=30)
if result and result.cat == "cmd" and result.success is True and result.phrase_id == 8:
attribute_query_client = rospy.ServiceProxy("/kcl_rosplan/get_current_knowledge", GetAttributeService)
knowledge_items = attribute_query_client.call("robot_at")
if len(knowledge_items.attributes) > 1:
rospy.loginfo("Failed to add goal. Robot in two places (robot_at)")
return
current_location = knowledge_items.attributes[0].values[0].value
update_knowledge_client = rospy.ServiceProxy("/kcl_rosplan/update_knowledge_base", KnowledgeUpdateService)
add_knowledge_type = KnowledgeUpdateServiceRequest.ADD_GOAL
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = "coke_at"
pair = KeyValue()
pair.key = "loc"
pair.value = current_location #room_2
knowledge.values.append(pair)
update_response = update_knowledge_client(add_knowledge_type, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed to add goal of attribute: %s", knowledge.attribute_name)
else:
rospy.loginfo("Added goal of attribute: %s", knowledge.attribute_name)
rosplan_pub.publish(String("plan"))
else:
rospy.loginfo("Couldn't get command. Closing")
def fill_order(self, order, attribute, first, second, count=-1):
#In transportationtask sometimes the destination is not there in inventory
#Needs to add fact about the inventory
if 'destination' == second:
name = self.load_instance_message(KnowledgeItem.INSTANCE, 'destination', order)
fact_msg = KnowledgeItem()
fact_msg.knowledge_type = KnowledgeItem.FACT
fact_msg.instance_type = ''
fact_msg.instance_name = ''
fact_msg.attribute_name = attribute
fact_msg.function_value = 0.0
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
output_value.value = self.msg_to_instance_name(order, first, count)
fact_msg.values.append(output_value)
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
#in order the container or location doesnt have the count
output_value.value = self.msg_to_instance_name(order, second )
fact_msg.values.append(output_value)
self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_GOAL'] )
def rosplan_update_knowledge(self,
knowledge_type,
instance_type,
instance_name,
attribute_name,
values,
function_value=0.0,
update_type='ADD_KNOWLEDGE'):
'''
This generic function receives knowledge parameters and does a service call
to rosplan knowledge base (mongodb)
rosplan_update_knowledge() = rosplan service call add knowledge
Example 1:
# upload instance : youbot has one dynamixel gripper (dynamixel - gripper)
rosservice call /kcl_rosplan/update_knowledge_base "update_type: 0 # ADD_KNOWLEDGE = 0
knowledge:
knowledge_type: 0 # INSTANCE = 0
instance_type: 'gripper'
instance_name: 'dynamixel'
attribute_name: ''
values:
-{}
function_value: 0.0";
# (on o4 S3)
example 2: object4 (o4) is on location S3
# upload fact :
rosservice call /kcl_rosplan/update_knowledge_base "update_type: 0 # ADD_KNOWLEDGE = 0
knowledge:
knowledge_type: 1 # FACT = 1
instance_type: ''
instance_name: ''
attribute_name: 'on'
values:
- {key: 'o', value: 'o4'}
- {key: 'l', value: 'S3'}
function_value: 0.0";
'''
msg = KnowledgeItem()
if knowledge_type == 0:
# instance
msg.knowledge_type = KnowledgeItem.INSTANCE # INSTANCE (rosplan_knowledge_msgs/msg/KnowledgeItem.msg)
msg.instance_type = instance_type
msg.instance_name = instance_name
msg.attribute_name = ''
elif knowledge_type == 1:
# fact
msg.knowledge_type = KnowledgeItem.FACT # FACT (rosplan_knowledge_msgs/msg/KnowledgeItem.msg)
msg.instance_type = ''
msg.instance_name = ''
msg.attribute_name = attribute_name
for each in values:
msg.values.append(KeyValue(each[0], each[1]))
else:
rospy.logerr(
'error: no information will be uploaded, Knowledge type should be INSTANCE = 0, or FACT = 1'
)
return False
msg.function_value = function_value
rospy.logdebug('======msg=========')
rospy.logdebug(msg)
rospy.logdebug('================')
rospy.loginfo(
'Waiting for /kcl_rosplan/update_knowledge_base to become available...'
)
rospy.wait_for_service('/kcl_rosplan/update_knowledge_base')
try:
update_kb = rospy.ServiceProxy(
'/kcl_rosplan/update_knowledge_base', KnowledgeUpdateService)
# get response
if update_type == 'ADD_KNOWLEDGE':
response = update_kb(
KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE, msg)
elif update_type == 'ADD_GOAL':
response = update_kb(KnowledgeUpdateServiceRequest.ADD_GOAL,
msg)
elif update_type == 'REMOVE_KNOWLEDGE':
response = update_kb(
KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE, msg)
elif update_type == 'REMOVE_GOAL':
response = update_kb(KnowledgeUpdateServiceRequest.REMOVE_GOAL,
msg)
else:
rospy.logerr(
'Error : Unknown update_type, admisible values are ADD_KNOWLEDGE, ADD_GOAL, REMOVE_KNOWLEDGE and REMOVE_GOAL'
)
rospy.logwarn('Knowledge base not updated...!')
except rospy.ServiceException, e:
rospy.logerr('Service call failed: %s' % e)
return False
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 apply_effects_to_KMS(self, block_name, other_block_name, action_name):
fname = "{}::{}".format(self.__class__.__name__, self.apply_effects_to_KMS.__name__)
emptyhand_update_type = KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE
not_emptyhand_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE
onblock_update_type = KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE
clear_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE
inhand_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE
# not_emptyhand predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "not_emptyhand"
update_response = self.update_knowledge_client(not_emptyhand_update_type, updated_knowledge)
rospy.logdebug(
"{}: Updated KMS with {} {}".format(fname, updated_knowledge.attribute_name, not_emptyhand_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, not_emptyhand_update_type, "not_emptyhand"))
# emptyhand predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "emptyhand"
update_response = self.update_knowledge_client(emptyhand_update_type, updated_knowledge)
rospy.logdebug(
"{}: Updated KMS with {} {}".format(fname, updated_knowledge.attribute_name, emptyhand_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, emptyhand_update_type, "emptyhand"))
# (on ?block ?from_block) predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "on"
pair = KeyValue()
pair.key = "block"
pair.value = block_name
updated_knowledge.values.append(pair)
pair = KeyValue()
pair.key = "on_block"
pair.value = other_block_name
updated_knowledge.values.append(pair)
update_response = self.update_knowledge_client(onblock_update_type, updated_knowledge)
rospy.logdebug("{}: Updated KMS with {} ({}, {}) {}".
format(fname,
updated_knowledge.attribute_name,
updated_knowledge.values[0].value,
updated_knowledge.values[1].value,
onblock_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, onblock_update_type, "on"))
# (clear ?from_block) predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "clear"
pair = KeyValue()
pair.key = "block"
pair.value = other_block_name
updated_knowledge.values.append(pair)
update_response = self.update_knowledge_client(clear_update_type, updated_knowledge)
rospy.logdebug("{}: Updated KMS with {} ({}) {}".
format(fname,
updated_knowledge.attribute_name,
updated_knowledge.values[0].value,
clear_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, clear_update_type, "clear"))
# (inhand ?block) predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "inhand"
pair = KeyValue()
pair.key = "block"
pair.value = block_name
updated_knowledge.values.append(pair)
update_response = self.update_knowledge_client(inhand_update_type, updated_knowledge)
rospy.logdebug("{}: Updated KMS with {} ({}) {}".
format(fname,
updated_knowledge.attribute_name,
updated_knowledge.values[0].value,
inhand_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, inhand_update_type, "inhand"))
def make_srv_req_to_KB(
knowledge_type_is_instance=True,
instance_type="",
instance_name="",
attribute_name="",
values=[],
update_type_is_knowledge=True,
):
"""
make a service call to ``rosplan_knowledge_base`` (mongodb)
:param knowledge_type_is_instance: type of knowledge
:type knowledge_type_is_instance: bool
:param instance_type: type of instance
:type instance_type: str
:param instance_name: name of instance
:type instance_name: str
:param attribute_name: name of attribute
:type attribute_name: str
:param values: values for attribute
:type value: list (tuple (str, str))
:param update_type_is_knowledge: type of update to be performed
:type update_type_is_knowledge: bool
:return: success
:rtype: bool
example 1:
**upload instance**: youbot has one ``dynamixel`` ``gripper``.
.. code-block:: bash
rosservice call /rosplan_knowledge_base/update "update_type: 0
knowledge:
knowledge_type: 0
instance_type: 'gripper'
instance_name: 'dynamixel'
attribute_name: ''
values: {}
function_value: 0.0";
To perform the following using this function
.. code-block:: python
ProblemUploader.make_srv_req_to_KB(knowledge_type_is_instance=True,
instance_type='gripper',
instance_name='dynamixel')
example 2:
**upload fact**: object ``o4`` is on location ``S3``
.. code-block:: bash
rosservice call /rosplan_knowledge_base/update "update_type: 0
knowledge:
knowledge_type: 1
instance_type: ''
instance_name: ''
attribute_name: 'on'
values:
- {key: 'o', value: 'o4'}
- {key: 'l', value: 'S3'}
function_value: 0.0";
To perform the following using this function
.. code-block:: python
ProblemUploader.make_srv_req_to_KB(knowledge_type_is_instance=False,
attribute_name='on',
values=[('o', 'o4'),
('l', 'S3')])
"""
msg = KnowledgeItem()
if knowledge_type_is_instance:
msg.knowledge_type = KnowledgeItem.INSTANCE
msg.instance_type = instance_type
msg.instance_name = instance_name.upper()
else:
msg.knowledge_type = KnowledgeItem.FACT
msg.attribute_name = attribute_name
msg.values = [KeyValue(i[0], i[1].upper()) for i in values]
update_kb_topic = "/rosplan_knowledge_base/update"
rospy.loginfo("Waiting for " + update_kb_topic)
rospy.wait_for_service(update_kb_topic)
try:
update_kb = rospy.ServiceProxy(update_kb_topic,
KnowledgeUpdateService)
if update_type_is_knowledge:
response = update_kb(Req.ADD_KNOWLEDGE, msg)
else:
response = update_kb(Req.ADD_GOAL, msg)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
if response.success:
rospy.loginfo("KB updated successfully.")
return True
else:
rospy.logerror("KB update failed.")
return False
def load_inventory_to_knowledge_base(self):
if (None == self._inventory_message):
return
#Creating the message
instance_msg = KnowledgeItem()
for item in self._inventory_message.items:
#Attribute on which the fact is written
attribute = None
object_instance_list = []
#################################
## Uploading INSTANCE of Objects
#################################
#check if the instance_id is present
if (0 != item.object.instance_id.data):
#uploading the instance of object
name = self.load_instance_message(KnowledgeItem.INSTANCE, 'object', item )
object_instance_list.append(name)
else:
#if not then
#check quantity and loop on quantity
if (0 != item.quantity.data):
for count in range(item.quantity.data):
#uploading the instance of object
name = self.load_instance_message(KnowledgeItem.INSTANCE, 'object', item, count)
object_instance_list.append(name)
else:
rospy.loginfo("Instance ID not provided as well as quantity ")
#uploading the instance of object without ID and quantity
name = self.load_instance_message(KnowledgeItem.INSTANCE, 'object', item)
object_instance_list.append(name)
self.event_out.publish("e_failure")
#Flag for identifying if location or container
instance_type_identifyer = None
#################################
## Uploading INSTANCE of locations
#################################
#check if location exist
if (0 != item.location.type.data):
name = self.load_instance_message(KnowledgeItem.INSTANCE, 'location', item)
#fill fact "Object" ON "location"
attribute = 'on'
instance_type_identifyer = 'location'
#################################
## Uploading INSTANCE of CONTAINER
#################################
elif (0 != item.container.type.data):
name = self.load_instance_message(KnowledgeItem.INSTANCE, 'container', item)
#upload fact "Object" IN "container"
attribute = 'in'
instance_type_identifyer = 'container'
#################################
## Uploading FACTS
#################################
if (attribute):
for uploaded_instance in object_instance_list:
fact_msg = KnowledgeItem()
fact_msg.knowledge_type = KnowledgeItem.FACT
fact_msg.instance_type = ''
fact_msg.instance_name = ''
fact_msg.attribute_name = attribute
fact_msg.function_value = 0.0
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
output_value.value = uploaded_instance
fact_msg.values.append(output_value)
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
output_value.value = self.msg_to_instance_name(item, \
instance_type_identifyer)
fact_msg.values.append(output_value)
self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_KNOWLEDGE'])
attribute = None
#Resetting the flag when the inventory contained some data
rospy.loginfo('Received inventory data updated knowledge base')
self.start_inventory_flag = False
# Triggering the completion of the inventory update
self.inventory_update_complete_flag = True
#clearning inventory message
self._inventory_message = None
self.event_out.publish("e_done")
def generate_knowledge_base(self):
# Remove earlier entries from knowledge database
self.clear_knowledge()
empty_waypoints = []
valid_occupants = self.objects['drone'] + self.objects['turtlebot']
for loc in self.objects['waypoint'] + self.objects['airwaypoint']:
valid = False
if loc in self.at:
for o in self.at[loc]:
valid = True if (o in valid_occupants) else valid
if not valid:
empty_waypoints.append(loc)
# Set up waypoint distances
for wp1 in self.objects['waypoint']:
for wp2 in self.objects['waypoint']:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FUNCTION
kitem.attribute_name = "move-duration"
kitem.values = [KeyValue('from', wp1), KeyValue('to', wp2)]
kitem.function_value = self.waypoint_distance(wp1,wp2)
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
for wp1 in self.objects['airwaypoint']:
for wp2 in self.objects['airwaypoint']:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FUNCTION
kitem.attribute_name = "move-duration"
kitem.values = [KeyValue('from', wp1), KeyValue('to', wp2)]
kitem.function_value = self.waypoint_distance(wp1,wp2)
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
# Set empty for agents and waypoints
for obj in self.objects['drone'] + self.objects['turtlebot'] + empty_waypoints:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FACT
kitem.values = [KeyValue('aw_object', obj)]
kitem.attribute_name = 'empty'
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
# Add agents
for key, items in self.objects.iteritems():
for item in items:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.INSTANCE
kitem.instance_type = key
kitem.instance_name = item
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
# Add locations for agents and boxes
for key, values in self.at.iteritems():
for value in values:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FACT
kitem.values = [KeyValue('object', value), KeyValue('waypoint', key)]
kitem.attribute_name = 'at'
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
# Add state airborne to drones at air waypoints
for air_wp in self.objects['airwaypoint']:
for drone in self.objects['drone']:
if drone in self.at[air_wp]:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FACT
kitem.values = [KeyValue('drone', drone)]
kitem.attribute_name = 'airborne'
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
# Set relation between air and ground waypoints
for air, ground in zip(self.objects['airwaypoint'], self.objects['waypoint']):
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FACT
kitem.values = [KeyValue('airwaypoint', air), KeyValue('waypoint', ground)]
kitem.attribute_name = 'over'
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
# Set rescued persons
for person in self.rescued:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FACT
kitem.values = [KeyValue('person', person)]
kitem.attribute_name = 'handled'
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_KNOWLEDGE, knowledge=kitem)
# Set goals, each person should be handled
for person in self.objects['person']:
kitem = KnowledgeItem()
kitem.knowledge_type = KnowledgeItem.FACT
kitem.values = [KeyValue('person', person)]
kitem.attribute_name = 'handled'
self.update_knowledge(update_type=KnowledgeUpdateServiceEnum.ADD_GOAL, knowledge=kitem)
