diff --git a/jvm/src/.DS_Store b/jvm/src/.DS_Store new file mode 100644 index 00000000..cd38025c Binary files /dev/null and b/jvm/src/.DS_Store differ diff --git a/jvm/src/test b/jvm/src/test index b4d7d40c..c9ef61df 160000 --- a/jvm/src/test +++ b/jvm/src/test @@ -1 +1 @@ -Subproject commit b4d7d40c07724bd222576f0c362f6c40b53e64b3 +Subproject commit c9ef61df144a2e68824d85eb7924bcbb6ae217f0 diff --git a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Generator.scala b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Generator.scala index b05d018c..fe3738aa 100644 --- a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Generator.scala +++ b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Generator.scala @@ -15,12 +15,6 @@ object Generator { val toolName: String = "Ros2Codegen" val node_executable_filename_suffix: String = "_exe" - val launch_node_decl_suffix: String = "_node" - val py_launch_file_name_suffix: String = ".launch.py" - val xml_launch_file_name_suffix: String = ".launch.xml" - val py_package_name_suffix: String = "_py_pkg" - val py_src_node_name_suffix: String = "_src.py" - val py_src_node_entry_point_name: String = "main" val cpp_package_name_suffix: String = "_cpp_pkg" val cpp_src_node_name_suffix: String = "_src.cpp" val cpp_src_node_header_name_suffix: String = "_src.hpp" @@ -33,37 +27,12 @@ object Generator { // Mutex is used for thread locking in C++ val mutex_name: String = "mutex_" - - def genPyLaunchFileName(compNameS: String): String = { - // create launch file name - val nodeNameT: String = s"${compNameS}${py_launch_file_name_suffix}" - return nodeNameT - } - - def genXmlLaunchFileName(compNameS: String): String = { - // create launch file name - val nodeNameT: String = s"${compNameS}${xml_launch_file_name_suffix}" - return nodeNameT - } - def genCppPackageName(packageNameS: String): String = { // create target package name val packageNameT: String = s"${packageNameS}${cpp_package_name_suffix}" return packageNameT } - def genPyPackageName(packageNameS: String): String = { - // create target package name - val packageNameT: String = s"${packageNameS}${py_package_name_suffix}" - return packageNameT - } - - def genPyNodeSourceName(compNameS: String): String = { - // create target node name - val nodeNameT: String = s"${compNameS}${py_src_node_name_suffix}" - return nodeNameT - } - def genCppNodeSourceName(compNameS: String): String = { // create node file name val nodeNameT: String = s"${compNameS}${cpp_src_node_name_suffix}" @@ -179,86 +148,6 @@ object Generator { return s"${prefix}${msg}" } - def seqToString(seq: ISZ[String], separator: String): String = { - var str = "" - for (s <- seq) { - str = s"$str$s$separator" - } - str = ops.StringOps(str).substring(0, str.size - 1) - return str - } - - //================================================ - // Setup file for node source package (Python) - // Example: https://github.com/santoslab/ros-examples/blob/main/tempControl_ws/src/tc_py_pkg/setup.py - //================================================ - - // genPySetupEntryPointDecl - generates entry point declaration - // (console scripts entry) in setup file - - // Example: - // "ts_exe = tc_py_pkg.ts_src:main" - def genPySetupEntryPointDecl(modelName: String, - componentName: String): ST = { - val node_source_file_nameT = genPyNodeSourceName(componentName) - val py_package_nameT = genPyPackageName(modelName) - val node_executable_file_nameT = genExecutableFileName(componentName) - val entryPointDecl:ST - = st"\"${node_executable_file_nameT} = ${py_package_nameT}.${node_source_file_nameT}:${py_src_node_entry_point_name}\"" - return entryPointDecl - } - - // Setup file for node source package - // Example: https://github.com/santoslab/ros-examples/blob/main/tempControl_ws/src/tc_py_pkg/setup.py - def genPySetupFile(modelName: String, threadComponents: ISZ[AadlThread]): (ISZ[String], ST) = { - val top_level_package_nameT: String = genPyPackageName(modelName) - val fileName: String = "setup.py" - - // build entry point declarations - var entry_point_decls: ISZ[ST] = IS() - for (comp <- threadComponents) { - val launch_node_decl_nameT = genPyFormatLaunchNodeDeclName(genNodeName(comp)) - entry_point_decls = - entry_point_decls :+ genPySetupEntryPointDecl(modelName, genNodeName(comp)) - } - - val setupFileBody = - st"""# ${fileName} in src/${top_level_package_nameT} - | - |from setuptools import find_packages, setup - | - |package_name = '${top_level_package_nameT}' - | - |setup( - | name=package_name, - | version='0.0.0', - | packages=find_packages(exclude=['test']), - | data_files=[ - | ('share/ament_index/resource_index/packages', - | ['resource/' + package_name]), - | ('share/' + package_name, ['package.xml']), - | ], - | install_requires=['setuptools'], - | zip_safe=True, - | maintainer='sireum', - | maintainer_email='sireum@todo.todo', - | description='TODO: Package description', - | license='TODO: License declaration', - | tests_require=['pytest'], - | entry_points={ - | 'console_scripts': [ - | ${(entry_point_decls, ",\n")} - | ], - | }, - |) - """ - - val filePath: ISZ[String] = IS("src", top_level_package_nameT, fileName) - - return (filePath, setupFileBody) - } - - //================================================ // Setup files for node source package (C++) // Example: https://github.com/santoslab/ros-examples/blob/main/tempControlcpp_ws/src/tc_cpp_pkg/CMakeLists.txt @@ -386,325 +275,6 @@ object Generator { return (filePath, setupFileBody, F, IS(Marker(startMarker, endMarker))) } - - //================================================ - // L a u n c h File Setup Files - //================================================ - - def genLaunchCMakeListsFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { - val top_level_package_nameT: String = genCppPackageName(modelName) - val fileName: String = "CMakeLists.txt" - - val setupFileBody = - st"""cmake_minimum_required(VERSION 3.8) - |project(${top_level_package_nameT}_bringup) - | - |if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - | add_compile_options(-Wall -Wextra -Wpedantic) - |endif() - | - |find_package(ament_cmake REQUIRED) - | - |install(DIRECTORY - | launch - | DESTINATION share/$${PROJECT_NAME} - |) - | - |ament_package() - """ - - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", fileName) - - return (filePath, setupFileBody, T, IS()) - } - - def genLaunchPackageFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { - val top_level_package_nameT: String = genCppPackageName(modelName) - val fileName: String = "package.xml" - - val startMarker: String = "" - val endMarker: String = "" - - val setupFileBody = - st""" - | - | - | ${top_level_package_nameT}_bringup - | 0.0.0 - | TODO: Package description - | sireum - | TODO: License declaration - | - | ament_cmake - | - | ${top_level_package_nameT} - | - | ${startMarker} - | - | ${endMarker} - | - | ament_lint_auto - | ament_lint_common - | - | - | ament_cmake - | - | - """ - - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", fileName) - - return (filePath, setupFileBody, F, IS(Marker(startMarker, endMarker))) - } - - - //================================================ - // L a u n c h File (Python Format) - //================================================ - - // Example: - // tc_node = Node( ## Example is "tc_node" python variable name - // package="tc_cpp_pkg", - // executable="tc_exe" - // ) - def genPyFormatLaunchNodeDeclName(componentNameS: String): String = { - // create target launch node decl name - val launch_node_decl_nameT: String = s"${componentNameS}${launch_node_decl_suffix}" - return launch_node_decl_nameT - } - - // genLaunchNodeDecl() - generate node declaration - // Example: - // tc_node = Node( - // package="tc_cpp_pkg", - // executable="tc_exe" - // ) - def genPyFormatLaunchNodeDecl(launch_node_decl_nameT: String, - top_level_package_nameT: String, - component: AadlThread): ST = { - val node_executable_file_nameT = genExecutableFileName(genNodeName(component)) - val s = - st""" - |${launch_node_decl_nameT} = Node( - | package = ${top_level_package_nameT}, - | executable = ${node_executable_file_nameT} - | ) - """ - return s - } - - // Example: - // ld.add_action(tc_node) - def genPyFormatLaunchAddAction(launch_node_decl_nameT: String): ST = { - val s = st"""ld.add_action(${launch_node_decl_nameT})""" - return s - } - - // For example, see https://github.com/santoslab/ros-examples/blob/main/tempControl_ws/src/tc_bringup/launch/tc.launch.py - def genPyFormatLaunchFile(modelName: String, threadComponents: ISZ[AadlThread]): (ISZ[String], ST) = { - val fileName = genPyLaunchFileName(modelName) - - val top_level_package_nameT: String = genPyPackageName(modelName) - - var node_decls: ISZ[ST] = IS() - var ld_entries: ISZ[ST] = IS() - - for (comp <- threadComponents) { - val launch_node_decl_nameT = genPyFormatLaunchNodeDeclName(genNodeName(comp)) - node_decls = node_decls :+ genPyFormatLaunchNodeDecl(launch_node_decl_nameT, top_level_package_nameT, comp) - ld_entries = ld_entries :+ genPyFormatLaunchAddAction(launch_node_decl_nameT) - } - - val launchFileBody = - st"""from launch import LaunchDescription - |from launch_ros.actions import Node - | - |def generate_launch_description(): - | ld = LaunchDescription() - | - | ${(node_decls, "\n")} - | ${(ld_entries, "\n")} - | - | return ld - """ - - val filePath: ISZ[String] = IS("src", s"${modelName}_bringup", "launch", fileName) - - return (filePath, launchFileBody) - } - - //================================================ - // L a u n c h File (XML Format) - //================================================ - - // Generate node launch code - // Example: - // - def genXmlFormatLaunchNodeDecl(top_level_package_nameT: String, - thread: AadlThread): ST = { - val node_executable_file_nameT = genExecutableFileName(genNodeName(thread)) - val s = - st""" - | - | - """ - return s - } - - // Generate system launch code (including a system launch file) - // Example: - // - def genXmlFormatLaunchSystemDecl(top_level_package_nameT: String, - system: AadlSystem): ST = { - val launchFileName: String = genXmlLaunchFileName(system.identifier) - val s = - st""" - | - """ - return s - } - - def genXmlFormatLaunchDecls(component: AadlComponent, packageName: String): ISZ[ST] = { - var launch_decls: ISZ[ST] = IS() - - for (comp <- component.subComponents) { - comp match { - case thread: AadlThread => - launch_decls = launch_decls :+ genXmlFormatLaunchNodeDecl(packageName, thread) - case system: AadlSystem => - launch_decls = launch_decls :+ genXmlFormatLaunchSystemDecl(packageName, system) - case process: AadlProcess => - launch_decls = launch_decls ++ genXmlFormatLaunchDecls(process, packageName) - case _ => - } - } - - return launch_decls - } - - // For example, see https://github.com/santoslab/ros-examples/blob/main/tempControl_ws/src/tc_bringup/launch/tc.launch.py - // Creates a launch file for each system component in the model - def genXmlFormatLaunchFiles(modelName: String, threadComponents: ISZ[AadlThread], - systemComponents: ISZ[AadlSystem]): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { - val top_level_package_nameT: String = genCppPackageName(modelName) - - var launchFiles: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() - - for (system <- systemComponents) { - val fileName = genXmlLaunchFileName(system.identifier) - - val launch_decls: ISZ[ST] = genXmlFormatLaunchDecls(system, top_level_package_nameT) - - val launchFileBody = - st""" - | ${(launch_decls, "\n")} - | - """ - - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", "launch", fileName) - - launchFiles = launchFiles :+ (filePath, launchFileBody, T, IS()) - } - - return launchFiles - } - - - //================================================ - // I n t e r f a c e s Setup Files - //================================================ - // ROS2 data/message types are defined in a "{package_name}_interfaces" package according to convention - // The "Empty" datatype, which has no data fields, is used for event ports - - def genMsgFiles(modelName: String, datatypeMap: Map[AadlType, (String, ISZ[String])]): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { - var msg_files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() - for (datatype <- datatypeMap.entries) { - msg_files = msg_files :+ genMsgFile(modelName, datatype._2._1, datatype._2._2) - } - msg_files = msg_files :+ (ISZ("src", s"${genCppPackageName(modelName)}_interfaces", "msg", "Empty.msg"), st"", T, IS()) - return msg_files - } - - def genMsgFile(modelName: String, datatypeName: String, datatypeContent: ISZ[String]): (ISZ[String], ST, B, ISZ[Marker]) = { - val top_level_package_nameT: String = genCppPackageName(modelName) - - val fileBody = st"${(datatypeContent, "\n")}" - - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_interfaces", "msg", s"${datatypeName}.msg") - - return (filePath, fileBody, T, IS()) - } - - def genInterfacesCMakeListsFile(modelName: String, datatypeMap: Map[AadlType, (String, ISZ[String])]): (ISZ[String], ST, B, ISZ[Marker]) = { - val top_level_package_nameT: String = genCppPackageName(modelName) - val fileName: String = "CMakeLists.txt" - var msgTypes: ISZ[String] = IS() - for (msg <- datatypeMap.valueSet.elements) { - msgTypes = msgTypes :+ s"msg/${msg._1}.msg" - } - msgTypes = msgTypes :+ s"msg/Empty.msg" - - val setupFileBody = - st"""cmake_minimum_required(VERSION 3.8) - |project(${top_level_package_nameT}_interfaces) - | - |if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - | add_compile_options(-Wall -Wextra -Wpedantic) - |endif() - | - |find_package(ament_cmake REQUIRED) - | - |find_package(rosidl_default_generators REQUIRED) - | - |rosidl_generate_interfaces($${PROJECT_NAME} - | ${(msgTypes, "\n")} - |) - | - |ament_export_dependencies(rosidl_default_runtime) - | - |ament_package() - """ - - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_interfaces", fileName) - - return (filePath, setupFileBody, T, IS()) - } - - def genInterfacesPackageFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { - val top_level_package_nameT: String = genCppPackageName(modelName) - val fileName: String = "package.xml" - - val setupFileBody = - st""" - | - | - | ${top_level_package_nameT}_interfaces - | 0.0.0 - | TODO: Package description - | sireum - | TODO: License declaration - | - | ament_cmake - | - | rosidl_default_generators - | rosidl_default_runtime - | rosidl_interface_packages - | - | ament_lint_auto - | ament_lint_common - | - | - | ament_cmake - | - | - """ - - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_interfaces", fileName) - - return (filePath, setupFileBody, T, IS()) - } - - //================================================ // Node files (C++) // Example: https://github.com/santoslab/ros-examples/tree/main/tempControlcpp_ws/src/tc_cpp_pkg/src @@ -2778,27 +2348,6 @@ object Generator { //================================================ // P a c k a g e G e n e r a t o r s //================================================ - - def genPyNodePkg(modelName: String, threadComponents: ISZ[AadlThread], connectionMap: Map[ISZ[String], ISZ[ISZ[String]]], - strictAADLMode: B): ISZ[(ISZ[String], ST)] = { - var files: ISZ[(ISZ[String], ST)] = IS() - - files = files :+ genPyFormatLaunchFile(modelName, threadComponents) - files = files :+ genPySetupFile(modelName, threadComponents) - - return files - } - - def genPyLaunchPkg(modelName: String, threadComponents: ISZ[AadlThread]): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { - var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() - - //files = files :+ genXmlFormatLaunchFile(modelName, threadComponents) - files = files :+ genLaunchCMakeListsFile(modelName) - files = files :+ genLaunchPackageFile(modelName) - - return files - } - def genCppNodePkg(modelName: String, threadComponents: ISZ[AadlThread], connectionMap: Map[ISZ[String], ISZ[ISZ[String]]], datatypeMap: Map[AadlType, (String, ISZ[String])], strictAADLMode: B, invertTopicBinding: B, reporter: Reporter): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { @@ -2816,26 +2365,4 @@ object Generator { return files } - - def genXmlLaunchPkg(modelName: String, threadComponents: ISZ[AadlThread], systemComponents: ISZ[AadlSystem]): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { - var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() - - files = files ++ genXmlFormatLaunchFiles(modelName, threadComponents, systemComponents) - files = files :+ genLaunchCMakeListsFile(modelName) - files = files :+ genLaunchPackageFile(modelName) - - return files - } - - // The same datatype package will work regardless of other packages' types - // ROS2 data/message types are defined in a "{package_name}_interfaces" package according to convention - def genInterfacesPkg(modelName: String, datatypeMap: Map[AadlType, (String, ISZ[String])]): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { - var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() - - files = files ++ genMsgFiles(modelName, datatypeMap) - files = files :+ genInterfacesCMakeListsFile(modelName, datatypeMap) - files = files :+ genInterfacesPackageFile(modelName) - - return files - } } diff --git a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorInterfaces.scala b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorInterfaces.scala new file mode 100644 index 00000000..ab0f394f --- /dev/null +++ b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorInterfaces.scala @@ -0,0 +1,136 @@ +// #Sireum + +package org.sireum.hamr.codegen.ros2 + +import org.sireum._ +import org.sireum.hamr.codegen.common.containers.Marker +import org.sireum.hamr.codegen.common.types.AadlType + +object GeneratorInterfaces { + + val py_package_name_suffix: String = "_py_pkg" + val cpp_package_name_suffix: String = "_cpp_pkg" + + def genCppPackageName(packageNameS: String): String = { + // create target package name + val packageNameT: String = s"${packageNameS}${cpp_package_name_suffix}" + return packageNameT + } + + def genPyPackageName(packageNameS: String): String = { + // create target package name + val packageNameT: String = s"${packageNameS}${py_package_name_suffix}" + return packageNameT + } + + def genLaunchPackageType(modelName: String, launchType: String): String = { + launchType match { + case "Cpp" => return genCppPackageName(modelName) + case "Python" => return genPyPackageName(modelName) + } + } + + //================================================ + // I n t e r f a c e s Setup Files + //================================================ + // ROS2 data/message types are defined in a "{package_name}_interfaces" package according to convention + // The "Empty" datatype, which has no data fields, is used for event ports + + def genMsgFile(modelName: String, datatypeName: String, datatypeContent: ISZ[String], top_level_package_nameT: String): (ISZ[String], ST, B, ISZ[Marker]) = { + + val fileBody = st"${(datatypeContent, "\n")}" + + val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_interfaces", "msg", s"${datatypeName}.msg") + + return (filePath, fileBody, T, IS()) + } + + def genMsgFiles(modelName: String, datatypeMap: Map[AadlType, (String, ISZ[String])], top_level_package_nameT: String): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + var msg_files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + for (datatype <- datatypeMap.entries) { + msg_files = msg_files :+ genMsgFile(modelName, datatype._2._1, datatype._2._2, top_level_package_nameT) + } + msg_files = msg_files :+ (ISZ("src", s"${top_level_package_nameT}_interfaces", "msg", "Empty.msg"), st"", T, IS()) + return msg_files + } + + def genInterfacesCMakeListsFile(modelName: String, datatypeMap: Map[AadlType, (String, ISZ[String])], top_level_package_nameT: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val fileName: String = "CMakeLists.txt" + var msgTypes: ISZ[String] = IS() + for (msg <- datatypeMap.valueSet.elements) { + msgTypes = msgTypes :+ s"msg/${msg._1}.msg" + } + msgTypes = msgTypes :+ s"msg/Empty.msg" + + val setupFileBody = + st"""cmake_minimum_required(VERSION 3.8) + |project(${top_level_package_nameT}_interfaces) + | + |if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + | add_compile_options(-Wall -Wextra -Wpedantic) + |endif() + | + |find_package(ament_cmake REQUIRED) + | + |find_package(rosidl_default_generators REQUIRED) + | + |rosidl_generate_interfaces($${PROJECT_NAME} + | ${(msgTypes, "\n")} + |) + | + |ament_export_dependencies(rosidl_default_runtime) + | + |ament_package() + """ + + val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_interfaces", fileName) + + return (filePath, setupFileBody, T, IS()) + } + + def genInterfacesPackageFile(modelName: String, top_level_package_nameT: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val fileName: String = "package.xml" + + val setupFileBody = + st""" + | + | + | ${top_level_package_nameT}_interfaces + | 0.0.0 + | TODO: Package description + | sireum + | TODO: License declaration + | + | ament_cmake + | + | rosidl_default_generators + | rosidl_default_runtime + | rosidl_interface_packages + | + | ament_lint_auto + | ament_lint_common + | + | + | ament_cmake + | + | + """ + + val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_interfaces", fileName) + + return (filePath, setupFileBody, T, IS()) + } + + // The same datatype package will work regardless of other packages' types + // ROS2 data/message types are defined in a "{package_name}_interfaces" package according to convention + def genInterfacesPkg(modelName: String, datatypeMap: Map[AadlType, (String, ISZ[String])], nodeLanguage: String): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + val top_level_package_nameT: String = genLaunchPackageType(modelName, nodeLanguage) + var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + + files = files ++ genMsgFiles(modelName, datatypeMap, top_level_package_nameT) + files = files :+ genInterfacesCMakeListsFile(modelName, datatypeMap, top_level_package_nameT) + files = files :+ genInterfacesPackageFile(modelName, top_level_package_nameT) + + return files + } +} diff --git a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorLaunch.scala b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorLaunch.scala new file mode 100644 index 00000000..91dd2e44 --- /dev/null +++ b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorLaunch.scala @@ -0,0 +1,327 @@ +// #Sireum + +package org.sireum.hamr.codegen.ros2 + +import org.sireum._ +import org.sireum.hamr.codegen.common.containers.Marker +import org.sireum.hamr.codegen.common.symbols.{AadlComponent, AadlProcess, AadlSystem, AadlThread} + +object GeneratorLaunch { + + val node_executable_filename_suffix: String = "_exe" + val launch_node_decl_suffix: String = "_node" + val py_package_name_suffix: String = "_py_pkg" + val cpp_package_name_suffix: String = "_cpp_pkg" + val py_launch_file_name_suffix: String = ".launch.py" + val xml_launch_file_name_suffix: String = ".launch.xml" + + def genCppPackageName(packageNameS: String): String = { + // create target package name + val packageNameT: String = s"${packageNameS}${cpp_package_name_suffix}" + return packageNameT + } + + def genPyPackageName(packageNameS: String): String = { + // create target package name + val packageNameT: String = s"${packageNameS}${py_package_name_suffix}" + return packageNameT + } + + def genLaunchPackageType(modelName: String, launchType: String): String = { + launchType match { + case "Cpp" => return genCppPackageName(modelName) + case "Python" => return genPyPackageName(modelName) + } + } + + //================================================ + // L a u n c h File Setup Files + //================================================ + def genLaunchCMakeListsFile(top_level_package_nameT: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val fileName: String = "CMakeLists.txt" + + val setupFileBody = + st"""cmake_minimum_required(VERSION 3.8) + |project(${top_level_package_nameT}_bringup) + | + |if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + | add_compile_options(-Wall -Wextra -Wpedantic) + |endif() + | + |find_package(ament_cmake REQUIRED) + | + |install(DIRECTORY + | launch + | DESTINATION share/$${PROJECT_NAME} + |) + | + |ament_package() + """ + + val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", fileName) + + return (filePath, setupFileBody, T, IS()) + } + + def genLaunchPackageFile(top_level_package_nameT: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val fileName: String = "package.xml" + + val startMarker: String = "" + val endMarker: String = "" + + val setupFileBody = + st""" + | + | + | ${top_level_package_nameT}_bringup + | 0.0.0 + | TODO: Package description + | sireum + | TODO: License declaration + | + | ament_cmake + | + | ${top_level_package_nameT} + | + | ${startMarker} + | + | ${endMarker} + | + | ament_lint_auto + | ament_lint_common + | + | + | ament_cmake + | + | + """ + + val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", fileName) + + return (filePath, setupFileBody, F, IS(Marker(startMarker, endMarker))) + } + + //================================================ + // L a u n c h XML Files + //================================================ + + def genExecutableFileName(componentNameS: String): String = { + // create target executable name + val executableFileNameT: String = s"${componentNameS}${node_executable_filename_suffix}" + return executableFileNameT + } + + def genNodeName(component: AadlThread): String = { + var name: ST = st"" + var i: Z = 1 + while (i < component.path.size) { + name = st"${name}_${component.path.apply(i)}" + i = i + 1 + } + return ops.StringOps(name.render).substring(1, name.render.size) + } + + def genXmlFormatLaunchNodeDecl(top_level_package_nameT: String, + thread: AadlThread): ST = { + val node_executable_file_nameT = genExecutableFileName(genNodeName(thread)) + val s = + st""" + | + | + """ + return s + } + + def genXmlLaunchFileName(compNameS: String): String = { + // create launch file name + val nodeNameT: String = s"${compNameS}${xml_launch_file_name_suffix}" + return nodeNameT + } + + def genXmlFormatLaunchSystemDecl(top_level_package_nameT: String, + system: AadlSystem): ST = { + val launchFileName: String = genXmlLaunchFileName(system.identifier) + val s = + st""" + | + """ + return s + } + + def genXmlFormatLaunchDecls(component: AadlComponent, packageName: String): ISZ[ST] = { + var launch_decls: ISZ[ST] = IS() + + for (comp <- component.subComponents) { + comp match { + case thread: AadlThread => + launch_decls = launch_decls :+ genXmlFormatLaunchNodeDecl(packageName, thread) + case system: AadlSystem => + launch_decls = launch_decls :+ genXmlFormatLaunchSystemDecl(packageName, system) + case process: AadlProcess => + launch_decls = launch_decls ++ genXmlFormatLaunchDecls(process, packageName) + case _ => + } + } + + return launch_decls + } + + def genXmlFormatLaunchFiles(modelName: String, threadComponents: ISZ[AadlThread], + systemComponents: ISZ[AadlSystem], top_level_package_nameT: String): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + var launchFiles: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + + for (system <- systemComponents) { + val fileName = genXmlLaunchFileName(system.identifier) + + val launch_decls: ISZ[ST] = genXmlFormatLaunchDecls(system, top_level_package_nameT) + + val launchFileBody = + st""" + | ${(launch_decls, "\n")} + | + """ + + val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", "launch", fileName) + + launchFiles = launchFiles :+ (filePath, launchFileBody, T, IS()) + } + + return launchFiles + } + + def genXmlLaunchPkg(modelName: String, threadComponents: ISZ[AadlThread], systemComponents: ISZ[AadlSystem], launchType: String): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + val top_level_package_nameT: String = genLaunchPackageType(modelName, launchType) + var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + + files = files ++ genXmlFormatLaunchFiles(modelName, threadComponents, systemComponents, top_level_package_nameT) + files = files :+ genLaunchCMakeListsFile(top_level_package_nameT) + files = files :+ genLaunchPackageFile(top_level_package_nameT) + + return files + } + + //================================================ + // L a u n c h Python Files + //================================================ + def genPyLaunchFileName(compNameS: String): String = { + // create launch file name + val nodeNameT: String = s"${compNameS}${py_launch_file_name_suffix}" + return nodeNameT + } + + def genPyFormatLaunchNodeDeclName(componentNameS: String): String = { + // create target launch node decl name + val launch_node_decl_nameT: String = s"${componentNameS}${launch_node_decl_suffix}" + return launch_node_decl_nameT + } + + def genPyFormatLaunchNodeDecl(top_level_package_nameT: String, + thread: AadlThread): ST = { + val node_executable_file_nameT = genExecutableFileName(genNodeName(thread)) + val launch_node_decl_nameT = genPyFormatLaunchNodeDeclName(genNodeName(thread)) + val s = + st"""${launch_node_decl_nameT} = Node( + | package = "${top_level_package_nameT}", + | executable = "${node_executable_file_nameT}" + |) + """ + return s + } + + def genPyFormatLaunchSystemDecl(top_level_package_nameT: String, + system: AadlSystem): ST = { + val launch_node_decl_nameT = genPyFormatLaunchNodeDeclName(system.identifier) + val launchFileName: String = genPyLaunchFileName(system.identifier) + val s = + st"""${launch_node_decl_nameT} = IncludeLaunchDescription( + | PythonLaunchDescriptionSource( + | os.path.join(get_package_share_directory('${top_level_package_nameT}_bringup'), + | 'launch/${launchFileName}') + | ) + |) + """ + return s + } + + def genPyFormatLaunchDecls(component: AadlComponent, packageName: String): ISZ[ST] = { + var launch_decls: ISZ[ST] = IS() + + for (comp <- component.subComponents) { + comp match { + case thread: AadlThread => + launch_decls = launch_decls :+ genPyFormatLaunchNodeDecl(packageName, thread) + case system: AadlSystem => + launch_decls = launch_decls :+ genPyFormatLaunchSystemDecl(packageName, system) + case process: AadlProcess => + launch_decls = launch_decls ++ genPyFormatLaunchDecls(process, packageName) + case _ => + } + } + + return launch_decls + } + + def genPyFormatLaunchAddAction(component: AadlComponent): ISZ[ST] = { + var ld_entries: ISZ[ST] = IS() + + for(comp <- component.subComponents) { + comp match { + case thread: AadlThread => + ld_entries = ld_entries :+ st"""ld.add_action(${genPyFormatLaunchNodeDeclName(genNodeName(thread))})""" + case system: AadlSystem => + ld_entries = ld_entries :+ st"""ld.add_action(${genPyFormatLaunchNodeDeclName(system.identifier)})""" + case process: AadlProcess => + ld_entries = ld_entries ++ genPyFormatLaunchAddAction(process) + case _ => + } + } + + return ld_entries + } + + def genPyFormatLaunchFile(modelName: String, threadComponents: ISZ[AadlThread], + systemComponents: ISZ[AadlSystem], top_level_package_nameT: String): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + var launchFiles: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + + for (system <- systemComponents) { + val fileName = genPyLaunchFileName(system.identifier) + + val node_decls: ISZ[ST] = genPyFormatLaunchDecls(system, top_level_package_nameT) + val ld_entries: ISZ[ST] = genPyFormatLaunchAddAction(system) + + val launchFileBody = + st"""from launch import LaunchDescription + |from launch_ros.actions import Node + | + |import os + |from ament_index_python.packages import get_package_share_directory + |from launch.actions import IncludeLaunchDescription + |from launch.launch_description_sources import PythonLaunchDescriptionSource + | + |def generate_launch_description(): + | ld = LaunchDescription() + | + | ${(node_decls, "\n")} + | ${(ld_entries, "\n")} + | + | return ld + """ + + val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", "launch", fileName) + + launchFiles = launchFiles :+ (filePath, launchFileBody, T, IS()) + } + + return launchFiles + } + + def genPyLaunchPkg(modelName: String, threadComponents: ISZ[AadlThread], systemComponents: ISZ[AadlSystem], nodeLanguage: String): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + val top_level_package_nameT: String = genLaunchPackageType(modelName, nodeLanguage) + var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + files = files :+ genLaunchCMakeListsFile(top_level_package_nameT) + files = files :+ genLaunchPackageFile(top_level_package_nameT) + files = files ++ genPyFormatLaunchFile(modelName, threadComponents, systemComponents, top_level_package_nameT) + + return files + } +} diff --git a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorPy.scala b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorPy.scala index 2a31a2c1..473711df 100644 --- a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorPy.scala +++ b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/GeneratorPy.scala @@ -3,46 +3,41 @@ package org.sireum.hamr.codegen.ros2 import org.sireum._ -import org.sireum.hamr.codegen.common.symbols.{AadlDataPort, AadlPort, AadlThread, Dispatch_Protocol} +import org.sireum.hamr.codegen.common.containers.Marker +import org.sireum.hamr.codegen.common.symbols.{AadlComponent, AadlDataPort, AadlEventDataPort, AadlPort, AadlProcess, AadlSystem, AadlThread, Dispatch_Protocol} +import org.sireum.hamr.codegen.common.types.{AadlType, EnumType} import org.sireum.hamr.ir.Direction +import org.sireum.message.Reporter +import org.sireum.ops.{ISZOps, StringOps} object GeneratorPy { + + val toolName: String = "Ros2Codegen" + val node_executable_filename_suffix: String = "_exe" - val launch_node_decl_suffix: String = "_node" - val py_launch_file_name_suffix: String = ".launch.py" val py_package_name_suffix: String = "_py_pkg" val py_src_node_name_suffix: String = "_src.py" val py_src_node_entry_point_name: String = "main" val py_node_runner_name_suffix: String = "_runner.py" - // TODO: Reentrant or mutually exclusive (or single-threaded executor)? - // This value will work for Python and C++ code - val callback_group_type: String = "Reentrant" - // TODO: Confirm this name (and maybe remove based on how callback groups are done) + val callback_group_type: String = "ReentrantCallbackGroup" val callback_group_name: String = "cb_group_" - val subscription_options_name: String = "subscription_options_" - - def genPyLaunchFileName(compNameS: String): String = { - // create launch file name - val nodeNameT: String = s"$compNameS$py_launch_file_name_suffix" - return nodeNameT - } def genPyPackageName(packageNameS: String): String = { // create target package name - val packageNameT: String = s"$packageNameS$py_package_name_suffix" + val packageNameT: String = s"${packageNameS}${py_package_name_suffix}" return packageNameT } def genPyNodeSourceName(compNameS: String): String = { // create target node name - val nodeNameT: String = s"$compNameS$py_src_node_name_suffix" + val nodeNameT: String = s"${compNameS}${py_src_node_name_suffix}" return nodeNameT } def genExecutableFileName(componentNameS: String): String = { // create target executable name - val executableFileNameT: String = s"$componentNameS$node_executable_filename_suffix" + val executableFileNameT: String = s"${componentNameS}${node_executable_filename_suffix}" return executableFileNameT } @@ -50,21 +45,67 @@ object GeneratorPy { return component.dispatchProtocol == Dispatch_Protocol.Sporadic } + def isEventPort(portType: String): B = { + return ops.StringOps(portType).substring(0, portType.size) == "Empty" + } + + def genNodeName(component: AadlThread): String = { + var name: ST = st"" + var i: Z = 1 + while (i < component.path.size) { + name = st"${name}_${component.path.apply(i)}" + i = i + 1 + } + return ops.StringOps(name.render).substring(1, name.render.size) + } + + def genPortName(port: AadlPort): String = { + var name: ST = st"" + var i: Z = 1 + while (i < port.path.size) { + name = st"${name}_${port.path.apply(i)}" + i = i + 1 + } + return ops.StringOps(name.render).substring(1, name.render.size) + } + def getPortNames(portNames: ISZ[ISZ[String]]): ISZ[String] = { var names: ISZ[String] = IS() - for (name <- portNames) { - names = names :+ seqToString(name, "_") + for (portName <- portNames) { + var name: ST = st"" + var i: Z = 1 + while (i < portName.size) { + name = st"${name}_${portName.apply(i)}" + i = i + 1 + } + names = names :+ ops.StringOps(name.render).substring(1, name.render.size) } return names } - def seqToString(seq: ISZ[String], separator: String): String = { - var str = "" - for (s <- seq) { - str = s"$str$s$separator" + def genPortDatatype(port: AadlPort, packageName: String, datatypeMap: Map[AadlType, (String, ISZ[String])], reporter: Reporter): String = { + val s: String = port match { + case dp: AadlDataPort => + val dtype = datatypeMap.get(dp.aadlType) + if (dtype.nonEmpty) { + s"${dtype.get._1}" + } + else { + reporter.error(None(), toolName, s"Port ${port.identifier}: datatype unknown, setting datatype to Empty") + s"Empty" + } + case edp: AadlEventDataPort => + val dtype = datatypeMap.get(edp.aadlType) + if (dtype.nonEmpty) { + s"${dtype.get._1}" + } + else { + reporter.error(None(), toolName, s"Port ${port.identifier}: datatype unknown, setting datatype to Empty") + s"Empty" + } + case _ => s"Empty" } - str = ops.StringOps(str).substring(0, str.size - 1) - return str + return s } //================================================ @@ -79,34 +120,33 @@ object GeneratorPy { // "ts_exe = tc_py_pkg.ts_src:main" def genPySetupEntryPointDecl(modelName: String, componentName: String): ST = { - val node_source_file_nameT = genPyNodeSourceName(componentName) + val node_source_file_nameT = st"${componentName}_runner" val py_package_nameT = genPyPackageName(modelName) val node_executable_file_nameT = genExecutableFileName(componentName) val entryPointDecl: ST - = st"\"$node_executable_file_nameT = $py_package_nameT.$node_source_file_nameT:$py_src_node_entry_point_name\"" + = st"\"${node_executable_file_nameT} = ${py_package_nameT}.base_code.${node_source_file_nameT}:${py_src_node_entry_point_name}\"" return entryPointDecl } // Setup file for node source package // Example: https://github.com/santoslab/ros-examples/blob/main/tempControl_ws/src/tc_py_pkg/setup.py - def genPySetupFile(modelName: String, threadComponents: ISZ[AadlThread]): (ISZ[String], ST) = { + def genPySetupFile(modelName: String, threadComponents: ISZ[AadlThread]): (ISZ[String], ST, B, ISZ[Marker]) = { val top_level_package_nameT: String = genPyPackageName(modelName) val fileName: String = "setup.py" // build entry point declarations var entry_point_decls: ISZ[ST] = IS() for (comp <- threadComponents) { - val launch_node_decl_nameT = genPyFormatLaunchNodeDeclName(comp.pathAsString("_")) entry_point_decls = - entry_point_decls :+ genPySetupEntryPointDecl(modelName, comp.pathAsString("_")) + entry_point_decls :+ genPySetupEntryPointDecl(modelName, genNodeName(comp)) } val setupFileBody = - st"""# $fileName in src/$top_level_package_nameT + st"""# ${fileName} in src/${top_level_package_nameT} | |from setuptools import find_packages, setup | - |package_name = '$top_level_package_nameT' + |package_name = '${top_level_package_nameT}' | |setup( | name=package_name, @@ -134,163 +174,226 @@ object GeneratorPy { val filePath: ISZ[String] = IS("src", top_level_package_nameT, fileName) - return (filePath, setupFileBody) + return (filePath, setupFileBody, true, IS()) } def genPackageFilePkgDependencies(packages: ISZ[String]): ISZ[ST] = { var requirements: ISZ[ST] = IS() for (pkg <- packages) { - requirements = requirements :+ st"$pkg" + requirements = requirements :+ st"${pkg}" } return requirements } - //================================================ - // L a u n c h File Setup Files - //================================================ - - def genLaunchCMakeListsFile(modelName: String): (ISZ[String], ST) = { + // Setup file for node source package + // Example: https://github.com/santoslab/ros-examples/blob/main/tempControl_ws/src/tc_py_pkg/setup.cfg + def genCfgSetupFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { val top_level_package_nameT: String = genPyPackageName(modelName) - val fileName: String = "CMakeLists.txt" + val fileName: String = "setup.cfg" - // TODO: Add 'find interface type packages' under rclcpp val setupFileBody = - st"""cmake_minimum_required(VERSION 3.8) - |project(${top_level_package_nameT}_bringup) - | - |if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - | add_compile_options(-Wall -Wextra -Wpedantic) - |endif() - | - |find_package(ament_cmake REQUIRED) - | - |install(DIRECTORY - | launch - | DESTINATION share/$${PROJECT_NAME} - |) - | - |ament_package() - """ + st"""# ${fileName} in src/${top_level_package_nameT} + |[develop] + |script_dir=$$base/lib/${top_level_package_nameT} + |[install] + |install_scripts=$$base/lib/${top_level_package_nameT} + """ - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", fileName) + val filePath: ISZ[String] = IS("src", top_level_package_nameT, fileName) - return (filePath, setupFileBody) + return (filePath, setupFileBody, true, IS()) } - def genLaunchPackageFile(modelName: String): (ISZ[String], ST) = { + def genXmlPackageFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { val top_level_package_nameT: String = genPyPackageName(modelName) val fileName: String = "package.xml" - // TODO: This list of message types is currently just a placeholder - // It should probably be built up while looping through each port - val packages: ISZ[String] = IS("example_interfaces") + val startMarker: String = "" + val endMarker: String = "" + + val packages: ISZ[String] = IS(s"${genPyPackageName(modelName)}_interfaces") val pkgDependencies: ISZ[ST] = genPackageFilePkgDependencies(packages) - // TODO: Add interface type package dependencies under rclcpp val setupFileBody = st""" | | - | ${top_level_package_nameT}_bringup - | 0.0.0 - | TODO: Package description - | sireum - | TODO: License declaration + | ${top_level_package_nameT} + | 0.0.0 + | TODO: Package description + | ed + | TODO: License declaration | - | ament_cmake + | rclpy + | rosidl_runtime_py + | ${(pkgDependencies, "\n")} | - | $top_level_package_nameT + | ${startMarker} | - | ament_lint_auto - | ament_lint_common + | ${endMarker} | - | - | ament_cmake - | + | ament_copyright + | ament_flake8 + | ament_pep257 + | python3-pytest + | + | + | ament_python + | | """ - val filePath: ISZ[String] = IS("src", s"${top_level_package_nameT}_bringup", fileName) + val filePath: ISZ[String] = IS("src", top_level_package_nameT, fileName) - return (filePath, setupFileBody) + return (filePath, setupFileBody, F, IS(Marker(startMarker, endMarker))) } + def genPyInitFile(packageName: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val fileName = "__init__.py" - //================================================ - // L a u n c h File (Python Format) - //================================================ + val fileBody = + st""" + """ - // Example: - // tc_node = Node( ## Example is "tc_node" python variable name - // package="tc_cpp_pkg", - // executable="tc_exe" - // ) - def genPyFormatLaunchNodeDeclName(componentNameS: String): String = { - // create target launch node decl name - val launch_node_decl_nameT: String = s"${componentNameS}${launch_node_decl_suffix}" - return launch_node_decl_nameT - } - - // genLaunchNodeDecl() - generate node declaration - // Example: - // tc_node = Node( - // package="tc_cpp_pkg", - // executable="tc_exe" - // ) - def genPyFormatLaunchNodeDecl(launch_node_decl_nameT: String, - top_level_package_nameT: String, - component: AadlThread): ST = { - val node_executable_file_nameT = genExecutableFileName(component.pathAsString("_")) - val s = + val filePath: ISZ[String] = IS("src", packageName, packageName, fileName) + + return (filePath, fileBody, T, IS()) + } + + def genPySubInitFile(modelName: String, subModelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val top_level_package_nameT: String = genPyPackageName(modelName) + val fileName: String = "__init__.py" + + val setupFileBody = st""" - |${launch_node_decl_nameT} = Node( - | package = ${top_level_package_nameT}, - | executable = ${node_executable_file_nameT} - | ) - """ - return s + """ + + val filePath: ISZ[String] = IS("src", top_level_package_nameT, top_level_package_nameT, subModelName, fileName) + + return (filePath, setupFileBody, T, IS()) } - // Example: - // ld.add_action(tc_node) - def genPyFormatLaunchAddAction(launch_node_decl_nameT: String): ST = { - val s = st"""ld.add_action(${launch_node_decl_nameT})""" - return s + def genPyResourceFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val top_level_package_nameT: String = genPyPackageName(modelName) + + val setupFileBody = + st""" + """ + + val filePath: ISZ[String] = IS("src", top_level_package_nameT, "resource", top_level_package_nameT) + + return (filePath, setupFileBody, T, IS()) } - // For example, see https://github.com/santoslab/ros-examples/blob/main/tempControl_ws/src/tc_bringup/launch/tc.launch.py - def genPyFormatLaunchFile(modelName: String, threadComponents: ISZ[AadlThread]): (ISZ[String], ST) = { - val fileName = genPyLaunchFileName(modelName) + def genPyCopyrightFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val top_level_package_nameT: String = genPyPackageName(modelName) + val fileName: String = "test_copyright.py" + val setupFileBody = + st"""# Copyright 2015 Open Source Robotics Foundation, Inc. + |# + |# Licensed under the Apache License, Version 2.0 (the "License"); + |# you may not use this file except in compliance with the License. + |# You may obtain a copy of the License at + |# + |# http://www.apache.org/licenses/LICENSE-2.0 + |# + |# Unless required by applicable law or agreed to in writing, software + |# distributed under the License is distributed on an "AS IS" BASIS, + |# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + |# See the License for the specific language governing permissions and + |# limitations under the License. + | + |from ament_copyright.main import main + |import pytest + | + | + |# Remove the `skip` decorator once the source file(s) have a copyright header + |@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') + |@pytest.mark.copyright + |@pytest.mark.linter + |def test_copyright(): + | rc = main(argv=['.', 'test']) + | assert rc == 0, 'Found errors' + """ + + val filePath: ISZ[String] = IS("src", top_level_package_nameT, "test", fileName) + + return (filePath, setupFileBody, T, IS()) + } + + def genPyFlakeFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { val top_level_package_nameT: String = genPyPackageName(modelName) + val fileName: String = "test_flake8.py" - var node_decls: ISZ[ST] = IS() - var ld_entries: ISZ[ST] = IS() + val setupFileBody = + st"""# Copyright 2017 Open Source Robotics Foundation, Inc. + |# + |# Licensed under the Apache License, Version 2.0 (the "License"); + |# you may not use this file except in compliance with the License. + |# You may obtain a copy of the License at + |# + |# http://www.apache.org/licenses/LICENSE-2.0 + |# + |# Unless required by applicable law or agreed to in writing, software + |# distributed under the License is distributed on an "AS IS" BASIS, + |# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + |# See the License for the specific language governing permissions and + |# limitations under the License. + | + |from ament_flake8.main import main_with_errors + |import pytest + | + | + |@pytest.mark.flake8 + |@pytest.mark.linter + |def test_flake8(): + | rc, errors = main_with_errors(argv=[]) + | assert rc == 0, \\ + | 'Found %d code style errors / warnings:\n' % len(errors) + \\ + | '\n'.join(errors) + """ - for (comp <- threadComponents) { - val launch_node_decl_nameT = genPyFormatLaunchNodeDeclName(comp.pathAsString("_")) - node_decls = node_decls :+ genPyFormatLaunchNodeDecl(launch_node_decl_nameT, top_level_package_nameT, comp) - ld_entries = ld_entries :+ genPyFormatLaunchAddAction(launch_node_decl_nameT) - } + val filePath: ISZ[String] = IS("src", top_level_package_nameT, "test", fileName) + + return (filePath, setupFileBody, T, IS()) + } - val launchFileBody = - st"""from launch import LaunchDescription - |from launch_ros.actions import Node + def genPyPrepFile(modelName: String): (ISZ[String], ST, B, ISZ[Marker]) = { + val top_level_package_nameT: String = genPyPackageName(modelName) + val fileName: String = "test_prep257.py" + + val setupFileBody = + st"""# Copyright 2015 Open Source Robotics Foundation, Inc. + |# + |# Licensed under the Apache License, Version 2.0 (the "License"); + |# you may not use this file except in compliance with the License. + |# You may obtain a copy of the License at + |# + |# http://www.apache.org/licenses/LICENSE-2.0 + |# + |# Unless required by applicable law or agreed to in writing, software + |# distributed under the License is distributed on an "AS IS" BASIS, + |# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + |# See the License for the specific language governing permissions and + |# limitations under the License. | - |def generate_launch_description(): - | ld = LaunchDescription() + |from ament_pep257.main import main + |import pytest | - | ${(node_decls, "\n")} - | ${(ld_entries, "\n")} | - | return ld + |@pytest.mark.linter + |@pytest.mark.pep257 + |def test_pep257(): + | rc = main(argv=['.', 'test']) + | assert rc == 0, 'Found code style errors / warnings' """ - val filePath: ISZ[String] = IS("src", s"${modelName}_bringup", "launch", fileName) + val filePath: ISZ[String] = IS("src", top_level_package_nameT, "test", fileName) - return (filePath, launchFileBody) + return (filePath, setupFileBody, T, IS()) } //================================================ @@ -298,62 +401,164 @@ object GeneratorPy { // Example: https://github.com/santoslab/ros-examples/tree/main/tempControl_ws/src/tc_py_pkg/tc_py_pkg //================================================ + def genPyDataPortInitializerStrict(inDataPort: AadlPort, nodeName: String, portType: String): ST = { + val portName = inDataPort.identifier + + val initializer: ST = + st"""def init_${portName}(self, val): + | self.enqueue(self.infrastructureIn_${portName}, val) + """ + return initializer + } + // Example: // self.subscription = self.create_subscription( // String, // 'fanCmd', // self.handle_fanCmd, // 10) - def genPyTopicSubscrptionStrict(inPort: AadlPort, nodeName: String, isSporadic: B): ST = { - val topicName = seqToString(inPort.path, "_") - val portName = inPort.identifier + def genPyTopicSubscriptionStrict(inPort: AadlPort, portType: String, outPortNames: ISZ[String]): ST = { + val portName = genPortName(inPort) + val handlerName = inPort.identifier + + val handler: ST = st"self.accept_${handlerName}" + + if (outPortNames.size == 1) { + val topicName = outPortNames.apply(0) + + val portCode: ST = + st"""self.${portName}_subscription_ = self.create_subscription( + | ${portType}, + | "${topicName}", + | ${handler}, + | 1, + | callback_group=self.${callback_group_name}) + """ + return portCode + } + + // If the port is a fan in port + var inputInstances: ISZ[ST] = IS() + var counter = 1 + + for (outPortName <- outPortNames) { + inputInstances = inputInstances :+ + st"""self.${portName}_subscription_${counter} = self.create_subscription( + | ${portType}, + | "${outPortName}", + | ${handler}, + | 1, + | callback_group=self.${callback_group_name}) + """ + counter = counter + 1 + } + + val fanPortCode: ST = + st"${(inputInstances, "\n")}" + + return fanPortCode + } + + def genPyThread(inPort: AadlPort, portType: String): ST = { + val handlerName = inPort.identifier + + val thread: ST = + st"""def ${handlerName}_thread(self): + | with self.lock_: + | self.receiveInputs(self.infrastructureIn_${handlerName}, self.applicationIn_${handlerName}) + | if len(self.applicationIn_${handlerName}) == 0: return + | self.handle_${handlerName}_base(self.applicationIn_${handlerName}[0]) + | self.applicationIn_${handlerName}.pop() + | self.sendOutputs() + """ + + return thread + } + + def genPyMessageAcceptor(inPort: AadlPort, isSporadic: B, portType: String): ST = { + val handlerName = inPort.identifier val handler: ST = - if (!isSporadic || inPort.isInstanceOf[AadlDataPort]) { - st"enqueue(infrastructureIn_${portName}, msg)" - } - else { - // TODO: CPP to Py - st"""enqueue(infrastructureIn_${portName}, msg) - | - |""" - } + if (!isSporadic || inPort.isInstanceOf[AadlDataPort]) st"self.enqueue(self.infrastructureIn_${handlerName}, msg)" + else + st"""self.enqueue(self.infrastructureIn_${handlerName}, msg) + |thread = threading.Thread(target=self.${handlerName}_thread) + |thread.daemon = True + |thread.start() + """ - // Int32 is a placeholder message value - val portCode: ST = - st"""self.${topicName}_subscription_ = self.create_subscription( - | Int32, - | "${topicName}", - | self.${handler}, - | 1, - | ${subscription_options_name}) - |""" - return portCode + val method: ST = + st"""def accept_${handlerName}(self, msg): + | ${handler} + """ + return method } - def genPyGetApplicationInValue(inPort: AadlPort, nodeName: String): ST = { + def genPyInfrastructureInQueue(inPort: AadlPort): ST = { + val portName = inPort.identifier + + val inMsgQueue: ST = + st"self.infrastructureIn_${portName} = deque()" + return inMsgQueue + } + + def genPyApplicationInQueue(inPort: AadlPort): ST = { + val portName = inPort.identifier + + val inMsgQueue: ST = + st"self.applicationIn_${portName} = deque()" + return inMsgQueue + } + + def genPyGetApplicationInValue(inPort: AadlPort, portType: String): ST = { val portName = inPort.identifier - // Int32 is a placeholder message value val subscriptionMessageHeader: ST = - st"""self.get_${portName}() - | MsgType msg = applicationIn_$portName.front() - | return (Int32)msg - |""" + st"""def get_${portName}(self): + | msg = self.applicationIn_${portName}[0] + | return msg + """ return subscriptionMessageHeader } - def genPySubscriptionHandlerBaseSporadic(inPort: AadlPort, nodeName: String): ST = { + def genPySubscriptionHandlerVirtualStrict(inPort: AadlPort, portType: String): ST = { + val handlerName = inPort.identifier + + var handlerCode: ST = st"" + if (isEventPort(portType)) { + handlerCode = + st"""def handle_${handlerName}(self): + | raise NotImplementedError("Subclasses must implement this method") + """ + } else { + handlerCode = + st"""def handle_${handlerName}(self, msg): + | raise NotImplementedError("Subclasses must implement this method") + """ + } + + return handlerCode + } + + def genPySubscriptionHandlerBaseSporadic(inPort: AadlPort, portType: String): ST = { val handlerName = inPort.identifier - // Int32 is a placeholder message value - val handlerCode: ST = - st"""def handle_${handlerName}_base(self, msg): - | if isinstance(&msg, Int32): - | handle_${handlerName}(*typedMsg) - | else: - | PRINT_ERROR("Sending out wrong type of variable on port ${handlerName}.\nThis shouldn't be possible. If you are seeing this message, please notify this tool's current maintainer.") - |""" + var handlerCode: ST = st"" + if (isEventPort(portType)) { + handlerCode = + st"""def handle_${handlerName}_base(self, msg): + | self.handle_${handlerName}() + """ + } + else { + handlerCode = + st"""def handle_${handlerName}_base(self, msg): + | if type(msg) is ${portType}: + | self.handle_${handlerName}(msg) + | else: + | self.get_logger.error("Receiving wrong type of variable on port ${handlerName}.\nThis shouldn't be possible. If you are seeing this message, please notify this tool's current maintainer.") + """ + } return handlerCode } @@ -363,19 +568,18 @@ object GeneratorPy { // "${inPortName}", // 10, // callback_group=self.${callback_group_name}) - def genPyTopicPublisher(outPort: AadlPort, inPortNames: ISZ[String]): ST = { - val portName = seqToString(outPort.path, "_") + def genPyTopicPublisher(outPort: AadlPort, portType: String, inPortNames: ISZ[String]): ST = { + val portName = genPortName(outPort) if (inPortNames.size == 1) { val inPortName = inPortNames.apply(0) - // Int32 is a placeholder message value val portCode: ST = st"""self.${portName}_publisher_ = self.create_publisher( - | Int32, - | "${inPortName}", - | 1) - |""" + | ${portType}, + | "${inPortName}", + | 1) + """ return portCode } @@ -386,11 +590,11 @@ object GeneratorPy { for (inPortName <- inPortNames) { outputInstances = outputInstances :+ st"""self.${portName}_publisher_${counter} = self.create_publisher( - | Int32, - | "${inPortName}", - | 1) - |""" - counter = counter + 1; + | ${portType}, + | "${inPortName}", + | 1) + """ + counter = counter + 1 } val fanPortCode: ST = @@ -399,89 +603,252 @@ object GeneratorPy { return fanPortCode } - def genPyTopicPublishMethodStrict(outPort: AadlPort, nodeName: String, inputPortCount: Z): ST = { - val portName = seqToString(outPort.path, "_") + def genPyTopicPublishMethodStrict(outPort: AadlPort, portType: String, inputPortCount: Z): ST = { + val portName = genPortName(outPort) val handlerName = outPort.identifier var publishers: ISZ[ST] = IS() if (inputPortCount == 1) { publishers = publishers :+ - st"self.${portName}_publisher_.publish(*typedMsg)" + st"self.${portName}_publisher_.publish(msg)" } else { for (i <- 1 to inputPortCount) { publishers = publishers :+ - st"self.${portName}_publisher_${i}.publish(*typedMsg)" + st"self.${portName}_publisher_${i}.publish(msg)" } } - // Int32 is a placeholder message value val publisherCode: ST = st"""def sendOut_${handlerName}(self, msg): - | if isinstance(&msg, Int32): - | ${(publishers, "\n")} - | else: - | PRINT_ERROR("Sending out wrong type of variable on port ${handlerName}.\nThis shouldn't be possible. If you are seeing this message, please notify this tool's current maintainer.") - |""" + | if type(msg) is ${portType}: + | ${(publishers, "\n")} + | else: + | self.get_logger().error("Sending out wrong type of variable on port ${handlerName}.\nThis shouldn't be possible. If you are seeing this message, please notify this tool's current maintainer.") + """ return publisherCode } - def genPyPutMsgMethodStrict(outPort: AadlPort, nodeName: String): ST = { + def genPyPutMsgMethodStrict(outPort: AadlPort, portType: String): ST = { val handlerName = outPort.identifier - // Int32 is a placeholder message value - val putMsgCode: ST = - st"""def put_${handlerName}(self, msg): - | enqueue(applicationOut_${handlerName}, msg) - |""" + var putMsgCode: ST = st"" + + if (isEventPort(portType)) { + putMsgCode = + st"""def put_${handlerName}(self): + | self.enqueue(self.applicationOut_${handlerName}, ${portType}()) + """ + } + else { + putMsgCode = + st"""def put_${handlerName}(self, msg): + | self.enqueue(self.applicationOut_${handlerName}, msg) + """ + } return putMsgCode } + def genPyDataPortInitializer(inDataPort: AadlPort): ST = { + val portName = inDataPort.identifier + + val initializer: ST = + st"""def init_${portName}(self, val): + | self.${portName}_msg_holder = val""" + return initializer + } + // Example: // self.subscription = self.create_subscription( // String, // 'fanCmd', // self.handle_fanCmd, // 10) - def genPyTopicSubscription(inPort: AadlPort, nodeName: String): ST = { - val topicName = seqToString(inPort.path, "_") - val portName = inPort.identifier + def genPyTopicSubscription(inPort: AadlPort, portType: String, outPortNames: ISZ[String]): ST = { + val portName = genPortName(inPort) + val handlerName = inPort.identifier + + var handler: ST = st"" - // Int32 in a placeholder message value - val portCode: ST = - st"""self.${topicName}_subscription_ = self.create_subscription( - | Int32, - | "${topicName}", - | 1, - | ${subscription_options_name} - |""" - return portCode + if(isEventPort(portType)) { + handler = st"self.event_handle_${handlerName}" + } + else { + handler = st"self.handle_${handlerName}" + } + + if (outPortNames.size == 1) { + val topicName = outPortNames.apply(0) + val portCode: ST = + st"""self.${portName}_subscription_ = self.create_subscription( + | ${portType}, + | "${topicName}", + | ${handler}, + | 1, + | callback_group=self.${callback_group_name}) + """ + return portCode + } + + // If the port is a fan in port + var inputInstances: ISZ[ST] = IS() + var counter = 1 + + for (outPortName <- outPortNames) { + inputInstances = inputInstances :+ + st"""self.${portName}_subscription_${counter} = self.create_subscription( + | ${portType}, + | "${outPortName}", + | ${handler}, + | 1, + | callback_group=self.${callback_group_name}) + """ + counter = counter + 1 + } + + val fanPortCode: ST = + st"${(inputInstances, "\n")}" + + return fanPortCode + } + + def genPyEventPortHandler(inPort: AadlPort, portType: String): ST = { + val handlerName = inPort.identifier + + val handler: ST = + st"""def event_handle_${handlerName}(self, msg): + | self.handle_${handlerName}() + """ + + return handler + } + + def genPyMessageToString(): ST = { + val message: ST = + st"""def message_to_string(self, msg): + | yaml_str = message_to_yaml(msg) + | return yaml_str + """ + + return message } - def genPySubscriptionHandlerPeriodic(inPort: AadlPort, nodeName: String): ST = { + def genPySubscriptionHandlerSporadicWithExamples(inPort: AadlPort, portType: String, + inDataPorts: ISZ[AadlPort]): ST = { + val handlerName = inPort.identifier + + var exampleUsage: ST = st"" + if (inDataPorts.size > 0) { + exampleUsage = st"# Example receiving messages on data ports" + for (inDataPort <- inDataPorts) { + exampleUsage = + st"""${exampleUsage} + |${inDataPort.identifier} = get_${inDataPort.identifier}() + |self.get_logger().info(f"Received ${inDataPort.identifier}: {self.message_to_string(${inDataPort.identifier})}") + """ + } + } + + + var subscriptionHandlerHeader: ST = st"" + if (isEventPort(portType)) { + subscriptionHandlerHeader = st"""def handle_${handlerName}(self): + | # Handle ${handlerName} event + | self.get_logger().info("Received ${handlerName}") + """ + } + else { + subscriptionHandlerHeader = st"""def handle_${handlerName}(self, msg): + | # Handle ${handlerName} msg + | self.get_logger().info(f"Received ${handlerName}: {self.message_to_string(msg)}") + """ + } + + if (inDataPorts.size > 0) { + subscriptionHandlerHeader = + st"""${subscriptionHandlerHeader} + | + | ${exampleUsage}""" + } + + return subscriptionHandlerHeader + } + + def genPySubscriptionHandlerPeriodic(inPort: AadlPort, portType: String): ST = { val handlerName = inPort.identifier - // Int32 is a placeholder message value val subscriptionHandlerHeader: ST = st"""def handle_${handlerName}(self, msg): - | self.${handlerName}_msg_holder = msg - |""" + | self.${handlerName}_msg_holder = msg + """ return subscriptionHandlerHeader } def genPyGetSubscriptionMessage(inPort: AadlPort, nodeName: String): ST = { val portName = inPort.identifier - // Int32 is a placeholder message value val subscriptionMessage: ST = - st"""def get_${portName}(): - | return ${portName}_msg_holder - |""" + st"""def get_${portName}(self): + | return self.${portName}_msg_holder + """ return subscriptionMessage } - def genPyTopicPublishMethod(outPort: AadlPort, nodeName: String, inputPortCount: Z): ST = { - val portName = seqToString(outPort.path, "_") + def genPySubscriptionMessageVar(inPort: AadlPort): ST = { + val portName = inPort.identifier + + val subscriptionMessageVar: ST = + st"self.${portName}_msg_holder = None" + return subscriptionMessageVar + } + + def genPySubscriptionHandlerVirtual(inPort: AadlPort, portType: String): ST = { + val handlerName = inPort.identifier + + var handlerCode: ST = st"" + if (isEventPort(portType)) { + handlerCode = + st"""def handle_${handlerName}(self): + | raise NotImplementedError("Subclasses must implement this method") + """ + } else { + handlerCode = + st"""def handle_${handlerName}(self, msg): + | raise NotImplementedError("Subclasses must implement this method") + """ + } + + return handlerCode + } + + def genPyInfrastructureOutQueue(inPort: AadlPort): ST = { + val portName = inPort.identifier + + val outMsgQueue: ST = + st"self.infrastructureOut_${portName} = deque()" + return outMsgQueue + } + + def genPyApplicationOutQueue(inPort: AadlPort): ST = { + val portName = inPort.identifier + + val outMsgQueue: ST = + st"self.applicationOut_${portName} = deque()" + return outMsgQueue + } + + def genPyFileMsgTypeIncludes(packageName: String, msgTypes: ISZ[String]): ISZ[ST] = { + var includes: ISZ[ST] = IS() + + for (msgType <- msgTypes) { + includes = includes :+ st"from ${packageName}_interfaces.msg import ${msgType}" + } + + return includes + } + + def genPyTopicPublishMethod(outPort: AadlPort, portType: String, inputPortCount: Z): ST = { + val portName = genPortName(outPort) val handlerName = outPort.identifier var publishers: ISZ[ST] = IS() @@ -492,37 +859,55 @@ object GeneratorPy { else { for (i <- 1 to inputPortCount) { publishers = publishers :+ - st"${portName}_publisher_${i}.publish(msg)" + st"self.${portName}_publisher_${i}.publish(msg)" } } - // Int32 is a placeholder message value - val publisherCode: ST = - st"""def put_${handlerName}(self, msg): - | ${(publishers, "\n")} - |""" + var publisherCode: ST = st"" + + if (isEventPort(portType)) { + publisherCode = + st"""def put_${handlerName}(self): + | msg = ${portType}() + | ${(publishers, "\n")} + """ + } + else { + publisherCode = + st"""def put_${handlerName}(self, msg): + | ${(publishers, "\n")} + """ + } return publisherCode } def genPyCallbackGroupVar(): ST = { val callbackGroup: ST = - st"${callback_group_name} = this.create_callback_group(${callback_group_type})" + st"self.${callback_group_name} = ${callback_group_type}()" return callbackGroup } - def genPyTimeTriggeredStrict(nodeName: String, component: AadlThread): ST = { - val period = component.period.get + def genPyTimeTriggeredBaseMethod(): ST = { + val timeTriggered: ST = + st"""def timeTriggered(self): + | raise NotImplementedError("Subclasses must implement this method") + """ + return timeTriggered + } + + def genPyTimeTriggeredStrict(component: AadlThread): ST = { + val period = component.period.get / 1000 val timer: ST = - st"""self.periodTimer_ = self.create_wall_timer(${period}, self.timeTriggeredCaller, ${callback_group_name})""" + st"""self.periodTimer_ = self.create_timer(${period}, self.timeTriggeredCaller, callback_group=self.${callback_group_name})""" return timer } - def genPyTimeTriggeredTimer(nodeName: String, component: AadlThread): ST = { - val period = component.period.get + def genPyTimeTriggeredTimer(component: AadlThread): ST = { + val period = component.period.get / 1000 val timer: ST = - st"""self.periodTimer_ = self.create_wall_timer(${period}, self.timeTriggered, ${callback_group_name})""" + st"""self.periodTimer_ = self.create_timer(${period}, self.timeTriggered, callback_group=self.${callback_group_name})""" return timer } @@ -530,12 +915,13 @@ object GeneratorPy { var tuples: ISZ[String] = IS() for (name <- portNames) { - tuples = tuples :+ s"{&infrastructureIn_${name}, &applicationIn_${name}}" + tuples = tuples :+ s"[self.infrastructureIn_${name}, self.applicationIn_${name}]" } val vector: ST = - st"""inDataPortTupleVector = - | ${(tuples, ",\n")} + st"""self.inDataPortTupleVector = [ + | ${(tuples, ",\n")} + |] """ return vector } @@ -544,109 +930,112 @@ object GeneratorPy { var tuples: ISZ[String] = IS() for (name <- portNames) { - tuples = tuples :+ s"{&infrastructureIn_${name}, &applicationIn_${name}}" + tuples = tuples :+ s"[self.infrastructureIn_${name}, self.applicationIn_${name}]" } val vector: ST = - st"""inEventPortTupleVector = - | ${(tuples, ",\n")} + st"""self.inEventPortTupleVector = [ + | ${(tuples, ",\n")} + |] """ return vector } - def genPyOutPortTupleVector(nodeName: String, portNames: ISZ[String]): ST = { + def genPyOutPortTupleVector(portNames: ISZ[String]): ST = { var tuples: ISZ[String] = IS() for (name <- portNames) { - tuples = tuples :+ s"{&applicationOut_${name}, &infrastructureOut_${name}, &self.sendOut_${name}}" + tuples = tuples :+ s"[self.applicationOut_${name}, self.infrastructureOut_${name}, self.sendOut_${name}]" } val vector: ST = - st"""outPortTupleVector = - | ${(tuples, ",\n")} + st"""self.outPortTupleVector = [ + | ${(tuples, ",\n")} + |] """ return vector } - def genPyTimeTriggeredCaller(nodeName: String): ST = { + def genPyTimeTriggeredCaller(): ST = { val timeTriggered: ST = st"""def timeTriggeredCaller(self): - | receiveInputs() - | timeTriggered() - | sendOutputs() + | self.receiveInputs() + | self.timeTriggered() + | self.sendOutputs() """ return timeTriggered } - def genPyReceiveInputsSporadic(nodeName: String): ST = { + def genPyReceiveInputsSporadic(): ST = { val method: ST = st"""def receiveInputs(self, infrastructureQueue, applicationQueue): - | if !infrastructureQueue.empty(): - | eventMsg = infrastructureQueue.front() - | infrastructureQueue.pop() - | enqueue(applicationQueue, eventMsg) + | if not(len(infrastructureQueue) == 0): + | eventMsg = infrastructureQueue[0] + | infrastructureQueue.pop() + | self.enqueue(applicationQueue, eventMsg) | - | for port in inDataPortTupleVector: - | infrastructureQueue = std::get<0>(port) - | if !infrastructureQueue.empty(): - | msg = infrastructureQueue.front() - | enqueue(*std::get<1>(port), msg) + | for port in self.inDataPortTupleVector: + | infrastructureQueue = port[0] + | if not(len(infrastructureQueue) == 0): + | msg = infrastructureQueue[0] + | self.enqueue(port[1], msg) """ return method } - def genPyReceiveInputsPeriodic(nodeName: String): ST = { + def genPyReceiveInputsPeriodic(): ST = { val method: ST = st"""def receiveInputs(self): - | for port in inDataPortTupleVector: - | auto infrastructureQueue = std::get<0>(port) - | if !infrastructureQueue.empty(): - | msg = infrastructureQueue.front() - | enqueue(*std::get<1>(port), msg) + | for port in self.inDataPortTupleVector: + | infrastructureQueue = port[0] + | if not(len(infrastructureQueue) == 0): + | msg = infrastructureQueue[0] + | self.enqueue(port[1], msg) | - | for port in inEventPortTupleVector: - | auto infrastructureQueue = std::get<0>(port) - | if !infrastructureQueue.empty(): - | msg = infrastructureQueue.front() - | infrastructureQueue->pop() - | enqueue(*std::get<1>(port), msg) + | for port in self.inEventPortTupleVector: + | infrastructureQueue = port[0] + | if not(len(infrastructureQueue) == 0): + | msg = infrastructureQueue[0] + | infrastructureQueue.pop() + | self.enqueue(port[1], msg) """ return method } - def genPyEnqueue(nodeName: String): ST = { + def genPyEnqueue(): ST = { val method: ST = st"""def enqueue(self, queue, val): - | if queue.size() >= 1: - | queue.pop() - | queue.push(val) + | if len(queue) >= 1: + | queue.pop() + | queue.append(val) """ return method } - def genPySendOutputs(nodeName: String): ST = { + def genPySendOutputs(): ST = { val method: ST = st"""def sendOutputs(self): - | for port in outPortTupleVector: - | applicationQueue = std::get<0>(port) - | if applicationQueue.size() != 0: - | msg = applicationQueue.front() - | applicationQueue.pop() - | enqueue(*std::get<1>(port), msg) + | for port in self.outPortTupleVector: + | applicationQueue = port[0] + | if len(applicationQueue) != 0: + | msg = applicationQueue[0] + | applicationQueue.pop() + | self.enqueue(port[1], msg) | - | for port in outPortTupleVector: - | infrastructureQueue = std::get<1>(port) - | if infrastructureQueue.size() != 0: - | msg = infrastructureQueue.front() - | infrastructureQueue.pop() - | (this->*std::get<2>(port))(msg) + | for port in self.outPortTupleVector: + | infrastructureQueue = port[1] + | if len(infrastructureQueue) != 0: + | msg = infrastructureQueue[0] + | infrastructureQueue.pop() + | (port[2])(msg) """ return method } def genPyBaseNodePyFile(packageName: String, component: AadlThread, connectionMap: Map[ISZ[String], ISZ[ISZ[String]]], - strictAADLMode: B): (ISZ[String], ST) = { - val nodeName = s"${component.pathAsString("_")}_base" + datatypeMap: Map[AadlType, (String, ISZ[String])], strictAADLMode: B, + invertTopicBinding: B, reporter: Reporter): (ISZ[String], ST, B, ISZ[Marker]) = { + val nodeName = s"${genNodeName(component)}_base" val fileName = genPyNodeSourceName(nodeName) var subscribers: ISZ[ST] = IS() @@ -654,268 +1043,638 @@ object GeneratorPy { var subscriberMethods: ISZ[ST] = IS() var publisherMethods: ISZ[ST] = IS() var subscriptionMessageGetters: ISZ[ST] = IS() + var eventPortHandlers: ISZ[ST] = IS() + var dataPortInitializers: ISZ[ST] = IS() var outPortNames: ISZ[String] = IS() - var inTuplePortNames: ISZ[String] = IS() + var inPortNames: ISZ[String] = IS() var strictPutMsgMethods: ISZ[ST] = IS() - var strictSubscriptionHandlerBaseMethods: ISZ[ST] = IS() + var strictSubscriptionMessageAcceptorMethods: ISZ[ST] = IS() + var subscriptionHandlerMethods: ISZ[ST] = IS() + var msgTypes: ISZ[String] = IS() + + var inMsgVars: ISZ[ST] = IS() + var outMsgVars: ISZ[ST] = IS() var hasInPorts = F for (p <- component.getPorts()) { + val portDatatype: String = genPortDatatype(p, packageName, datatypeMap, reporter) + if (!ISZOps(msgTypes).contains(portDatatype)) { + msgTypes = msgTypes :+ portDatatype + } if (strictAADLMode) { if (p.direction == Direction.In) { - subscribers = subscribers :+ genPyTopicSubscrptionStrict(p, nodeName, isSporadic(component)) + if (p.isInstanceOf[AadlDataPort]) { + dataPortInitializers = dataPortInitializers :+ genPyDataPortInitializerStrict(p, nodeName, portDatatype) + } + + if (invertTopicBinding) { + if (connectionMap.get(p.path).nonEmpty) { + val outputPorts = connectionMap.get(p.path).get + val outputPortNames = getPortNames(outputPorts) + subscribers = subscribers :+ genPyTopicSubscriptionStrict(p, portDatatype, outputPortNames) + } + else { + subscribers = subscribers :+ genPyTopicSubscriptionStrict(p, portDatatype, getPortNames(IS(p.path.toISZ))) + } + } + else { + subscribers = subscribers :+ genPyTopicSubscriptionStrict(p, portDatatype, getPortNames(IS(p.path.toISZ))) + } + + if (isSporadic(component) && !p.isInstanceOf[AadlDataPort]) { + strictSubscriptionMessageAcceptorMethods = strictSubscriptionMessageAcceptorMethods :+ genPyThread(p, portDatatype) + } + + strictSubscriptionMessageAcceptorMethods = strictSubscriptionMessageAcceptorMethods :+ genPyMessageAcceptor(p, isSporadic(component), portDatatype) + + inMsgVars = inMsgVars :+ genPyInfrastructureInQueue(p) + inMsgVars = inMsgVars :+ genPyApplicationInQueue(p) + if (!isSporadic(component) || p.isInstanceOf[AadlDataPort]) { - inTuplePortNames = inTuplePortNames :+ p.identifier - subscriptionMessageGetters = subscriptionMessageGetters :+ genPyGetApplicationInValue(p, nodeName) + inPortNames = inPortNames :+ p.identifier + subscriptionMessageGetters = subscriptionMessageGetters :+ genPyGetApplicationInValue(p, portDatatype) } else { - strictSubscriptionHandlerBaseMethods = strictSubscriptionHandlerBaseMethods :+ - genPySubscriptionHandlerBaseSporadic(p, nodeName) + subscriptionHandlerMethods = subscriptionHandlerMethods :+ + genPySubscriptionHandlerVirtualStrict(p, portDatatype) + subscriptionHandlerMethods = subscriptionHandlerMethods :+ + genPySubscriptionHandlerBaseSporadic(p, portDatatype) } hasInPorts = T } else { + outMsgVars = outMsgVars :+ genPyInfrastructureOutQueue(p) + outMsgVars = outMsgVars :+ genPyApplicationOutQueue(p) outPortNames = outPortNames :+ p.identifier - if (connectionMap.get(p.path).nonEmpty) { - val inputPorts = connectionMap.get(p.path).get - val inputPortNames = getPortNames(inputPorts) - publishers = publishers :+ genPyTopicPublisher(p, inputPortNames) + if (invertTopicBinding) { + publishers = publishers :+ genPyTopicPublisher(p, portDatatype, getPortNames(IS(p.path.toISZ))) publisherMethods = publisherMethods :+ - genPyTopicPublishMethodStrict(p, nodeName, inputPortNames.size) + genPyTopicPublishMethodStrict(p, portDatatype, 1) } else { - publisherMethods = publisherMethods :+ - genPyTopicPublishMethodStrict(p, nodeName, 0) + if (connectionMap.get(p.path).nonEmpty) { + val inputPorts = connectionMap.get(p.path).get + val inputPortNames = getPortNames(inputPorts) + publishers = publishers :+ genPyTopicPublisher(p, portDatatype, inputPortNames) + publisherMethods = publisherMethods :+ + genPyTopicPublishMethodStrict(p, portDatatype, inputPortNames.size) + } + else { + // Out ports with no connections should still publish to a topic + publishers = publishers :+ genPyTopicPublisher(p, portDatatype, getPortNames(IS(p.path.toISZ))) + publisherMethods = publisherMethods :+ + genPyTopicPublishMethodStrict(p, portDatatype, 1) + } } - strictPutMsgMethods = strictPutMsgMethods :+ genPyPutMsgMethodStrict(p, nodeName) + strictPutMsgMethods = strictPutMsgMethods :+ genPyPutMsgMethodStrict(p, portDatatype) } } else { if (p.direction == Direction.In) { - subscribers = subscribers :+ genPyTopicSubscription(p, nodeName) + if (p.isInstanceOf[AadlDataPort]) { + dataPortInitializers = dataPortInitializers :+ genPyDataPortInitializer(p) + } + + if (invertTopicBinding) { + if (connectionMap.get(p.path).nonEmpty) { + val outputPorts = connectionMap.get(p.path).get + val outputPortNames = getPortNames(outputPorts) + subscribers = subscribers :+ genPyTopicSubscription(p, portDatatype, outputPortNames) + } + else { + // In ports with no connections should still subscribe to a topic + subscribers = subscribers :+ + genPyTopicSubscription(p, portDatatype, getPortNames(IS(p.path.toISZ))) + } + } + else { + subscribers = subscribers :+ + genPyTopicSubscription(p, portDatatype, getPortNames(IS(p.path.toISZ))) + } + // Specifically for event ports, not eventdata ports (no data to be handled) + if (isEventPort(portDatatype)) { + eventPortHandlers = eventPortHandlers :+ genPyEventPortHandler(p, portDatatype) + } if (!isSporadic(component) || p.isInstanceOf[AadlDataPort]) { subscriberMethods = subscriberMethods :+ - genPySubscriptionHandlerPeriodic(p, nodeName) + genPySubscriptionHandlerPeriodic(p, portDatatype) subscriptionMessageGetters = subscriptionMessageGetters :+ genPyGetSubscriptionMessage(p, nodeName) + inMsgVars = inMsgVars :+ genPySubscriptionMessageVar(p) + } else { + subscriptionHandlerMethods = subscriptionHandlerMethods :+ + genPySubscriptionHandlerVirtual(p, portDatatype) } hasInPorts = T } else { - if (connectionMap.get(p.path).nonEmpty) { - val inputPorts = connectionMap.get(p.path).get - val inputPortNames = getPortNames(inputPorts) - publishers = publishers :+ genPyTopicPublisher(p, inputPortNames) + if (invertTopicBinding) { + publishers = publishers :+ genPyTopicPublisher(p, portDatatype, getPortNames(IS(p.path.toISZ))) publisherMethods = publisherMethods :+ - genPyTopicPublishMethod(p, nodeName, inputPortNames.size) + genPyTopicPublishMethod(p, portDatatype, 1) + } + else { + if (connectionMap.get(p.path).nonEmpty) { + val inputPorts = connectionMap.get(p.path).get + val inputPortNames = getPortNames(inputPorts) + publishers = publishers :+ genPyTopicPublisher(p, portDatatype, inputPortNames) + publisherMethods = publisherMethods :+ + genPyTopicPublishMethod(p, portDatatype, inputPortNames.size) + } + else { + // Out ports with no connections should still publish to a topic + publishers = publishers :+ genPyTopicPublisher(p, portDatatype, getPortNames(IS(p.path.toISZ))) + publisherMethods = publisherMethods :+ + genPyTopicPublishMethod(p, portDatatype, 1) + } } } } } + val typeIncludes: ISZ[ST] = genPyFileMsgTypeIncludes(packageName, msgTypes) + var stdIncludes: ST = + st"""from collections import deque""" + + if (strictAADLMode) { + stdIncludes = + st"""${stdIncludes} + |from typing import Union + |import threading""" + } + + if (!strictAADLMode && subscribers.size > 0) { + stdIncludes = + st"""${stdIncludes} + |from ${packageName}.user_code.${genNodeName(component)}_src import *""" + } + var fileBody = st"""#!/usr/bin/env python3 |import rclpy |from rclpy.node import Node + |${(stdIncludes, "\n")} + |from rclpy.callback_groups import ${callback_group_type} + |${(typeIncludes, "\n")} | - |//================================================= - |// D O N O T E D I T T H I S F I L E - |//================================================= + |#======================================================== + |# Re-running Codegen will overwrite changes to this file + |#======================================================== | |class ${nodeName}(Node): - | def __init__(self): - | super().__init__("${component.pathAsString("_")}") + | def __init__(self): + | super().__init__("${genNodeName(component)}") | - | ${genPyCallbackGroupVar()} + | ${genPyCallbackGroupVar()} """ - if (hasInPorts) { + if (strictAADLMode) { fileBody = st"""${fileBody} - | ${subscription_options_name}.callback_group = ${callback_group_name} - """ + | self.lock_ = threading.Lock() + """ } fileBody = st"""${fileBody} - | // Setting up connections - | ${(subscribers ++ publishers, "\n")}""" + | # Setting up connections + | ${(subscribers ++ publishers, "\n")}""" if(!isSporadic(component)) { if (strictAADLMode) { fileBody = st"""${fileBody} - | // timeTriggeredCaller callback timer - | ${genPyTimeTriggeredStrict(nodeName, component)} + | # timeTriggeredCaller callback timer + | ${genPyTimeTriggeredStrict(component)} """ } else { fileBody = st"""${fileBody} - | // timeTriggered callback timer - | ${genPyTimeTriggeredTimer(nodeName, component)} + | # timeTriggered callback timer + | ${genPyTimeTriggeredTimer(component)} """ } } + if (inMsgVars.size > 0) { + fileBody = + st"""${fileBody} + | ${(inMsgVars, "\n")} + """ + } + + if (outMsgVars.size > 0) { + fileBody = + st"""${fileBody} + | ${(outMsgVars, "\n")} + """ + } if(strictAADLMode) { fileBody = st"""${fileBody} - | // Used by receiveInputs - | ${genPyInDataPortTupleVector(inTuplePortNames)}""" + | # Used by receiveInputs + | ${genPyInDataPortTupleVector(inPortNames)}""" if (!isSporadic(component)) { fileBody = st"""${fileBody} - | // Used by receiveInputs - | ${genPyInEventPortTupleVector(inTuplePortNames)}""" + | # Used by receiveInputs + | ${genPyInEventPortTupleVector(inPortNames)}""" } fileBody = st"""${fileBody} - | // Used by sendOutputs - | ${genPyOutPortTupleVector(nodeName, outPortNames)}""" + | # Used by sendOutputs + | ${genPyOutPortTupleVector(outPortNames)}""" } - if (subscriberMethods.size > 0 || publisherMethods.size > 0) { + if (dataPortInitializers.size > 0) { fileBody = st"""${fileBody} - |//================================================= - |// C o m m u n i c a t i o n - |//================================================= + | ${(dataPortInitializers, "\n\n")} """ + } + + fileBody = + st"""${fileBody} + | ${genPyTimeTriggeredBaseMethod()}""" + + if (subscriberMethods.size > 0 || publisherMethods.size > 0 || (strictAADLMode && subscribers.size > 0)) { + fileBody = + st"""${fileBody} + | #================================================= + | # C o m m u n i c a t i o n + | #================================================= + """ + + if (strictSubscriptionMessageAcceptorMethods.size > 0) { + fileBody = + st"""${fileBody} + | ${(strictSubscriptionMessageAcceptorMethods, "\n")}""" + } if (subscriberMethods.size > 0) { fileBody = st"""${fileBody} - |${(subscriberMethods, "\n")}""" + | ${(subscriberMethods, "\n")}""" } if (subscriptionMessageGetters.size > 0) { fileBody = st"""${fileBody} - |${(subscriptionMessageGetters, "\n")}""" + | ${(subscriptionMessageGetters, "\n")}""" } - if (strictSubscriptionHandlerBaseMethods.size > 0) { + if (eventPortHandlers.size > 0) { fileBody = st"""${fileBody} - |${(strictSubscriptionHandlerBaseMethods, "\n")}""" + | ${(eventPortHandlers, "\n")}""" } if (publisherMethods.size > 0) { fileBody = st"""${fileBody} - |${(publisherMethods, "\n")} - |${(strictPutMsgMethods, "\n")}""" + | ${(publisherMethods, "\n")} + | ${(strictPutMsgMethods, "\n")}""" } } + if (subscriptionHandlerMethods.size > 0) { + fileBody = + st"""${fileBody} + | #================================================= + | # C o m p u t e E n t r y P o i n t + | #================================================= + | ${(subscriptionHandlerMethods, "\n")} + """ + } + if (strictAADLMode) { if (!isSporadic(component)) { fileBody = st"""${fileBody} - |${genPyTimeTriggeredCaller(nodeName)}""" + | ${genPyTimeTriggeredCaller()}""" } - val receiveInputs: ST = if (isSporadic(component)) genPyReceiveInputsSporadic(nodeName) - else genPyReceiveInputsPeriodic(nodeName) + val receiveInputs: ST = if (isSporadic(component)) genPyReceiveInputsSporadic() + else genPyReceiveInputsPeriodic() fileBody = st"""${fileBody} - |${receiveInputs} - |${genPyEnqueue(nodeName)} - |${genPySendOutputs(nodeName)}""" + | ${receiveInputs} + | ${genPyEnqueue()} + | ${genPySendOutputs()}""" } - val filePath: ISZ[String] = IS("src", packageName, "src", "base_code", fileName) + val filePath: ISZ[String] = IS("src", packageName, packageName, "base_code", fileName) - return (filePath, fileBody) + return (filePath, fileBody, T, IS()) } - def genPySubscriptionHandlerSporadicStrict(inPort: AadlPort, nodeName: String): ST = { + def genPySubscriptionHandlerSporadicStrict(inPort: AadlPort, portType: String): ST = { val handlerName = inPort.identifier - // Int32 is a placeholder message value - val subscriptionHandlerHeader: ST = - st"""def handle_${handlerName}(self, msg) - |{ - | // Handle ${handlerName} msg - |} - """ + var subscriptionHandlerHeader: ST = st"" + if (isEventPort(portType)) { + subscriptionHandlerHeader = + st"""def handle_${handlerName}(self): + | # Handle ${handlerName} event + | self.get_logger().info("Received ${handlerName}") + """ + } + else { + subscriptionHandlerHeader = + st"""def handle_${handlerName}(self, msg): + | # Handle ${handlerName} msg + | self.get_logger().info(f"Received ${handlerName}: {self.message_to_string(msg)}") + """ + } return subscriptionHandlerHeader } - def genPySubscriptionHandlerSporadic(inPort: AadlPort, nodeName: String): ST = { + def genPySubscriptionHandlerSporadic(inPort: AadlPort, portType: String): ST = { val handlerName = inPort.identifier - // Int32 is a placeholder message value - val subscriptionHandlerHeader: ST = - st"""def handle_${handlerName}(self, msg) - |{ - | // Handle ${handlerName} msg - |} - """ + var subscriptionHandlerHeader: ST = st"" + if (isEventPort(portType)) { + subscriptionHandlerHeader = + st"""def handle_${handlerName}(self): + | # Handle ${handlerName} event + | self.get_logger().info("Received ${handlerName}") + """ + } + else { + subscriptionHandlerHeader = + st"""def handle_${handlerName}(self, msg): + | # Handle ${handlerName} msg + | self.get_logger().info(f"Received ${handlerName}: {self.message_to_string(msg)}") + """ + } return subscriptionHandlerHeader } - def genPyTimeTriggeredMethod(nodeName: String): ST = { - val timeTriggered: ST = - st"""def timeTriggered(self) - |{ - | // Handle communication - |} + def genPySubscriptionHandlerSporadicStrictWithExamples(inPort: AadlPort, portType: String, + inDataPorts: ISZ[AadlPort]): ST = { + val handlerName = inPort.identifier + + var exampleUsage: ST = st"" + if (inDataPorts.size > 0) { + exampleUsage = st"# Example receiving messages on data ports" + for (inDataPort <- inDataPorts) { + exampleUsage = + st"""${exampleUsage} + |${inDataPort.identifier} = self.get_${inDataPort.identifier}() + |self.get_logger().info(f"Received ${inDataPort.identifier}: {self.message_to_string(${inDataPort.identifier})}") + """ + } + } + + var subscriptionHandlerHeader: ST = st"" + if (isEventPort(portType)) { + subscriptionHandlerHeader = st"""def handle_${handlerName}(self): + | # Handle ${handlerName} event + | self.get_logger().info("Received ${handlerName}") + """ + } + else { + subscriptionHandlerHeader = st"""def handle_${handlerName}(self, msg): + | # Handle ${handlerName} msg + | self.get_logger().info(f"Received ${handlerName}: {self.message_to_string(msg)}") + """ + } + + if (inDataPorts.size > 0) { + subscriptionHandlerHeader = + st"""${subscriptionHandlerHeader} + | + | ${exampleUsage}""" + } + + return subscriptionHandlerHeader + } + + def genPyExamplePublisher(outPort: AadlPort, packageName: String, + datatypeMap: Map[AadlType, (String, ISZ[String])], + reporter: Reporter): ST = { + val handlerName = outPort.identifier + val dataPortType: String = genPortDatatype(outPort, packageName, datatypeMap, reporter) + + var publisherCode: ST = st"" + + if (isEventPort(dataPortType)) { + publisherCode = + st"self.put_${handlerName}()" + } else { + publisherCode = + st"""${handlerName} = ${dataPortType}() + |self.put_${handlerName}(${handlerName}) + """ + } + + return publisherCode + } + + def genPyTimeTriggeredMethod(inDataPorts: ISZ[AadlPort], examplePublishers: ISZ[ST], + strictAADLMode: B): ST = { + var exampleUsage: ST = st"" + if (inDataPorts.size > 0) { + exampleUsage = st"# Example receiving messages on data ports" + for (inDataPort <- inDataPorts) { + + if (strictAADLMode) { + exampleUsage = + st"""${exampleUsage} + |${inDataPort.identifier} = self.get_${inDataPort.identifier}() + |self.get_logger().info(f"Received ${inDataPort.identifier}: {self.message_to_string(${inDataPort.identifier})}") + """ + } + else { + exampleUsage = + st"""${exampleUsage} + |${inDataPort.identifier} = self.get_${inDataPort.identifier}() + |self.get_logger().info(f"Received ${inDataPort.identifier}: {self.message_to_string(${inDataPort.identifier})}") + """ + } + } + } + + var timeTriggered: ST = + st"""def timeTriggered(self): + | # Handle communication """ + + if (inDataPorts.size > 0) { + timeTriggered = + st"""${timeTriggered} + | ${exampleUsage} + """ + } + + if (examplePublishers.nonEmpty) { + timeTriggered = + st"""${timeTriggered} + | # Example publishing messages + | ${(examplePublishers, "\n")}""" + } + return timeTriggered } - def genPyUserNodePyFile(packageName: String, component: AadlThread, strictAADLMode: B): (ISZ[String], ST) = { - val nodeName = component.pathAsString("_") + def genPyUserDataPortInitializers(inDataPorts: ISZ[AadlPort], packageName: String, + datatypeMap: Map[AadlType, (String, ISZ[String])], reporter: Reporter): ISZ[ST] = { + var initializers: ISZ[ST] = IS() + + for (p <- inDataPorts) { + val portDatatype = genPortDatatype(p, packageName, datatypeMap, reporter) + val portName = p.identifier + + initializers = initializers :+ + st"""${portName} = ${portDatatype}() + |self.init_${portName}(${portName}) + """ + } + + return initializers + } + + def genPyUserNodePyFile(packageName: String, component: AadlThread, datatypeMap: Map[AadlType, (String, ISZ[String])], + hasConverterFiles: B, strictAADLMode: B, reporter: Reporter): (ISZ[String], ST, B, ISZ[Marker]) = { + val nodeName = genNodeName(component) val fileName = genPyNodeSourceName(nodeName) + var examplePublishers: ISZ[ST] = IS() + var inDataPorts: ISZ[AadlPort] = IS() + var msgTypes: ISZ[String] = IS() + + val startMarker: String = "# Additions within these tags will be preserved when re-running Codegen" + val endMarker: String = "# Additions within these tags will be preserved when re-running Codegen" + + for (p <- component.getPorts()) { + if (p.direction == Direction.Out) { + examplePublishers = examplePublishers :+ genPyExamplePublisher(p, packageName, datatypeMap, reporter) + } + else if (p.direction == Direction.In && p.isInstanceOf[AadlDataPort]) { + inDataPorts = inDataPorts :+ p + } + + val portDatatype: String = genPortDatatype(p, packageName, datatypeMap, reporter) + if (!ISZOps(msgTypes).contains(portDatatype)) { + msgTypes = msgTypes :+ portDatatype + } + } var subscriptionHandlers: ISZ[ST] = IS() if (isSporadic(component)) { + var firstSubscriptionHandler: B = true + for (p <- component.getPorts()) { - // TODO: Datatypes + val portDatatype: String = genPortDatatype(p, packageName, datatypeMap, reporter) if (p.direction == Direction.In && !p.isInstanceOf[AadlDataPort]) { if (strictAADLMode) { - subscriptionHandlers = subscriptionHandlers :+ - genPySubscriptionHandlerSporadicStrict(p, nodeName) + if (firstSubscriptionHandler) { + subscriptionHandlers = subscriptionHandlers :+ + genPySubscriptionHandlerSporadicStrictWithExamples(p, portDatatype, inDataPorts) + firstSubscriptionHandler = false + } else { + subscriptionHandlers = subscriptionHandlers :+ + genPySubscriptionHandlerSporadicStrict(p, portDatatype) + } } else { - subscriptionHandlers = subscriptionHandlers :+ - genPySubscriptionHandlerSporadic(p, nodeName) + if (firstSubscriptionHandler) { + subscriptionHandlers = subscriptionHandlers :+ + genPySubscriptionHandlerSporadicWithExamples(p, portDatatype, inDataPorts) + firstSubscriptionHandler = false + } else { + subscriptionHandlers = subscriptionHandlers :+ + genPySubscriptionHandlerSporadic(p, portDatatype) + } } } } } else { - subscriptionHandlers = subscriptionHandlers :+ genPyTimeTriggeredMethod(nodeName) + subscriptionHandlers = subscriptionHandlers :+ genPyTimeTriggeredMethod(inDataPorts, examplePublishers, strictAADLMode) } - val fileBody = + val inDataPortInitializers: ISZ[ST] = genPyUserDataPortInitializers(inDataPorts, packageName, datatypeMap, reporter) + + val typeIncludes: ISZ[ST] = genPyFileMsgTypeIncludes(packageName, msgTypes) + + var includeFiles: ST = st"""#!/usr/bin/env python3 |import rclpy |from rclpy.node import Node - |//================================================= - |// I n i t i a l i z e E n t r y P o i n t - |//================================================= - |def initialize(self) - |{ - | PRINT_INFO("Initialize Entry Point invoked"); + |from rosidl_runtime_py.convert import message_to_yaml + |from ${packageName}.base_code.${nodeName}_base_src import ${nodeName}_base""" + + if (typeIncludes.size != 0) { + includeFiles = + st"""${includeFiles} + |${(typeIncludes, "\n")}""" + } + + if (hasConverterFiles) { + includeFiles = + st"""${includeFiles} + |from ${packageName}.base_code.enum_converter import *""" + } + + var fileBody = + st"""${includeFiles} + | + |#=========================================================== + |# This file will not be overwritten when re-running Codegen + |#=========================================================== + |class ${nodeName}(${nodeName}_base): + | def __init__(self): + | super().__init__() + | # invoke initialize entry point + | self.initialize() + | + | self.get_logger().info("${nodeName} infrastructure set up") | - | // Initialize the node - |} + |#================================================= + |# I n i t i a l i z e E n t r y P o i n t + |#================================================= + | def initialize(self): + | self.get_logger().info("Initialize Entry Point invoked") + | + | # Initialize the node + | + | # Initialize the node's incoming data port values here + """ + + if (inDataPortInitializers.size != 0) { + fileBody = + st"""${fileBody} + | # Initialize the node's incoming data port values here + | ${(inDataPortInitializers, "\n")}""" + } + + fileBody = + st"""${fileBody} + | + |#================================================= + |# C o m p u t e E n t r y P o i n t + |#================================================= + | ${genPyMessageToString()} + | ${(subscriptionHandlers, "\n")} + """ + + fileBody = + st"""${fileBody} + |#================================================= + |# Include any additional declarations here + |#================================================= + |${startMarker} | - |//================================================= - |// C o m p u t e E n t r y P o i n t - |//================================================= - |${(subscriptionHandlers, "\n")} + |${endMarker} """ - val filePath: ISZ[String] = IS("src", packageName, "src", "user_code", fileName) + val filePath: ISZ[String] = IS("src", packageName, packageName, "user_code", fileName) - return (filePath, fileBody) + return (filePath, fileBody, F, IS()) } def genPyNodeRunnerName(compNameS: String): String = { @@ -924,84 +1683,172 @@ object GeneratorPy { return nodeNameT } - def genPyNodeRunnerFile(packageName: String, component: AadlThread): (ISZ[String], ST) = { - val nodeName = component.pathAsString("_") + def genPyNodeRunnerFile(packageName: String, component: AadlThread): (ISZ[String], ST, B, ISZ[Marker]) = { + val nodeName = genNodeName(component) val fileName = genPyNodeRunnerName(nodeName) val fileBody = st"""#!/usr/bin/env python3 |import rclpy |from rclpy.node import Node - |//================================================= - |// D O N O T E D I T T H I S F I L E - |//================================================= + |from rclpy.executors import MultiThreadedExecutor + |from ${packageName}.user_code.${nodeName}_src import ${nodeName} + |#======================================================== + |# Re-running Codegen will overwrite changes to this file + |#======================================================== + |def main(args=None): + | rclpy.init(args=args) + | node = ${nodeName}() + | executor = MultiThreadedExecutor() + | executor.add_node(node) + | executor.spin() + | rclpy.shutdown() | - |${nodeName}::${nodeName}() : ${nodeName}_base() - |{ - | // Invoke initialize entry point - | initialize(); - | - | PRINT_INFO("${nodeName} infrastructure set up"); - |} - | - |int main(int argc, char **argv) - |{ - | rclcpp::init(argc, argv); - | auto executor = rclcpp::executors::MultiThreadedExecutor(); - | auto node = std::make_shared<${nodeName}>(); - | executor.add_node(node); - | executor.spin(); - | rclcpp::shutdown(); - | return 0; - |} + |if __name__ == "__main__": + | main() """ - val filePath: ISZ[String] = IS("src", packageName, "src", "base_code", fileName) + val filePath: ISZ[String] = IS("src", packageName, packageName, "base_code", fileName) - return (filePath, fileBody) + return (filePath, fileBody, T, IS()) } def genPyNodeFiles(modelName: String, threadComponents: ISZ[AadlThread], connectionMap: Map[ISZ[String], ISZ[ISZ[String]]], - strictAADLMode: B): ISZ[(ISZ[String], ST)] = { + datatypeMap: Map[AadlType, (String, ISZ[String])], hasConverterFiles: B, strictAADLMode: B, + invertTopicBinding: B, reporter: Reporter): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { val top_level_package_nameT: String = genPyPackageName(modelName) - var py_files: ISZ[(ISZ[String], ST)] = IS() + var py_files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() for (comp <- threadComponents) { py_files = - py_files :+ genPyBaseNodePyFile(top_level_package_nameT, comp, connectionMap, strictAADLMode) + py_files :+ genPyBaseNodePyFile(top_level_package_nameT, comp, connectionMap, datatypeMap, strictAADLMode, invertTopicBinding, reporter) + py_files = + py_files :+ genPyUserNodePyFile(top_level_package_nameT, comp, datatypeMap, hasConverterFiles, strictAADLMode, reporter) py_files = - py_files :+ genPyUserNodePyFile(top_level_package_nameT, comp, strictAADLMode) - py_files :+ genPyNodeRunnerFile(top_level_package_nameT, comp) + py_files :+ genPyNodeRunnerFile(top_level_package_nameT, comp) } return py_files } - //================================================ - // P a c k a g e G e n e r a t o r s - //================================================ + def genPyEnumConverters(enumTypes: ISZ[(String, AadlType)], strictAADLMode: B): ISZ[ST] = { + var converters: ISZ[ST] = IS() - // TODO: Python pkgs - def genPyNodePkg(modelName: String, threadComponents: ISZ[AadlThread], connectionMap: Map[ISZ[String], ISZ[ISZ[String]]], - strictAADLMode: B): ISZ[(ISZ[String], ST)] = { - var files: ISZ[(ISZ[String], ST)] = IS() + for (enum <- enumTypes) { + val enumName: String = ops.StringOps(enum._2.classifier.apply(enum._2.classifier.size - 1)).replaceAllLiterally("_", "") + val enumValues: ISZ[String] = enum._2.asInstanceOf[EnumType].values - files = files :+ genPyFormatLaunchFile(modelName, threadComponents) - files = files :+ genPySetupFile(modelName, threadComponents) + var cases: ISZ[ST] = IS() + + for (value <- enumValues) { + cases = cases :+ + st"""case ${enumName}.${StringOps(enum._1).toUpper}_${StringOps(value).toUpper}: + | return "${enumName} ${value}"""" + } + + //Only difference was a pointer? + if (strictAADLMode) { + converters = converters :+ + st"""def enumToString(value): + | typedValue = ${enumName}() + | typedValue.data = value + | match (typedValue.${enum._1}): + | ${(cases, "\n")} + | case default: + | return "Unknown value for ${enumName}" + """ + } + else { + converters = converters :+ + st"""def enumToString(value): + | typedValue = ${enumName}() + | typedValue.data = value + | match (typedValue.${enum._1}): + | ${(cases, "\n")} + | case default: + | return "Unknown value for ${enumName}" + """ + } + } - for(file <- genPyNodeFiles(modelName, threadComponents, connectionMap, strictAADLMode)) { - files = files :+ file + return converters + } + + def genPyEnumConverterFile(packageName: String, enumTypes: ISZ[(String, AadlType)], + strictAADLMode: B): (ISZ[String], ST, B, ISZ[Marker]) = { + var includes: ISZ[ST] = IS() + + for (enum <- enumTypes) { + val enumName: String = ops.StringOps(enum._2.classifier.apply(enum._2.classifier.size - 1)).replaceAllLiterally("_", "") + includes = includes :+ st"from ${packageName}_interfaces.msg import ${enumName}" } + val fileBody = + st"""#!/usr/bin/env python3 + |${(includes, "\n")} + | + |#======================================================== + |# Re-running Codegen will overwrite changes to this file + |#======================================================== + | + |${(genPyEnumConverters(enumTypes, strictAADLMode), "\n")} + """ + + val filePath: ISZ[String] = IS("src", packageName, packageName, "base_code", "enum_converter.py") + + return (filePath, fileBody, T, IS()) + } + + def genPyEnumConverterFiles(modelName: String, datatypeMap: Map[AadlType, (String, ISZ[String])], + strictAADLMode: B): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + var enumTypes: ISZ[(String, AadlType)] = IS() + + for (key <- datatypeMap.keys) { + key match { + case _: EnumType => + val datatype: String = datatypeMap.get(key).get._2.apply(0) + val datatypeName: String = StringOps(datatype).substring(StringOps(datatype).indexOf(' ') + 1, datatype.size) + enumTypes = enumTypes :+ (datatypeName, key) + case x => + } + } + + if (enumTypes.size == 0) { + return IS() + } + + var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + val packageName: String = genPyPackageName(modelName) + + files = files :+ genPyEnumConverterFile(packageName, enumTypes, strictAADLMode) + return files } - // TODO: Python pkgs - def genPyLaunchPkg(modelName: String, threadComponents: ISZ[AadlThread]): ISZ[(ISZ[String], ST)] = { - var files: ISZ[(ISZ[String], ST)] = IS() + //================================================ + // P a c k a g e G e n e r a t o r s + //================================================ + def genPyNodePkg(modelName: String, threadComponents: ISZ[AadlThread], connectionMap: Map[ISZ[String], ISZ[ISZ[String]]], + datatypeMap: Map[AadlType, (String, ISZ[String])], strictAADLMode: B, invertTopicBinding: B, + reporter: Reporter): ISZ[(ISZ[String], ST, B, ISZ[Marker])] = { + var files: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = IS() + + val converterFiles: ISZ[(ISZ[String], ST, B, ISZ[Marker])] = genPyEnumConverterFiles(modelName, datatypeMap, strictAADLMode) + val hasConverterFiles: B = (converterFiles.size > 0) + val top_level_package_nameT: String = genPyPackageName(modelName) + + files = files ++ converterFiles + files = files ++ genPyNodeFiles(modelName, threadComponents, connectionMap, datatypeMap, hasConverterFiles, strictAADLMode, + invertTopicBinding, reporter) - // TODO - //files = files :+ genXmlFormatLaunchFile(modelName, threadComponents) - files = files :+ genLaunchCMakeListsFile(modelName) - files = files :+ genLaunchPackageFile(modelName) + files = files :+ genPyInitFile(top_level_package_nameT) + files = files :+ genPySubInitFile(modelName, "base_code") + files = files :+ genPySubInitFile(modelName, "user_code") + files = files :+ genPyResourceFile(modelName) + files = files :+ genPySetupFile(modelName, threadComponents) + files = files :+ genXmlPackageFile(modelName) + files = files :+ genCfgSetupFile(modelName) + files = files :+ genPyCopyrightFile(modelName) + files = files :+ genPyFlakeFile(modelName) + files = files :+ genPyPrepFile(modelName) return files } diff --git a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Ros2Codegen.scala b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Ros2Codegen.scala index ca18665a..123537f7 100644 --- a/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Ros2Codegen.scala +++ b/shared/src/main/scala/org/sireum/hamr/codegen/ros2/Ros2Codegen.scala @@ -49,17 +49,19 @@ object Ros2Codegen { case "Cpp" => files = Generator.genCppNodePkg(modelName, threadComponents, connectionMap, datatypeMap, options.strictAadlMode, options.invertTopicBinding, reporter) - //case "Python" => files = Generator.genPyNodePkg(modelName, threadComponents, connectionMap, options.strictAadlMode) + case "Python" => + files = GeneratorPy.genPyNodePkg(modelName, threadComponents, connectionMap, datatypeMap, options.strictAadlMode, + options.invertTopicBinding, reporter) case _ => reporter.error(None(), toolName, s"Unknown code type: ${options.ros2NodesLanguage.name}") } options.ros2LaunchLanguage.name match { - case "Xml" => files = files ++ Generator.genXmlLaunchPkg(modelName, threadComponents, systemComponents) - case "Python" => files = files ++ Generator.genPyLaunchPkg(modelName, threadComponents) + case "Xml" => files = files ++ GeneratorLaunch.genXmlLaunchPkg(modelName, threadComponents, systemComponents, options.ros2NodesLanguage.name) + case "Python" => files = files ++ GeneratorLaunch.genPyLaunchPkg(modelName, threadComponents, systemComponents, options.ros2NodesLanguage.name) case _ => reporter.error(None(), toolName, s"Unknown code type: ${options.ros2NodesLanguage.name}") } - files = files ++ Generator.genInterfacesPkg(modelName, datatypeMap) + files = files ++ GeneratorInterfaces.genInterfacesPkg(modelName, datatypeMap, options.ros2NodesLanguage.name) for (file <- files) { var filePath: String = ""