Implement ROS1-ROS2 bridges generator

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<EObject> 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 <Components> getRos1Nodes (System rossystem) {
+        val nodeList = new ArrayList<RosNode>
+        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 <Systems> getSubsystems (System rossystem) {
             val subSystemsList = new ArrayList<System>
@@ -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<String> 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<String> 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<String> 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<String> getAllRepos(System system) {
         RepoList = new ArrayList<String>()

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;

plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/PackageXmlCompiler.xtend

@@ -49,8 +49,7 @@ class PackageXmlCompiler{
   <exec_depend>launch</exec_depend>
   «FOR pkg:system.getPkgsDependencies»
   <exec_depend>«pkg»</exec_depend>
-  «ENDFOR»
-
+  «ENDFOR»«IF TopicBridgeGenerated(system) || ServiceFromBridgeGenerated(system) || ServiceToBridgeGenerated(system)»<exec_depend>ros1_bridge</exec_depend>«ENDIF»
   <!--test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

plugins/de.fraunhofer.ipa.rossystem.xtext/src/de/fraunhofer/ipa/rossystem/generator/Ros1BridgesYamlFileCompiler.xtend

@@ -0,0 +1,26 @@
+package de.fraunhofer.ipa.rossystem.generator
+
+import com.google.inject.Inject
+import system.System
+import system.RosSystemConnection
+
+class Ros1BridgesYamlFileCompiler {
+
+    @Inject extension GeneratorHelpers
+
+    def compile_ROS1bridges_config(System system)'''«IF TopicBridgeGenerated(system)»
+bridge_«system.name»_topics/topics:«FOR connection:system.connections»«IF !getTopicBridgeInterfaces(connection as RosSystemConnection).get(0).empty»
+  - topic: «getTopicBridgeInterfaces(connection as RosSystemConnection).get(0)»
+    type: «getTopicBridgeInterfaces(connection as RosSystemConnection).get(1)»
+«ENDIF»«ENDFOR»«ENDIF»«IF ServiceFromBridgeGenerated(system)»
+bridge_«system.name»_from_services/services_2_to_1:«FOR connection:system.connections»«IF !getServiceFromBridgeInterfaces(connection as RosSystemConnection).get(0).empty»
+  - service: «getServiceFromBridgeInterfaces(connection as RosSystemConnection).get(0)»
+    type: «getServiceFromBridgeInterfaces(connection as RosSystemConnection).get(1)»
+«ENDIF»«ENDFOR»«ENDIF»«IF ServiceToBridgeGenerated(system)»
+bridge_«system.name»_to_services/services_2_to_1:«FOR connection:system.connections»«IF !getServiceToBridgeInterfaces(connection as RosSystemConnection).get(0).empty»
+  - service: «getServiceToBridgeInterfaces(connection as RosSystemConnection).get(0)»
+    type: «getServiceToBridgeInterfaces(connection as RosSystemConnection).get(1)»
+«ENDIF»«ENDFOR»«ENDIF»
+    '''
+
+}