def subscribe_topic(self, predicate_name, predicate_info):
params_loaded = self.load_params(
predicate_name,
predicate_info) # If parameters found, add 1 to the indexes
if not params_loaded[0]:
return False
if len(predicate_info) < 2 + int(params_loaded[1]):
rospy.logerr("Error: Wrong configuration file for predicate %s" %
predicate_name)
return False
try:
msg_type = predicate_info['msg_type']
except KeyError:
rospy.logerr("Error: msg_type was not specified for predicate %s" %
predicate_name)
return False
self.import_msg(msg_type)
try:
if isinstance(predicate_info['topic'], list):
for topic in predicate_info['topic']:
pred_info = predicate_info.copy()
pred_info['publisher'] = topic
rospy.Subscriber(str(topic),
eval(msg_type[msg_type.rfind('/') + 1:]),
self.subs_callback,
(predicate_name, pred_info))
else:
rospy.Subscriber(predicate_info['topic'],
eval(msg_type[msg_type.rfind('/') + 1:]),
self.subs_callback,
(predicate_name, predicate_info))
except KeyError:
rospy.logerr("Error: topic was not specified for predicate %s" %
predicate_name)
return False
# self.sensed_topics[predicate_name] = (None, False)
try: # Update KB to inform about the sensed predicates
sensed_srv_req = SetNamedBoolRequest()
sensed_srv_req.name = predicate_name
sensed_srv_req.value = True
self.set_sensed_predicate_srv.call(sensed_srv_req)
except Exception as e:
rospy.logerr(
'KCL: (RosplanSensing) Could not update sensing information in Knowledge Base for proposition %s',
predicate_name)
rospy.loginfo(
'KCL: (RosplanSensing) Predicate %s: Subscribed to topic %s of type %s',
predicate_name, predicate_info['topic'], msg_type)
return True
def create_service_client(self, predicate_name, predicate_info):
params_loaded = self.load_params(
predicate_name,
predicate_info) # If parameters found, add 1 to the indexes
if not params_loaded[0]:
return False
if len(predicate_info) < 2 + int(params_loaded[1]):
rospy.logerr("Error: Wrong configuration file for predicate %s" %
predicate_name)
return False
try:
srv_type = predicate_info['srv_type']
except KeyError:
rospy.logerr("Error: service was not specified for predicate %s" %
predicate_name)
return False
srv_typename = self.import_srv(srv_type)
self.service_type_names.append(srv_typename)
self.clients_sub_idx.append(predicate_info['sub_idx'])
try:
self.service_clients.append(
rospy.ServiceProxy(predicate_info['service'],
eval(srv_typename)))
self.service_names.append(predicate_info['service'])
except KeyError:
rospy.logerr("Error: service was not specified for predicate %s" %
predicate_name)
return False
self.service_predicate_names.append(predicate_name)
self.last_called_time.append(rospy.Time(0))
try:
self.time_between_calls.append(
predicate_info['time_between_calls'])
except:
rospy.logerr(
"Error: time_between_calls was not specified for predicate %s"
% predicate_name)
return False
# Gets the request creation
if 'request' in predicate_info:
self.request_src.append(predicate_info['request'])
else:
self.request_src.append(None)
# Gets the result processing
if 'operation' in predicate_info:
self.response_process_src.append(predicate_info['operation'])
else:
self.response_process_src.append(None)
try: # Update KB to inform about the sensed predicates
sensed_srv_req = SetNamedBoolRequest()
sensed_srv_req.name = predicate_name
sensed_srv_req.value = True
self.set_sensed_predicate_srv.call(sensed_srv_req)
except Exception as e:
rospy.logerr(
'KCL: (RosplanSensing) Could not update sensing information in Knowledge Base for proposition %s',
predicate_name)
rospy.loginfo(
'KCL: (RosplanSensing) Predicate %s: Client for service %s of type %s is ready',
predicate_name, predicate_info['service'], srv_type)
return True
def __init__(self):
self.mutex = Lock()
self.srv_mutex = Lock()
################################################################################################################
# Init clients and publishers
rospy.wait_for_service('/rosplan_knowledge_base/update_array')
self.update_kb_srv = rospy.ServiceProxy(
'/rosplan_knowledge_base/update_array',
KnowledgeUpdateServiceArray)
rospy.wait_for_service(
'/rosplan_knowledge_base/domain/predicate_details')
self.get_predicates_srv = rospy.ServiceProxy(
'/rosplan_knowledge_base/domain/predicate_details',
GetDomainPredicateDetailsService)
rospy.wait_for_service('/rosplan_knowledge_base/domain/functions')
self.get_functions_srv = rospy.ServiceProxy(
'/rosplan_knowledge_base/domain/functions',
GetDomainAttributeService)
rospy.wait_for_service('/rosplan_knowledge_base/state/instances')
self.get_instances_srv = rospy.ServiceProxy(
'/rosplan_knowledge_base/state/instances', GetInstanceService)
rospy.wait_for_service('/rosplan_knowledge_base/state/propositions')
self.get_state_propositions_srv = rospy.ServiceProxy(
'/rosplan_knowledge_base/state/propositions', GetAttributeService)
rospy.wait_for_service('/rosplan_knowledge_base/state/functions')
self.get_state_functions_srv = rospy.ServiceProxy(
'/rosplan_knowledge_base/state/functions', GetAttributeService)
set_sensed_predicate_srv = rospy.ServiceProxy(
'/rosplan_knowledge_base/update_sensed_predicates', SetNamedBool)
################################################################################################################
# Get cfg
self.cfg_topics = self.cfg_service = {}
self.functions_path = None
found_config = False
if rospy.has_param('~topics'):
self.cfg_topics = rospy.get_param('~topics')
found_config = True
if rospy.has_param('~services'):
self.cfg_service = rospy.get_param('~services')
found_config = True
if rospy.has_param('~functions'):
self.functions_path = rospy.get_param('~functions')[0]
regexp = re.compile('\$\(find (.*)\)')
groups = regexp.match(self.functions_path).groups()
if len(groups):
try:
ros_pkg_path = rospkg.RosPack().get_path(groups[0])
self.functions_path = regexp.sub(ros_pkg_path,
self.functions_path)
except:
rospy.logerr(
'KCL: (RosplanSensing) Error: Package %s was not found! Fix configuration file and retry.'
% groups[0])
rospy.signal_shutdown('Wrong path in cfg file')
return
if not found_config:
rospy.logerr(
'KCL: (RosplanSensing) Error: configuration file is not defined!'
)
rospy.signal_shutdown('Config not found')
return
################################################################################################################
# Load scripts
if self.functions_path:
self.scripts = imp.load_source('sensing_scripts',
self.functions_path)
# Declare tools in the scripts module:
self.scripts.get_kb_attribute = self.get_kb_attribute
self.scripts.rospy = rospy
################################################################################################################
# Init variables
self.sensed_topics = {}
self.sensed_services = {}
self.params = {} # Params defined for each predicate
######
# Subscribe to all the topics
self.offset = {} # Offset for reading cfg
for predicate_name, predicate_info in self.cfg_topics.iteritems():
params_loaded = self.load_params(
predicate_name,
predicate_info) # If parameters found, add 1 to the indexes
if not params_loaded[0]:
continue
if len(predicate_info) < 2 + int(params_loaded[1]):
rospy.logerr(
"Error: Wrong configuration file for predicate %s" %
predicate_name)
continue
try:
msg_type = predicate_info['msg_type']
except KeyError:
rospy.logerr(
"Error: msg_type was not specified for predicate %s" %
predicate_name)
continue
self.import_msg(msg_type)
try:
rospy.Subscriber(predicate_info['topic'],
eval(msg_type[msg_type.rfind('/') + 1:]),
self.subs_callback, predicate_name)
except KeyError:
rospy.logerr(
"Error: topic was not specified for predicate %s" %
predicate_name)
continue
# self.sensed_topics[predicate_name] = (None, False)
try: # Update KB to inform about the sensed predicates
sensed_srv_req = SetNamedBoolRequest()
sensed_srv_req.name = predicate_name
sensed_srv_req.value = True
set_sensed_predicate_srv.call(sensed_srv_req)
except Exception as e:
rospy.logerr(
'KCL: (RosplanSensing) Could not update sensing information in Knowledge Base for proposition %s',
predicate_name)
rospy.loginfo(
'KCL: (RosplanSensing) Predicate %s: Subscribed to topic %s of type %s',
predicate_name, predicate_info['topic'], msg_type)
############
# Create clients for all the services
self.service_clients = []
self.service_names = []
self.service_type_names = []
self.service_predicate_names = []
self.last_called_time = []
self.time_between_calls = []
self.request_src = []
self.response_process_src = []
for predicate_name, predicate_info in self.cfg_service.iteritems():
params_loaded = self.load_params(
predicate_name,
predicate_info) # If parameters found, add 1 to the indexes
if not params_loaded[0]:
continue
if len(predicate_info) < 2 + int(params_loaded[1]):
rospy.logerr(
"Error: Wrong configuration file for predicate %s" %
predicate_name)
continue
try:
srv_type = predicate_info['srv_type']
except KeyError:
rospy.logerr(
"Error: service was not specified for predicate %s" %
predicate_name)
continue
srv_typename = self.import_srv(srv_type)
self.service_type_names.append(srv_typename)
try:
self.service_clients.append(
rospy.ServiceProxy(predicate_info['service'],
eval(srv_typename)))
self.service_names.append(predicate_info['service'])
except KeyError:
rospy.logerr(
"Error: service was not specified for predicate %s" %
predicate_name)
continue
self.service_predicate_names.append(predicate_name)
self.last_called_time.append(rospy.Time(0))
try:
self.time_between_calls.append(
predicate_info['time_between_calls'])
except:
rospy.logerr(
"Error: time_between_calls was not specified for predicate %s"
% predicate_name)
continue
# Gets the request creation
if 'request' in predicate_info:
self.request_src.append(predicate_info['request'])
else:
self.request_src.append(None)
# Gets the result processing
if 'operation' in predicate_info:
self.response_process_src.append(predicate_info['operation'])
else:
self.response_process_src.append(None)
try: # Update KB to inform about the sensed predicates
sensed_srv_req = SetNamedBoolRequest()
sensed_srv_req.name = predicate_name
sensed_srv_req.value = True
set_sensed_predicate_srv.call(sensed_srv_req)
except Exception as e:
rospy.logerr(
'KCL: (RosplanSensing) Could not update sensing information in Knowledge Base for proposition %s',
predicate_name)
rospy.loginfo(
'KCL: (RosplanSensing) Predicate %s: Client for service %s of type %s is ready',
predicate_name, predicate_info['service'], srv_type)