From a10dbd8f63566e4d1fa41ba14535088a78cdfd1a Mon Sep 17 00:00:00 2001 From: ipa-nhg Date: Tue, 14 May 2024 14:03:03 +0200 Subject: [PATCH] Implement ROS1-ROS2 bridges generator --- .../generator/GeneratorHelpers.xtend | 140 ++++++++++++++- .../generator/LaunchFileCompiler_ROS2.xtend | 165 +++++++++++------- .../generator/PackageXmlCompiler.xtend | 3 +- .../Ros1BridgesYamlFileCompiler.xtend | 26 +++ .../generator/RosSystemGenerator.xtend | 9 + 5 files changed, 273 insertions(+), 70 deletions(-) create mode 100644 plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/Ros1BridgesYamlFileCompiler.xtend diff --git a/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/GeneratorHelpers.xtend b/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/GeneratorHelpers.xtend index 04721360..3cd5d515 100644 --- a/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/GeneratorHelpers.xtend +++ b/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/GeneratorHelpers.xtend @@ -16,6 +16,16 @@ import ros.impl.ParameterStructMemberImpl import system.RosNode import system.SubSystem import system.System +import system.RosInterface +import system.RosSystemConnection +import system.RosPublisherReference +import system.RosServiceServerReference +import system.RosActionServerReference +import system.RosSubscriberReference +import system.RosServiceClientReference +import system.RosActionClientReference +import org.eclipse.emf.ecore.EObject +import java.util.Arrays class GeneratorHelpers { @@ -29,23 +39,36 @@ class GeneratorHelpers { RosNode node String[] FromFileInfo Boolean os_import - + + String ros1_bridge_name + String ros1_bridge_type + List Ros1Ports + def void init_pkg(){ PackageSet=false } + def boolean generate_yaml(RosNode component){ + var yaml_gen=false + for(param:component.rosparameters){ + if(param.eContents.get(0).eClass.name.contains("ParameterStruct")){ + yaml_gen=true + } + } + if(component.rosparameters.length>5){ + yaml_gen=true + } + return yaml_gen + } + def boolean YamlFileGenerated(System rossystem) { os_import=false for (component: getRos2Nodes(rossystem)){ - for(param:component.rosparameters){ - if(param.eContents.get(0).eClass.name.contains("ParameterStruct")){ - os_import=true - } - } - if(component.rosparameters.length>5){ - os_import=true - } + os_import=generate_yaml(component) + } + if (TopicBridgeGenerated(rossystem) || ServiceFromBridgeGenerated(rossystem) || ServiceToBridgeGenerated(rossystem)){ + os_import=true } return os_import } @@ -62,6 +85,19 @@ class GeneratorHelpers { }} return nodeList } + + def getRos1Nodes (System rossystem) { + val nodeList = new ArrayList + if (!rossystem.components.nullOrEmpty){ + for (component: rossystem.components) { + if (component.class.toString.contains("RosNode")){ + if((component as RosNode).from.eContainer.eContainer.class.toString.contains("Catkin")){ + nodeList.add(component as RosNode) + } + } + }} + return nodeList + } def getSubsystems (System rossystem) { val subSystemsList = new ArrayList @@ -72,6 +108,92 @@ class GeneratorHelpers { } return subSystemsList } + + def boolean TopicBridgeGenerated(System rossystem){ + for (connection: rossystem.connections){ + if (!getTopicBridgeInterfaces(connection as RosSystemConnection).get(0).empty){ + return true + } + } + return false + } + + def boolean ServiceFromBridgeGenerated(System rossystem){ + for (connection: rossystem.connections){ + if (!getServiceFromBridgeInterfaces(connection as RosSystemConnection).get(0).empty){ + return true + } + } + return false + } + + def boolean ServiceToBridgeGenerated(System rossystem){ + for (connection: rossystem.connections){ + if (!getServiceToBridgeInterfaces(connection as RosSystemConnection).get(0).empty){ + return true + } + } + return false + } + + def List getTopicBridgeInterfaces(RosSystemConnection connection){ + val from_connection=(connection as RosSystemConnection).from + val to_connection=(connection as RosSystemConnection).to + ros1_bridge_name="" + ros1_bridge_type="" + if (from_connection.reference.eClass.name=='RosPublisherReference'){ + var bridge_interface = (from_connection.reference as RosPublisherReference).from + if (bridge_interface.eContainer.eContainer.eContainer.eClass.toString.contains("CatkinPackage")){ + ros1_bridge_name=bridge_interface.name + ros1_bridge_type=bridge_interface.message.fullname.replace("/","/msg/") + } + } + if (to_connection.reference.eClass.name=='RosSubscriberReference'){ + val bridge_interface = (to_connection.reference as RosSubscriberReference).from + if (bridge_interface.eContainer.eContainer.eContainer.eClass.toString.contains("CatkinPackage")){ + ros1_bridge_name=bridge_interface.name + ros1_bridge_type=bridge_interface.message.fullname.replace("/","/msg/") + } + } + return Arrays.asList(ros1_bridge_name, ros1_bridge_type); + } + + def List getServiceFromBridgeInterfaces(RosSystemConnection connection){ + val from_connection=(connection as RosSystemConnection).from + ros1_bridge_name="" + ros1_bridge_type="" + if (from_connection.reference.eClass.name=='RosServiceServerReference'){ + val bridge_interface = (from_connection.reference as RosServiceServerReference).from + if (bridge_interface.eContainer.eContainer.eContainer.eClass.toString.contains("CatkinPackage")){ + ros1_bridge_name=bridge_interface.name + ros1_bridge_type=bridge_interface.service.fullname + } + } + return Arrays.asList(ros1_bridge_name, ros1_bridge_type); + } + + def List getServiceToBridgeInterfaces(RosSystemConnection connection){ + val to_connection=(connection as RosSystemConnection).to + ros1_bridge_name="" + ros1_bridge_type="" + if (to_connection.reference.eClass.name=='RosServiceClientReference'){ + val bridge_interface = (to_connection.reference as RosServiceClientReference).from + if (bridge_interface.eContainer.eContainer.eContainer.eClass.toString.contains("CatkinPackage")){ + ros1_bridge_name=bridge_interface.name + ros1_bridge_type=bridge_interface.service.fullname + } + } + return Arrays.asList(ros1_bridge_name, ros1_bridge_type); + } + + + def boolean fromRos1Node(EObject bridge_interface){ + if (bridge_interface.eContainer.eContainer.eContainer.eClass.toString.contains("CatkinPackage")){ + Ros1Ports.add(bridge_interface) + return true + } + return false + } def ArrayList getAllRepos(System system) { RepoList = new ArrayList() diff --git a/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/LaunchFileCompiler_ROS2.xtend b/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/LaunchFileCompiler_ROS2.xtend index 3303a0bb..9e13b279 100644 --- a/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/LaunchFileCompiler_ROS2.xtend +++ b/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/LaunchFileCompiler_ROS2.xtend @@ -39,71 +39,118 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression, PathJoinSubstitution, TextSubstitution def generate_launch_description(): - ld = LaunchDescription() + ld = LaunchDescription() + + # *** PARAMETERS *** + «FOR component:getRos2Nodes(system)»«IF generate_yaml(component)» + «component.name»_config = os.path.join( + get_package_share_directory('«system.getName().toLowerCase»'), + 'config', + '«component.name».yaml' + ) + «ELSE» + «FOR parameter:component.rosparameters» + «parameter.name»_arg = DeclareLaunchArgument( + "«parameter.name»", default_value=TextSubstitution(text="«get_param_value(parameter.value,parameter.name)»") + ) + ld.add_action(«parameter.name»_arg) + «ENDFOR» + «ENDIF» + «ENDFOR» - «FOR component:getRos2Nodes(system)»«IF generate_yaml(component)» - «component.name»_config = os.path.join( - get_package_share_directory('«system.getName().toLowerCase»'), - 'config', - '«component.name».yaml' - ) - «ELSE» - «FOR parameter:component.rosparameters» - «parameter.name»_arg = DeclareLaunchArgument( - "«parameter.name»", default_value=TextSubstitution(text="«get_param_value(parameter.value,parameter.name)»") - ) - ld.add_action(«parameter.name»_arg) - «ENDFOR» - «ENDIF» - «ENDFOR» + # *** ROS 2 nodes *** + «FOR component:getRos2Nodes(system)» + «(component as RosNode).name» = Node( + package="«((component as RosNode).from.eContainer.eContainer as AmentPackageImpl).name»",«IF !component.namespace.nullOrEmpty» + namespace="«component.namespace»",«ENDIF» + executable="«((component as RosNode).from.eContainer as Artifact).name»", + prefix = 'xterm -e', + output='screen', + name="«(component as RosNode).name»"«compile_remappings_str(component as RosNode, system.connections)»«IF !component.rosparameters.nullOrEmpty»«IF generate_yaml(component)», + parameters = [«component.name»_config]«ELSE», + parameters=[{«FOR param:component.rosparameters» + "«param.from.name»": LaunchConfiguration("«param.name»"),«ENDFOR»}]«ENDIF»«ENDIF» + ) + «ENDFOR» - «FOR component:getRos2Nodes(system)» - «(component as RosNode).name» = Node( - package="«((component as RosNode).from.eContainer.eContainer as AmentPackageImpl).name»",«IF !component.namespace.nullOrEmpty» - namespace="«component.namespace»",«ENDIF» - executable="«((component as RosNode).from.eContainer as Artifact).name»", - prefix = 'xterm -e', - output='screen', - name="«(component as RosNode).name»"«compile_remappings_str(component as RosNode, system.connections)»«IF !component.rosparameters.nullOrEmpty», -«IF generate_yaml(component)» parameters = [«component.name»_config] -«ELSE» parameters=[{«FOR param:component.rosparameters» - "«param.from.name»": LaunchConfiguration("«param.name»"),«ENDFOR»}]«ENDIF»«ENDIF» - ) - «ENDFOR» - «FOR subsystem:getSubsystems(system)» - «IF subsystem.fromFile.nullOrEmpty» - include_«subsystem.name»= IncludeLaunchDescription( - PythonLaunchDescriptionSource([ get_package_share_directory('«subsystem.name»') + '/launch/«subsystem.name».launch.py']) - ) - «ELSE» - include_«subsystem.name»= IncludeLaunchDescription( - PythonLaunchDescriptionSource([get_package_share_directory('«subsystem.fromFile.split("/",2).get(0)»') + '/«subsystem.fromFile.split("/",2).get(1)»']) - ) - «ENDIF» - «ENDFOR» + # *** ROS 2 subsystems (include launch files)*** + «FOR subsystem:getSubsystems(system)» + «IF subsystem.fromFile.nullOrEmpty» + include_«subsystem.name»= IncludeLaunchDescription( + PythonLaunchDescriptionSource([ get_package_share_directory('«subsystem.name»') + '/launch/«subsystem.name».launch.py']) + ) + «ELSE» + include_«subsystem.name»= IncludeLaunchDescription( + PythonLaunchDescriptionSource([get_package_share_directory('«subsystem.fromFile.split("/",2).get(0)»') + '/«subsystem.fromFile.split("/",2).get(1)»']) + ) + «ENDIF» + + # *** ROS 1 to ROS 2 bridges *** + «ENDFOR»«IF TopicBridgeGenerated(system) || ServiceFromBridgeGenerated(system)|| ServiceToBridgeGenerated(system) » + «system.name»_ros1_bridge_config = os.path.join( + get_package_share_directory('«system.getName().toLowerCase»'), + 'config', + 'ros1_bridges.yaml' + )«ENDIF»«IF TopicBridgeGenerated(system)» + ros1_topic_bridge_parameter_bridge = Node( + package='ros1_bridge', + executable='parameter_bridge', + name='ros1_topic_bridge_parameter_bridge', + namespace='bridge_«system.name»_topic', + output='screen', + parameters=[ + {'__ns': 'bridge_«system.name»_topic'}, + {'__name': 'bridge_«system.name»_topic'} + ], + arguments=[ + '--ros-args', '-r', '__name:=bridge_«system.name»_topic' + ] + )«ENDIF»«IF ServiceFromBridgeGenerated(system)» + ros1_service_from_bridge_parameter_bridge = Node( + package='ros1_bridge', + executable='parameter_bridge', + name='ros1_service_from_bridge_parameter_bridge', + namespace='bridge_«system.name»_from_services', + output='screen', + parameters=[ + {'__ns': 'bridge_«system.name»_from_services'}, + {'__name': 'bridge_«system.name»_from_services'} + ], + arguments=[ + '--ros-args', '-r', '__name:=bridge_«system.name»_from_services' + ] + )«ENDIF»«IF ServiceToBridgeGenerated(system)» + ros1_service_to_bridge_parameter_bridge = Node( + package='ros1_bridge', + executable='parameter_bridge', + name='ros1_service_to_bridge_parameter_bridge', + namespace='bridge_«system.name»_to_services', + output='screen', + parameters=[ + {'__ns': 'bridge_«system.name»_to_services'}, + {'__name': 'bridge_«system.name»_to_services'} + ], + arguments=[ + '--ros-args', '-r', '__name:=bridge_«system.name»_to_services' + ] + )«ENDIF» - «FOR component:getRos2Nodes(system)» - ld.add_action(«(component as RosNode).name») - «ENDFOR»«FOR subsystem:getSubsystems(system)» - ld.add_action(include_«subsystem.name») - «ENDFOR» + # *** Add actions *** + «FOR component:getRos2Nodes(system)» + ld.add_action(«(component as RosNode).name») + «ENDFOR»«FOR subsystem:getSubsystems(system)» + ld.add_action(include_«subsystem.name») + «ENDFOR»«IF TopicBridgeGenerated(system)» + ld.add_action(ros1_topic_bridge_parameter_bridge) + «ENDIF»«IF ServiceFromBridgeGenerated(system)» + ld.add_action(ros1_service_from_bridge_parameter_bridge) + «ENDIF»«IF ServiceToBridgeGenerated(system)» + ld.add_action(ros1_service_to_bridge_parameter_bridge) + «ENDIF» - return ld + return ld ''' - def Boolean generate_yaml(RosNode component){ - var yaml_gen=false - for(param:component.rosparameters){ - if(param.eContents.get(0).eClass.name.contains("ParameterStruct")){ - yaml_gen=true - } - } - if(component.rosparameters.length>5){ - yaml_gen=true - } - return yaml_gen - } - // def void compile_list_of_ROS2components(RosSystem system, ComponentStack stack) { // components_tmp_.clear; // Ros2components.clear; diff --git a/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/PackageXmlCompiler.xtend b/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/PackageXmlCompiler.xtend index 46be8ec0..d5b8f413 100644 --- a/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/PackageXmlCompiler.xtend +++ b/plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/PackageXmlCompiler.xtend @@ -49,8 +49,7 @@ class PackageXmlCompiler{ launch «FOR pkg:system.getPkgsDependencies» «pkg» - «ENDFOR» - + «ENDFOR»«IF TopicBridgeGenerated(system) || ServiceFromBridgeGenerated(system) || ServiceToBridgeGenerated(system)»ros1_bridge«ENDIF»