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