def generateCode(self):
# Subito prima di generare il codice devo aggiungere la libreria con la node configuration
# se serve ed è usata
if self.associated_class.node_configuration:
lib = Library()
lib.setPath(self.associated_class.node_configuration.
getSourceLibraryPath())
self.addLibrary(lib)
if self.uses_tf:
self.addLibrary(ROSBase_TF_Interface())
code = ""
for l in self.libraries:
code += l.generateCode()
code += "\n"
for f in self.functions:
code += "".join(
[gen_s + "\n" for gen_s in f.generateCode().split("\n")])
return code
def populateData(self):
main_thread = self.associated_class.getMainThread()
if main_thread == None:
return False, "Unable to get the Main Thread"
#########################
# TRANSFORMATION FRAME #
########################
# Controllo l'uso del Transformation Frame
self.thread_uses_tf = self.setUsesTransformationFrame()
###############
# Output Port #
###############
# Essendo birezeizonale posso trovare la connessione sia come source che come dest
conn_by_source = True
process_input_port = tfs.getConnectionPortInfoBySource(
self.process, self.type, self.input_port_name)
if process_input_port is None:
conn_by_source = False
process_output_port = tfs.getConnectionPortInfoByDest(
self.process, self.type, self.input_port_name)
if process_input_port is None:
return (
False,
"Unable to find the right binding between process requires subprogram access port and "
"thread input port")
if conn_by_source:
(source_parent_name,
source_name) = tfs.getDestFromPortInfo(process_input_port)
else:
(source_parent_name,
source_name) = tfs.getSourceFromPortInfo(process_output_port)
if source_parent_name is None or source_name is None:
return False, "Unable to find the process provides subprogram access port name"
self.process_port = tfs.getFeatureByName(self.process,
name=source_name)
if self.process_port is None:
return False, "Unable to find the process provides subprogram access port feature"
# Dopo aver trovato la porta del process, controllo il nome di default del
# services associato
(topic_namespace, self.default_service_name) = tfs.getDefaultTopicName(
self.process_port)
if self.default_service_name == None:
self.default_service_name = "service_name_default"
log.warning(
"Default Service Name not found, set as {} as default.".format(
self.default_service_name))
##################################
### ASN.1 Request and Response ###
##################################
(aadl_namespace,
aadl_type) = tfs.getPortDatatypeByPort(self.process_port)
if aadl_namespace == None or aadl_type == None:
return (False, "Unable to identify process port type")
# Controllo se c'è un file ASN.1 associato alla porta. Se c'è allora il tipo di servizio
# è custom e lo dovrò generare, mentre se non c'è allora è un servizio standard ROS
port_data_info = tfs.getPortDataInfo(self.process_port)
if port_data_info == None:
return (False, "Unable to get the port data info for process port")
self.asn_description = tfs.getSourceText(port_data_info)
if self.asn_description == None:
# @TODO: Standard Service
log.warning("STANDARD SERVICE")
# return (False, "Unable to find property Source_Text for the services caller with ASN.1 description")
else:
# Creo il servizio custom e lo associo al nodo che lo ha generato
self.service = sfs.getServiceFromJSON(aadl_namespace, aadl_type,
self.asn_description,
self.associated_class)
if self.service == None:
return (False, "Error in ASN.1 parsing")
# Genero ed aggiungo la libreria del services al nodo
service_library = Library(self.associated_class)
service_library.setPath("{}/{}.h".format(self.service.namespace,
self.service.name))
self.associated_class.addLibrary(service_library)
##########################
### SERVICE SERVER VAR ###
##########################
var_service_server = Variable(self.associated_class)
var_service_server.setName("service_server_{}".format(self.name))
var_service_server.setType(ROS_ServiceServer(self.associated_class))
self.associated_class.addInternalVariable(var_service_server)
###############################
### SERVICE SERVER CALLBACK ###
###############################
self.server_callback = Method(self.associated_class)
self.server_callback.method_name = "{}_service_callback".format(
self.name)
self.server_callback.return_type = Bool(self.associated_class)
self.server_callback.namespace = self.associated_class.class_name
# REQUEST Parameter
input_param_req = Variable(self.associated_class)
input_param_req.setName("&req")
input_param_req.setType(
ROS_ServiceServer_Request(
self.associated_class, "{}::{}".format(self.service.namespace,
self.service.name)))
input_param_req.setIsParameter()
# RESPONSE Parameter
input_param_res = Variable(self.associated_class)
input_param_res.setName("&res")
input_param_res.setType(
ROS_ServiceServer_Response(
self.associated_class, "{}::{}".format(self.service.namespace,
self.service.name)))
input_param_res.setIsParameter()
self.server_callback.addInputParameter(input_param_req)
self.server_callback.addInputParameter(input_param_res)
###############
# SOURCE TEXT #
###############
function = tfs.getSubcomponentByInfo(self.thread,
name=self.function_name,
namespace="ros",
category="subprogram")
if function is None:
return False, "Unable to find the function subprogram"
self.source_text_function = self.createSourceTextFileFromSourceText(
tfs.getSourceText(function), tfs.getSourceName(function))
if self.source_text_function is None:
return False, "Unable to find property Source_Text or Source_Name"
self.source_text_function.setTF(self.thread_uses_tf)
# Aggiungo la chiamata alla funzione custom
if self.source_text_function != None:
self.source_text_function.addServiceReqAndRes(
input_param_req, input_param_res)
self.source_text_function.addLibrary(service_library)
self.source_text_function.setFunctionType(
Bool(self.associated_class))
code = "return {};".format(
self.source_text_function.generateInlineCode())
self.server_callback.addMiddleCode(code)
self.associated_class.addPrivateMethod(self.server_callback)
main_thread.prepare.addMiddleCode(
"{} = handle.advertiseService(\"{}\", {}, this);".format(
var_service_server.name, self.default_service_name,
self.server_callback.getThreadPointer()))
return (True, "")
def populateSubscriberData(self):
##################
### Input Port ###
##################
# Ottengo la connesione che mappa la porta di input del thread subscriber
# con quella che entra nel process
process_input_port = tfs.getConnectionPortInfoByDest(
self.process, self.type, self.input_port_name)
if process_input_port == None:
return (
False,
"Unable to find the right binding between process input port and thread input port"
)
(source_parent_name,
source_name) = tfs.getSourceFromPortInfo(process_input_port)
if source_parent_name == None or source_name == None:
return (False, "Unable to find the process input port name")
self.sub_process_port = tfs.getFeatureByName(self.process,
name=source_name)
if self.sub_process_port == None:
return (False,
"Unable to find the process input port name feature")
##################
### INPUT TYPE ###
##################
(aadl_namespace,
aadl_type) = tfs.getPortDatatypeByPort(self.sub_process_port)
if aadl_namespace == None or aadl_type == None:
return (False, "Unable to identify process port type")
# Controllo se c'è un file ASN.1 associato alla porta. Se c'è allora il tipo di messaggio
# è custom e lo dovrò generare, mentre se non c'è allora è un messaggio standard ROS
port_data_info = tfs.getPortDataInfo(self.sub_process_port)
if port_data_info == None:
return (False, "Unable to get the port data info for process port")
port_data_source_asn = tfs.getSourceText(port_data_info)
if port_data_source_asn == None:
# Se è None allora non c'è alcun file ASN.1 associato e quindi è un messaggio standard ROS
raw_output_type = dt.getROSDatatypeFromAADL(
aadl_namespace, aadl_type, self.associated_class)
if raw_output_type == None:
return (False,
"Datatype {} NOT supported".format(raw_output_type))
else:
self.input_type = raw_output_type
else:
self.custom_message = mfs.getMessageFromJSON(
aadl_namespace, aadl_type, port_data_source_asn,
self.associated_class)
self.input_type = Type(self.associated_class)
self.input_type.setTypeName(self.custom_message.name)
self.input_type.setNamespace(self.custom_message.namespace)
self.input_type.setConst(_const=True)
self.input_type.setAfterTypeName("::ConstPtr&")
# Associo la librerie del messaggio al tipo di output, sia custom che standard
input_type_library = Library()
input_type_library.setPath("{}/{}.h".format(self.input_type.namespace,
self.input_type.type_name))
self.input_type.setLibrary(input_type_library)
########################
### SUBSCRIBER TOPIC ###
########################
(status, desc) = self.getDefaultTopicName(self.input_port_name,
input=True)
if status == False:
return (status, desc)
##################
### QUEUE SIZE ###
##################
queue_size_default_value = 1
self.queue_size = tfs.getSubscriberQueueSize(
self.thread, port_name=self.input_port_name)
if self.queue_size == None:
self.queue_size = queue_size_default_value
log.info("Queue size set to default value: {}".format(
self.queue_size))
######################
### SUBSCRIBER VAR ###
######################
var_subscriber_pub = Variable(self.associated_class)
var_subscriber_pub.setName("sub_{}".format(self.name))
var_subscriber_pub.setType(ROS_Subscriber(self.associated_class))
self.associated_class.addInternalVariable(var_subscriber_pub)
###########################
### SUBSCRIBER CALLBACK ###
###########################
self.sub_callback = Method(self.associated_class)
self.sub_callback.method_name = "{}_callback".format(self.name)
self.sub_callback.return_type = Void(self.associated_class)
self.sub_callback.namespace = self.associated_class.class_name
self.sub_input_var = Variable(self.associated_class)
self.sub_input_var.setType(self.input_type)
self.sub_input_var.setName("msg")
self.sub_input_var.setIsParameter()
self.sub_callback.addInputParameter(self.sub_input_var)
self.associated_class.addPrivateMethod(self.sub_callback)
self.main_thread.prepare.addMiddleCode(
"{} = handle.subscribe(\"{}\", {}, {}, this);".format(
var_subscriber_pub.name, self.topic, self.queue_size,
self.sub_callback.getThreadPointer()))
return (True, "")
def populatePublisherData(self):
###################
### Output Port ###
###################
# Ottengo la connesione che mappa la porta di input del thread subscriber
# con quella che entra nel process
process_output_port = tfs.getConnectionPortInfoBySource(
self.process, self.type, self.output_port_name)
if process_output_port == None:
return (
False,
"Unable to find the right binding between process input port and thread input port"
)
(dest_parent_name,
dest_name) = tfs.getDestFromPortInfo(process_output_port)
if dest_parent_name == None or dest_name == None:
return (False, "Unable to find the process input port name")
self.pub_process_port = tfs.getFeatureByName(self.process,
name=dest_name)
if self.pub_process_port == None:
return (False,
"Unable to find the process input port name feature")
###################
### OUTPUT TYPE ###
###################
(aadl_namespace,
aadl_type) = tfs.getPortDatatypeByPort(self.pub_process_port)
if aadl_namespace == None or aadl_type == None:
return (False, "Unable to identify process port type")
# Controllo se c'è un file ASN.1 associato alla porta. Se c'è allora il tipo di messaggio
# è custom e lo dovrò generare, mentre se non c'è allora è un messaggio standard ROS
port_data_info = tfs.getPortDataInfo(self.pub_process_port)
if port_data_info == None:
return (False, "Unable to get the port data info for process port")
port_data_source_asn = tfs.getSourceText(port_data_info)
if port_data_source_asn == None:
# Se è None allora non c'è alcun file ASN.1 associato e quindi è un messaggio standard ROS
raw_output_type = dt.getROSDatatypeFromAADL(
aadl_namespace, aadl_type, self.associated_class)
if raw_output_type == None:
return (False,
"Datatype {} NOT supported".format(raw_output_type))
else:
self.output_type = raw_output_type
else:
self.custom_message = mfs.getMessageFromJSON(
aadl_namespace, aadl_type, port_data_source_asn,
self.associated_class)
self.output_type = Type(self.associated_class)
self.output_type.setTypeName(self.custom_message.name)
self.output_type.setNamespace(self.custom_message.namespace)
# Associo la librerie del messaggio al tipo di output, sia custom che standard
output_type_library = Library()
output_type_library.setPath("{}/{}.h".format(
self.output_type.namespace, self.output_type.type_name))
self.output_type.setLibrary(output_type_library)
#############
### TOPIC ###
#############
(status, desc) = self.getDefaultTopicName(self.output_port_name,
output=True)
if not status:
return status, desc
#####################
### PUBLISHER VAR ###
#####################
self.var_publisher_pub = Variable(self.associated_class)
self.var_publisher_pub.setName("pub_{}".format(self.name))
self.var_publisher_pub.setType(ROS_Publisher(self.associated_class))
self.associated_class.addInternalVariable(self.var_publisher_pub)
self.main_thread.prepare.addMiddleCode(
"{} = handle.advertise<{}>(\"{}\", 10);".format(
self.var_publisher_pub.name, self.output_type.generateCode(),
self.topic))
return True, ""
def populateData(self):
main_thread = self.associated_class.getMainThread()
if main_thread == None:
return (False, "Unable to get the Main Thread")
###################
### Output Port ###
###################
# Essendo bidirezionale posso trovare la connessione sia come source che come dest
conn_by_source = True
process_output_port = tfs.getConnectionPortInfoBySource(
self.process, self.type, self.output_port_name)
if process_output_port == None:
conn_by_source = False
process_output_port = tfs.getConnectionPortInfoByDest(
self.process, self.type, self.output_port_name)
if process_output_port == None:
return (
False,
"Unable to find the right binding between process requires subprogram access port and "
"thread input port")
if conn_by_source:
(dest_parent_name,
dest_name) = tfs.getDestFromPortInfo(process_output_port)
else:
(dest_parent_name,
dest_name) = tfs.getSourceFromPortInfo(process_output_port)
if dest_parent_name == None or dest_name == None:
return (
False,
"Unable to find the process requires subprogram access port name"
)
self.process_port = tfs.getFeatureByName(self.process, name=dest_name)
if self.process_port == None:
return (
False,
"Unable to find the process requires subprogram access port feature"
)
# Dopo aver trovato la porta del process, controllo il nome di default del
# services associato
(topic_namespace, self.default_service_name) = tfs.getDefaultTopicName(
self.process_port)
if self.default_service_name == None:
self.default_service_name = "service_name_default"
log.warning(
"Default Service Name not found, set as {} as default.".format(
self.default_service_name))
##################################
### ASN.1 Request and Response ###
##################################
(aadl_namespace,
aadl_type) = tfs.getPortDatatypeByPort(self.process_port)
if aadl_namespace is None or aadl_type is None:
return False, "Unable to identify process port type"
# Controllo se c'è un file ASN.1 associato alla porta. Se c'è allora il tipo di servizio
# è custom e lo dovrò generare, mentre se non c'è allora è un servizio standard ROS
port_data_info = tfs.getPortDataInfo(self.process_port)
if port_data_info is None:
return False, "Unable to get the port data info for process port"
self.asn_description = tfs.getSourceText(port_data_info)
if self.asn_description is None:
self.service = Service(aadl_namespace, aadl_type)
else:
# Creo il servizio custom e lo associo al nodo che lo ha generato
self.service = sfs.getServiceFromJSON(self.asn_description,
self.associated_class)
# if self.service == None:
# return (False, "Error in ASN.1 parsing")
# self.associated_class.addService( self.service )
# Genero ed aggiungo la libreria del services al nodo
service_library = Library(self.service.namespace)
service_library.setPath("{}/{}.h".format(self.service.namespace,
self.service.name))
self.associated_class.addLibrary(service_library)
##########################
### SERVICE CLIENT VAR ###
##########################
var_serviceclient = Variable(self.associated_class)
var_serviceclient.setName("service_client_{}".format(self.name))
var_serviceclient.setType(ROS_ServiceClient(self.associated_class))
self.associated_class.addInternalVariable(var_serviceclient)
main_thread.prepare.addMiddleCode(
"{} = handle.serviceClient<{}::{}>(\"{}\");".format(
var_serviceclient.name, self.service.namespace,
self.service.name, self.default_service_name))
return True, ""
def populateData(self):
main_thread = self.associated_class.getMainThread()
if main_thread == None:
return (False, "Unable to get the Main Thread")
# Ottengo le informazioni necessarie per i thread di tipo Publisher:
# - Source Text
# - Period
thread_function = tfs.getSubprogram(self.thread)
if thread_function == None:
return (False, "Unable to find the right Subprogram")
############################
### TRANSFORMATION FRAME ###
############################
# Controllo l'uso del Transformation Frame
self.thread_uses_tf = self.setUsesTransformationFrame()
###################
### Output Port ###
###################
# Ottengo la connesione che mappa la porta di input del thread subscriber
# con quella che entra nel process
process_output_port = tfs.getConnectionPortInfoBySource(self.process, self.type, self.output_port_name)
if process_output_port is None:
return False, "Unable to find the right binding between process input port and thread input port"
(dest_parent_name, dest_name) = tfs.getDestFromPortInfo(process_output_port)
if dest_parent_name is None or dest_name is None:
return False, "Unable to find the process input port name"
self.process_port = tfs.getFeatureByName(self.process, name=dest_name)
if self.process_port is None:
return False, "Unable to find the process input port name feature"
(aadl_namespace, aadl_type) = tfs.getPortDatatypeByPort(self.process_port)
if aadl_namespace is None or aadl_type is None:
return False, "Unable to identify process port type"
# Controllo se c'è un file ASN.1 associato alla porta. Se c'è allora il tipo di messaggio
# è custom e lo dovrò generare, mentre se non c'è allora è un messaggio standard ROS
port_data_info = tfs.getPortDataInfo(self.process_port)
if port_data_info is None:
return False, "Unable to get the port data info for process port"
port_data_source_asn = tfs.getSourceText(port_data_info)
if port_data_source_asn is None:
# Se è None allora non c'è alcun file ASN.1 associato e quindi è un messaggio standard ROS
raw_output_type = dt.getROSDatatypeFromAADL(aadl_namespace, aadl_type, self.associated_class)
if raw_output_type is None:
return False, "Datatype {} NOT supported".format(raw_output_type)
else:
self.output_type = raw_output_type
else:
self.custom_message = mfs.getMessageFromJSON(aadl_namespace,
aadl_type,
port_data_source_asn,
self.associated_class)
self.output_type = Type(self.associated_class)
self.output_type.setTypeName(self.custom_message.name)
self.output_type.setNamespace(self.custom_message.namespace)
# Associo la librerie del messaggio al tipo di output, sia custom che standard
output_type_library = Library()
output_type_library.setPath("{}/{}.h".format(self.output_type.namespace, self.output_type.type_name))
self.output_type.setLibrary(output_type_library)
###################
### Source Text ###
###################
self.source_text_function = self.createSourceTextFileFromSourceText(tfs.getSourceText(thread_function),
tfs.getSourceName(thread_function))
if self.source_text_function is None:
return False, "Unable to find property Source_Text or Source_Name"
self.source_text_function.setTF(self.thread_uses_tf)
self.source_text_function.setFunctionType(self.output_type)
#################
### FREQUENCY ###
#################
(period, period_unit) = tfs.getPeriod(self.thread)
if period is None or period_unit is None:
return False, "Unable to find property Period with relative value and unit"
# Conversione in secondi della frequenza a partire da qualunque unità di misura
try:
period_quantity = ureg("{} {}".format(period, period_unit))
period_quantity.ito(ureg.second)
self.frequency_in_hz = 1.0 / period_quantity.magnitude
self.period_in_seconds = period_quantity.magnitude
except ValueError:
return False, "Unable to convert Period in seconds"
# param_freq = Variable( self.associated_class )
# param_freq.setName( "frequency_{}".format(self.name) )
# param_freq.setType( Int( self.associated_class ))
# self.associated_class.addParameter( param_freq )
#############
### TOPIC ###
#############
(status, desc) = self.getDefaultTopicName(self.output_port_name, output=True)
if status is False:
return status, desc
#####################
### PUBLISHER VAR ###
#####################
var_publisher_pub = Variable(self.associated_class)
var_publisher_pub.setName("pub_{}".format(self.name))
var_publisher_pub.setType(ROS_Publisher(self.associated_class))
self.associated_class.addInternalVariable(var_publisher_pub)
#######################
### PUBLISHER TIMER ###
#######################
var_timer_pub = Variable(self.associated_class)
var_timer_pub.setName("timer_{}".format(self.name))
var_timer_pub.setType(ROS_Timer(self.associated_class))
self.associated_class.addInternalVariable(var_timer_pub)
##########################
### PUBLISHER CALLBACK ###
##########################
self.publisherCallback = Method(self.associated_class)
self.publisherCallback.method_name = "{}_callback".format(self.name)
self.publisherCallback.return_type = Void(self.associated_class)
self.publisherCallback.namespace = self.associated_class.class_name
input_par = Variable(self.associated_class)
input_par.setIsParameter()
input_par.setType(ROS_TimerEvent(self.associated_class))
input_par.setName("")
self.publisherCallback.addInputParameter(input_par)
# Aggiungo la chiamata alla funzione custome
if self.source_text_function:
code = "{}.publish({});".format(var_publisher_pub.name,
self.source_text_function.generateInlineCode())
self.publisherCallback.addMiddleCode(code)
self.associated_class.addPrivateMethod(self.publisherCallback)
main_thread.prepare.addMiddleCode("{} = handle.advertise < {} > (\"{}\", 10);"
.format(var_publisher_pub.name, self.output_type.generateCode(), self.topic))
main_thread.prepare.addMiddleCode("{} = handle.createTimer(ros::Duration({}), {}, this);"
.format(var_timer_pub.name, self.period_in_seconds,
self.publisherCallback.getThreadPointer()))
return True, ""