def get_surface_category_counts(self, obj_category_map):
'''Returns a dictionary of surfaces and object category counts in which
each key represents a surface in the environment and the value
is a dictionary of object counts for each category
'''
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'on'
on_result = self.attribute_fetching_client(request)
surface_category_counts = dict()
for item in on_result.attributes:
obj_name = ''
obj_surface = ''
for param in item.values:
if param.key == 'obj':
obj_name = param.value
elif param.key == 'plane':
obj_surface = param.value
# we don't want to place items on the table, so we
# don't consider the table as a placing surface
if obj_surface not in surface_category_counts and obj_surface != 'table':
surface_category_counts[obj_surface] = dict()
if obj_surface != 'table':
obj_category = obj_category_map[obj_name]
if obj_category not in surface_category_counts[obj_surface]:
surface_category_counts[obj_surface][obj_category] = 1
else:
surface_category_counts[obj_surface][obj_category] += 1
return surface_category_counts
def get_all_attributes(self, predicate_name):
'''Returns a list of all instances of the given predicate in the knowledge base.
@param predicate_name -- string representing the name of a predicate
'''
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = predicate_name
result = self.attribute_retrieval_client(request)
return result.attributes
def get_surface_category_counts(self, obj_category_map):
'''Returns a dictionary of surfaces and object category counts in which
each key represents a surface in the environment and the value
is a dictionary of object counts for each category
'''
surface_category_counts = dict()
# we take all explored surfaces and populate 'surface_category_counts'
# with an empty dictionary for each surface; each such dictionary
# will store the object category count for the respective surface
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'explored'
explored_result = self.attribute_fetching_client(request)
for item in explored_result.attributes:
surface_name = ''
for param in item.values:
if param.key == 'plane':
surface_name = param.value
# we don't want to place items on the table, so we
# don't consider the table as a placing surface
if surface_name not in surface_category_counts and surface_name.find('table') == -1:
surface_category_counts[surface_name] = dict()
# we collect a dictionary of object category counts for each surface
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'on'
on_result = self.attribute_fetching_client(request)
for item in on_result.attributes:
obj_name = ''
obj_surface = ''
for param in item.values:
if param.key == 'obj':
obj_name = param.value
elif param.key == 'plane':
obj_surface = param.value
if obj_surface.find('table') == -1:
obj_category = obj_category_map[obj_name]
if obj_category not in surface_category_counts[obj_surface]:
surface_category_counts[obj_surface][obj_category] = 1
else:
surface_category_counts[obj_surface][obj_category] += 1
return surface_category_counts
def get_placing_surface(self, obj_name, surface_prefix='table'):
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'explored'
explored_result = self.attribute_fetching_client(request)
for item in explored_result.attributes:
surface_name = ''
for param in item.values:
if param.key == 'plane' and param.value.find(
surface_prefix) != -1:
return param.value
def execute(self, userdata):
if self.save_sm_state:
self.save_current_state()
# if the current location of the robot was not specified
# in the request, we query this information from the knowledge base
original_location = ''
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'robot_at'
result = self.attribute_fetching_client(request)
for item in result.attributes:
if not item.is_negative:
for param in item.values:
if param.key == 'bot' and param.value != self.robot_name:
break
if param.key == 'wp':
original_location = param.value
break
break
for destination_location in self.destination_locations:
dispatch_msg = self.get_dispatch_msg(original_location,
destination_location)
rospy.loginfo('Sending the base to %s' % destination_location)
self.say('Going to ' + destination_location)
self.action_dispatch_pub.publish(dispatch_msg)
self.executing = True
self.succeeded = False
start_time = time.time()
duration = 0.
while self.executing and duration < self.timeout:
rospy.sleep(0.1)
duration = time.time() - start_time
if self.succeeded:
rospy.loginfo('Successfully reached %s' % destination_location)
original_location = destination_location
else:
rospy.logerr('Could not reach %s' % destination_location)
self.say('Could not reach ' + destination_location)
if self.retry_count == self.number_of_retries:
self.say('Aborting operation')
return 'failed_after_retrying'
self.retry_count += 1
return 'failed'
return 'succeeded'
def surface_empty(self, surface_name='table'):
no_objects_on_surface = True
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'on'
result = self.attribute_fetching_client(request)
for item in result.attributes:
object_on_desired_surface = False
if not item.is_negative:
for param in item.values:
if param.key == 'plane' and param.value == surface_name:
object_on_desired_surface = True
if object_on_desired_surface:
no_objects_on_surface = False
break
return no_objects_on_surface
def get_obj_category_map(self):
'''Returns a dictionary of objects and object categories in which
each key represents an object and the value is its category
'''
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'object_category'
category_result = self.attribute_fetching_client(request)
obj_category_dict = dict()
for item in category_result.attributes:
obj_name = ''
obj_category = ''
for param in item.values:
if param.key == 'obj':
obj_name = param.value
elif param.key == 'cat':
obj_category = param.value
obj_category_dict[obj_name] = obj_category
return obj_category_dict
def get_surface_objects(self, surface_prefix='table'):
surface_objects = dict()
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'on'
result = self.attribute_fetching_client(request)
for item in result.attributes:
object_on_desired_surface = False
object_name = ''
surface_name = ''
if not item.is_negative:
for param in item.values:
if param.key == 'plane' and param.value.find(
surface_prefix) != -1:
object_on_desired_surface = True
surface_name = param.value
if surface_name not in surface_objects:
surface_objects[surface_name] = list()
elif param.key == 'obj':
object_name = param.value
if object_on_desired_surface:
surface_objects[surface_name].append(object_name)
return surface_objects
def __init__(self):
# a client for the /rosplan_knowledge_base/domain/predicates service
self.predicate_retrieval_client = None
# a client for the /rosplan_knowledge_base/update service
self.knowledge_update_client = None
# a client for the /rosplan_knowledge_base/state/propositions service
self.attribute_retrieval_client = None
# a mongodb_store.message_store.MessageStoreProxy instance
self.msg_store_client = None
# name of the host robot; value read from the knowledge base
self.robot_name = ''
rospy.loginfo(
'[kb_interface] Creating a domain predicate retrieval client')
try:
rospy.wait_for_service('/rosplan_knowledge_base/domain/predicates',
5.)
self.predicate_retrieval_client = rospy.ServiceProxy(
'/rosplan_knowledge_base/domain/predicates',
rosplan_srvs.GetDomainAttributeService)
except (rospy.ServiceException, rospy.ROSException) as exc:
rospy.logwarn(
'The service /rosplan_knowledge_base/domain/predicates does not appear to exist.\n'
+ 'Please spawn rosplan_knowledge_base/knowledgeBase')
rospy.loginfo('[kb_interface] Creating a knowledge base update client')
try:
rospy.wait_for_service('/rosplan_knowledge_base/update', 5.)
self.knowledge_update_client = rospy.ServiceProxy(
'/rosplan_knowledge_base/update',
rosplan_srvs.KnowledgeUpdateService)
except (rospy.ServiceException, rospy.ROSException) as exc:
rospy.logwarn(
'The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n'
+ 'Please spawn rosplan_knowledge_base/knowledgeBase')
rospy.loginfo('[kb_interface] Creating a knowledge base query client')
try:
rospy.wait_for_service(
'/rosplan_knowledge_base/state/propositions', 5.)
self.attribute_retrieval_client = rospy.ServiceProxy(
'/rosplan_knowledge_base/state/propositions',
rosplan_srvs.GetAttributeService)
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'robot_name'
result = self.attribute_retrieval_client(request)
for item in result.attributes:
for param in item.values:
if param.key == 'bot':
self.robot_name = param.value
break
break
except (rospy.ServiceException, rospy.ROSException) as exc:
rospy.logwarn(
'The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n'
+ 'Please spawn rosplan_knowledge_base/knowledgeBase')
rospy.loginfo('[kb_interface] Creating a message store client')
try:
self.msg_store_client = MessageStoreProxy()
except:
print('Could not create a mongodb_store proxy.\n' +
'Please spawn mongodb_store/message_store_node.py')
def __init__(self,
action_name,
outcomes,
input_keys=list(),
output_keys=list(),
save_sm_state=False):
smach.State.__init__(self,
outcomes=outcomes,
input_keys=input_keys,
output_keys=output_keys)
self.sm_id = ''
self.state_name = ''
self.action_name = action_name
self.save_sm_state = save_sm_state
self.retry_count = 0
self.executing = False
self.succeeded = False
self.knowledge_storing_enabled = rospy.get_param(
'/store_knowledge', False)
self.say_pub = rospy.Publisher('/say',
String,
latch=True,
queue_size=1)
self.action_dispatch_pub = rospy.Publisher(
'/kcl_rosplan/action_dispatch',
plan_dispatch_msgs.ActionDispatch,
queue_size=1)
rospy.Subscriber('/kcl_rosplan/action_feedback',
plan_dispatch_msgs.ActionFeedback,
self.get_action_feedback)
self.robot_name = ''
self.attribute_fetching_client = None
self.msg_store_client = None
if self.knowledge_storing_enabled:
try:
rospy.wait_for_service('/kcl_rosplan/get_current_knowledge',
5.)
self.attribute_fetching_client = rospy.ServiceProxy(
'/kcl_rosplan/get_current_knowledge',
rosplan_srvs.GetAttributeService)
self.robot_name = ''
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'robot_name'
result = self.attribute_fetching_client(request)
for item in result.attributes:
for param in item.values:
if param.key == 'bot':
self.robot_name = param.value
break
break
except (rospy.ServiceException, rospy.ROSException), exc:
rospy.logwarn(
'Service /kcl_rosplan/get_current_knowledge does not appear to exist.\n'
+
'If you intend to use the knowledge base, please spawn ' +
'rosplan_knowledge_base/knowledgeBase')
try:
self.msg_store_client = MessageStoreProxy()
except Exception, exc:
rospy.logwarn(
'Could not create a mongodb_store proxy.\n' +
'If you intend to store online knowledge, please spawn\n' +
'mongodb_store/message_store_node.py')