├── .editorconfig ├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md └── workflows │ ├── build_test.yml │ ├── deploy_docs.yml │ └── lint_docs.yml ├── .gitignore ├── .markdownlint.json ├── .pre-commit-config.yaml ├── CHANGELOG.rst ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.rst ├── Soar ├── main.soar └── wait.soar ├── doc ├── About.rst ├── ClassDiagram.md ├── Images │ ├── THA_FEE_Logo_S_Red_RGB.svg │ ├── ai_prod_network.png │ ├── bayern_innovativ.svg │ ├── hta.png │ ├── soar_ros_icon_black.svg │ ├── soar_ros_icon_default.svg │ ├── soar_ros_icon_slogan_black.svg │ ├── soar_ros_icon_slogan_default.svg │ ├── soar_ros_icon_slogan_white.svg │ ├── soar_ros_icon_white.svg │ ├── soar_ros_slogan_black.svg │ ├── soar_ros_slogan_default.svg │ ├── soar_ros_slogan_white.svg │ ├── stmwi.png │ └── stmwk.png ├── ROSInterface.md ├── SoftwareArchitecture.md ├── TestAgent.md ├── _static │ └── custom.css └── conf.py ├── include └── soar_ros │ ├── Client.hpp │ ├── Interface.hpp │ ├── Publisher.hpp │ ├── SafeQueue.hpp │ ├── Service.hpp │ ├── SoarRunner.hpp │ ├── Subscriber.hpp │ └── soar_ros.hpp ├── launch └── soar_ros.launch.py ├── package.xml ├── requirements.txt ├── src ├── SoarRunner.cpp └── soar_ros.cpp └── test ├── test_launch.py ├── test_launch_client.py └── test_soar_ros.cpp /.editorconfig: -------------------------------------------------------------------------------- 1 | [*] 2 | end_of_line = lf 3 | trim_trailing_whitespace = true 4 | charset = utf-8 5 | indent_style = space 6 | indent_size = 4 7 | insert_final_newline = true 8 | tab_width = 4 9 | 10 | [*.soar] 11 | tab_width = 3 12 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Environment** 27 | - OS: [e.g. Ubuntu/ Mac/ Windows] 28 | - Version [e.g. 22] 29 | - Docker 30 | 31 | **Additional context** 32 | Add any other context about the problem here. 33 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.github/workflows/build_test.yml: -------------------------------------------------------------------------------- 1 | name: Build and test run for the package 2 | run-name: build_test 3 | 4 | on: 5 | pull_request: 6 | workflow_dispatch: 7 | 8 | jobs: 9 | humble: 10 | runs-on: ubuntu-latest 11 | container: 12 | image: ubuntu:jammy 13 | steps: 14 | - uses: ros-tooling/setup-ros@v0.7 15 | - uses: ros-tooling/action-ros-ci@v0.3 16 | with: 17 | package-name: soar_ros 18 | target-ros2-distro: humble 19 | jazzy: 20 | runs-on: ubuntu-latest 21 | container: 22 | image: ubuntu:noble 23 | steps: 24 | - uses: ros-tooling/setup-ros@v0.7 25 | - uses: ros-tooling/action-ros-ci@v0.3 26 | with: 27 | package-name: soar_ros 28 | target-ros2-distro: jazzy 29 | rolling: 30 | runs-on: ubuntu-latest 31 | container: 32 | image: ubuntu:latest 33 | steps: 34 | - uses: ros-tooling/setup-ros@v0.7 35 | - uses: ros-tooling/action-ros-ci@v0.3 36 | with: 37 | package-name: soar_ros 38 | target-ros2-distro: rolling 39 | -------------------------------------------------------------------------------- /.github/workflows/deploy_docs.yml: -------------------------------------------------------------------------------- 1 | name: Build and Deploy ROS2 Documentation 2 | 3 | on: 4 | push: 5 | tags: 6 | - "*" 7 | workflow_dispatch: 8 | 9 | permissions: 10 | contents: write 11 | 12 | jobs: 13 | build-docs: 14 | runs-on: ubuntu-latest 15 | 16 | steps: 17 | # Step 1: Check out the repository 18 | - name: Checkout repository 19 | uses: actions/checkout@v3 20 | 21 | - uses: ssciwr/doxygen-install@v1 22 | with: 23 | version: "1.12.0" 24 | 25 | # Step 3: Install dependencies (rosdoc2 and doxygen) 26 | - name: Install ROS2 and documentation dependencies 27 | run: | 28 | sudo apt-get update 29 | sudo apt-get install -y graphviz 30 | pip install -r requirements.txt 31 | 32 | # Step 4: Build the documentation using rosdoc2 33 | - name: Build documentation 34 | run: | 35 | rosdoc2 build --package-path . 36 | 37 | # Step 5: Deploy the documentation to the gh-pages branch 38 | - name: Deploy to GitHub Pages 39 | uses: peaceiris/actions-gh-pages@v4 40 | with: 41 | github_token: ${{ secrets.GITHUB_TOKEN }} 42 | publish_dir: ./docs_output/soar_ros/ 43 | publish_branch: gh-pages 44 | -------------------------------------------------------------------------------- /.github/workflows/lint_docs.yml: -------------------------------------------------------------------------------- 1 | name: Lint docs 2 | 3 | on: 4 | pull_request: 5 | workflow_dispatch: 6 | 7 | jobs: 8 | docs_linter: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - uses: actions/checkout@master 12 | with: 13 | fetch-depth: 0 14 | 15 | - uses: gaurav-nelson/github-action-markdown-link-check@v1 16 | with: 17 | check-modified-files-only: "yes" 18 | base-branch: main 19 | 20 | - uses: tj-actions/changed-files@v41 21 | id: changed-files 22 | with: 23 | files: "**/*.md" 24 | separator: "," 25 | 26 | - uses: DavidAnson/markdownlint-cli2-action@v14 27 | continue-on-error: false 28 | if: steps.changed-files.outputs.any_changed == 'true' 29 | with: 30 | globs: ${{ steps.changed-files.outputs.all_changed_files }} 31 | separator: "," 32 | config: ".markdownlint.json" 33 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cross_reference/ 2 | docs_build/ 3 | docs_output/ 4 | 5 | # Generated by dynamic reconfigure 6 | *.cfgc 7 | /cfg/cpp/ 8 | /cfg/*.py 9 | 10 | # Ignore generated docs 11 | *.dox 12 | *.wikidoc 13 | 14 | # eclipse stuff 15 | .project 16 | .cproject 17 | 18 | # qcreator stuff 19 | CMakeLists.txt.user 20 | 21 | srv/_*.py 22 | *.pcd 23 | *.pyc 24 | qtcreator-* 25 | *.user 26 | 27 | /planning/cfg 28 | /planning/docs 29 | /planning/src 30 | 31 | *~ 32 | 33 | # Emacs 34 | .#* 35 | 36 | # Catkin custom files 37 | CATKIN_IGNORE 38 | -------------------------------------------------------------------------------- /.markdownlint.json: -------------------------------------------------------------------------------- 1 | { 2 | "MD007": { 3 | "indent": 4 4 | }, 5 | "MD040": true, 6 | "MD046": { 7 | "style": "fenced" 8 | }, 9 | "MD047": true, 10 | "MD013": { 11 | "tables": false, 12 | "code_blocks": false, 13 | "strict": false 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v2.1.0 4 | hooks: 5 | - id: end-of-file-fixer 6 | - id: trailing-whitespace 7 | 8 | - repo: https://github.com/compilerla/conventional-pre-commit 9 | rev: v3.2.0 10 | hooks: 11 | - id: conventional-pre-commit 12 | stages: [commit-msg] 13 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package soar_ros 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.2 (2024-12-17) 6 | ------------------ 7 | * ci: add jazzy and rolling tests. Disable flaky client test 8 | * docs: Add logos and update images. 9 | * fix: Stop SoarRunner Kernel thread on error. 10 | * ci: Change docs build to 1.12 Doxygen version. 11 | * build: Make ament_index_cpp dependency explicit 12 | * docs: Fix typos and add Test Agent description. 13 | * build: Fix tests and restrict testing to launch testing. 14 | * ci: Publish subdirectory of rosdoc2 build process 15 | * Contributors: Moritz Schmidt, dependabot[bot] 16 | 17 | 0.0.1 (2024-09-13) 18 | ------------------ 19 | * Initial commit 20 | * Contributors: Moritz Schmidt 21 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(soar_ros) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | set(CMAKE_CXX_STANDARD 20) 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | 13 | set(THIS_PACKAGE_INCLUDE_DEPENDS 14 | rclcpp 15 | std_msgs 16 | std_srvs 17 | ament_index_cpp 18 | SQLite3 19 | ) 20 | 21 | find_package(example_interfaces REQUIRED) 22 | 23 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 24 | find_package(${Dependency} REQUIRED) 25 | endforeach() 26 | 27 | 28 | include(FetchContent) 29 | 30 | FetchContent_Declare( 31 | soar 32 | GIT_REPOSITORY https://github.com/moschmdt/Soar.git 33 | GIT_TAG cmake 34 | ) 35 | 36 | FetchContent_MakeAvailable(soar) 37 | 38 | add_library(soar_ros src/soar_ros.cpp src/SoarRunner.cpp) 39 | target_include_directories(soar_ros 40 | PUBLIC 41 | "$" 42 | "$") 43 | ament_target_dependencies(soar_ros rclcpp std_msgs std_srvs ament_index_cpp) 44 | target_link_libraries(soar_ros soar_lib) 45 | set_target_properties(soar_ros 46 | PROPERTIES CMAKE_CXX_STANDARD 20) 47 | 48 | add_executable(test_example test/test_soar_ros.cpp) 49 | ament_target_dependencies(test_example ${THIS_PACKAGE_INCLUDE_DEPENDS} example_interfaces) 50 | target_link_libraries(test_example soar_ros) 51 | set_target_properties(test_example 52 | PROPERTIES CMAKE_CXX_STANDARD 20) 53 | 54 | 55 | if(BUILD_TESTING) 56 | find_package(launch_testing_ament_cmake) 57 | add_launch_test(test/test_launch.py) 58 | # add_launch_test(test/test_launch_client.py) 59 | endif() 60 | 61 | install(TARGETS test_example 62 | DESTINATION lib/${PROJECT_NAME}) 63 | 64 | install( 65 | DIRECTORY include/ 66 | DESTINATION include/${PROJECT_NAME} 67 | ) 68 | 69 | install( 70 | TARGETS soar_ros soar_lib 71 | EXPORT export_${PROJECT_NAME} 72 | LIBRARY DESTINATION lib 73 | ARCHIVE DESTINATION lib 74 | RUNTIME DESTINATION bin 75 | ) 76 | 77 | install(DIRECTORY 78 | Soar/ 79 | DESTINATION share/${PROJECT_NAME}/Soar 80 | ) 81 | 82 | install(DIRECTORY 83 | launch 84 | DESTINATION share/${PROJECT_NAME} 85 | ) 86 | 87 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 88 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 89 | 90 | ament_package() 91 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | You are very welcome to contribute to the project by creating issues or 4 | submitting a pull-request. Please follow standard contributing guidelines, for 5 | instance (non extensive list): 6 | 7 | - Check for existing issues before creating a new one 8 | - Check with maintainers before starting to work on an issue or submit a PR in 9 | order to prevent duplicate work 10 | - Commit messages correspond to the changes 11 | 12 | ## Mandatory Checks 13 | 14 | The changelog is generated based on commit messages and therefore the 15 | [conventional commit](https://www.conventionalcommits.org/en/v1.0.0/) style is 16 | enforced. Please ensure that the commit messages are formatted accordingly. 17 | 18 | There is a [pre-commit](https://pre-commit.com/) file to ensure compliance if 19 | you prefer to use a tool. 20 | 21 | ## Code Style 22 | 23 | The project follows the [ROS2 coding style](https://docs.ros.org/en/rolling/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html). 24 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | # License 2 | 3 | Apache License 4 | Version 2.0, January 2004 5 | 6 | 7 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 8 | 9 | 1. Definitions. 10 | 11 | "License" shall mean the terms and conditions for use, reproduction, 12 | and distribution as defined by Sections 1 through 9 of this document. 13 | 14 | "Licensor" shall mean the copyright owner or entity authorized by 15 | the copyright owner that is granting the License. 16 | 17 | "Legal Entity" shall mean the union of the acting entity and all 18 | other entities that control, are controlled by, or are under common 19 | control with that entity. For the purposes of this definition, 20 | "control" means (i) the power, direct or indirect, to cause the 21 | direction or management of such entity, whether by contract or 22 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 23 | outstanding shares, or (iii) beneficial ownership of such entity. 24 | 25 | "You" (or "Your") shall mean an individual or Legal Entity 26 | exercising permissions granted by this License. 27 | 28 | "Source" form shall mean the preferred form for making modifications, 29 | including but not limited to software source code, documentation 30 | source, and configuration files. 31 | 32 | "Object" form shall mean any form resulting from mechanical 33 | transformation or translation of a Source form, including but 34 | not limited to compiled object code, generated documentation, 35 | and conversions to other media types. 36 | 37 | "Work" shall mean the work of authorship, whether in Source or 38 | Object form, made available under the License, as indicated by a 39 | copyright notice that is included in or attached to the work 40 | (an example is provided in the Appendix below). 41 | 42 | "Derivative Works" shall mean any work, whether in Source or Object 43 | form, that is based on (or derived from) the Work and for which the 44 | editorial revisions, annotations, elaborations, or other modifications 45 | represent, as a whole, an original work of authorship. For the purposes 46 | of this License, Derivative Works shall not include works that remain 47 | separable from, or merely link (or bind by name) to the interfaces of, 48 | the Work and Derivative Works thereof. 49 | 50 | "Contribution" shall mean any work of authorship, including 51 | the original version of the Work and any modifications or additions 52 | to that Work or Derivative Works thereof, that is intentionally 53 | submitted to Licensor for inclusion in the Work by the copyright owner 54 | or by an individual or Legal Entity authorized to submit on behalf of 55 | the copyright owner. For the purposes of this definition, "submitted" 56 | means any form of electronic, verbal, or written communication sent 57 | to the Licensor or its representatives, including but not limited to 58 | communication on electronic mailing lists, source code control systems, 59 | and issue tracking systems that are managed by, or on behalf of, the 60 | Licensor for the purpose of discussing and improving the Work, but 61 | excluding communication that is conspicuously marked or otherwise 62 | designated in writing by the copyright owner as "Not a Contribution." 63 | 64 | "Contributor" shall mean Licensor and any individual or Legal Entity 65 | on behalf of whom a Contribution has been received by Licensor and 66 | subsequently incorporated within the Work. 67 | 68 | 2. Grant of Copyright License. Subject to the terms and conditions of 69 | this License, each Contributor hereby grants to You a perpetual, 70 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 71 | copyright license to reproduce, prepare Derivative Works of, 72 | publicly display, publicly perform, sublicense, and distribute the 73 | Work and such Derivative Works in Source or Object form. 74 | 75 | 3. Grant of Patent License. Subject to the terms and conditions of 76 | this License, each Contributor hereby grants to You a perpetual, 77 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 78 | (except as stated in this section) patent license to make, have made, 79 | use, offer to sell, sell, import, and otherwise transfer the Work, 80 | where such license applies only to those patent claims licensable 81 | by such Contributor that are necessarily infringed by their 82 | Contribution(s) alone or by combination of their Contribution(s) 83 | with the Work to which such Contribution(s) was submitted. If You 84 | institute patent litigation against any entity (including a 85 | cross-claim or counterclaim in a lawsuit) alleging that the Work 86 | or a Contribution incorporated within the Work constitutes direct 87 | or contributory patent infringement, then any patent licenses 88 | granted to You under this License for that Work shall terminate 89 | as of the date such litigation is filed. 90 | 91 | 4. Redistribution. You may reproduce and distribute copies of the 92 | Work or Derivative Works thereof in any medium, with or without 93 | modifications, and in Source or Object form, provided that You 94 | meet the following conditions: 95 | 96 | (a) You must give any other recipients of the Work or 97 | Derivative Works a copy of this License; and 98 | 99 | (b) You must cause any modified files to carry prominent notices 100 | stating that You changed the files; and 101 | 102 | (c) You must retain, in the Source form of any Derivative Works 103 | that You distribute, all copyright, patent, trademark, and 104 | attribution notices from the Source form of the Work, 105 | excluding those notices that do not pertain to any part of 106 | the Derivative Works; and 107 | 108 | (d) If the Work includes a "NOTICE" text file as part of its 109 | distribution, then any Derivative Works that You distribute must 110 | include a readable copy of the attribution notices contained 111 | within such NOTICE file, excluding those notices that do not 112 | pertain to any part of the Derivative Works, in at least one 113 | of the following places: within a NOTICE text file distributed 114 | as part of the Derivative Works; within the Source form or 115 | documentation, if provided along with the Derivative Works; or, 116 | within a display generated by the Derivative Works, if and 117 | wherever such third-party notices normally appear. The contents 118 | of the NOTICE file are for informational purposes only and 119 | do not modify the License. You may add Your own attribution 120 | notices within Derivative Works that You distribute, alongside 121 | or as an addendum to the NOTICE text from the Work, provided 122 | that such additional attribution notices cannot be construed 123 | as modifying the License. 124 | 125 | You may add Your own copyright statement to Your modifications and 126 | may provide additional or different license terms and conditions 127 | for use, reproduction, or distribution of Your modifications, or 128 | for any such Derivative Works as a whole, provided Your use, 129 | reproduction, and distribution of the Work otherwise complies with 130 | the conditions stated in this License. 131 | 132 | 5. Submission of Contributions. Unless You explicitly state otherwise, 133 | any Contribution intentionally submitted for inclusion in the Work 134 | by You to the Licensor shall be under the terms and conditions of 135 | this License, without any additional terms or conditions. 136 | Notwithstanding the above, nothing herein shall supersede or modify 137 | the terms of any separate license agreement you may have executed 138 | with Licensor regarding such Contributions. 139 | 140 | 6. Trademarks. This License does not grant permission to use the trade 141 | names, trademarks, service marks, or product names of the Licensor, 142 | except as required for reasonable and customary use in describing the 143 | origin of the Work and reproducing the content of the NOTICE file. 144 | 145 | 7. Disclaimer of Warranty. Unless required by applicable law or 146 | agreed to in writing, Licensor provides the Work (and each 147 | Contributor provides its Contributions) on an "AS IS" BASIS, 148 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 149 | implied, including, without limitation, any warranties or conditions 150 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 151 | PARTICULAR PURPOSE. You are solely responsible for determining the 152 | appropriateness of using or redistributing the Work and assume any 153 | risks associated with Your exercise of permissions under this License. 154 | 155 | 8. Limitation of Liability. In no event and under no legal theory, 156 | whether in tort (including negligence), contract, or otherwise, 157 | unless required by applicable law (such as deliberate and grossly 158 | negligent acts) or agreed to in writing, shall any Contributor be 159 | liable to You for damages, including any direct, indirect, special, 160 | incidental, or consequential damages of any character arising as a 161 | result of this License or out of the use or inability to use the 162 | Work (including but not limited to damages for loss of goodwill, 163 | work stoppage, computer failure or malfunction, or any and all 164 | other commercial damages or losses), even if such Contributor 165 | has been advised of the possibility of such damages. 166 | 167 | 9. Accepting Warranty or Additional Liability. While redistributing 168 | the Work or Derivative Works thereof, You may choose to offer, 169 | and charge a fee for, acceptance of support, warranty, indemnity, 170 | or other liability obligations and/or rights consistent with this 171 | License. However, in accepting such obligations, You may act only 172 | on Your own behalf and on Your sole responsibility, not on behalf 173 | of any other Contributor, and only if You agree to indemnify, 174 | defend, and hold each Contributor harmless for any liability 175 | incurred by, or claims asserted against, such Contributor by reason 176 | of your accepting any such warranty or additional liability. 177 | 178 | END OF TERMS AND CONDITIONS 179 | 180 | ## Soar 181 | 182 | The Soar license can be found on the 183 | [project homepage](https://github.com/SoarGroup/Soar/blob/development/LICENSE.md). 184 | 185 | Copyright © 2000-2012. The Regents of University of Michigan. 186 | Copyright © 1995-1999. Carnegie Mellon University, The Regents of University 187 | of Michigan, University of Southern California/Information Sciences Institute. 188 | All rights reserved. 189 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | soar_ros: A ROS 2 Interface for Soar 2 | ==================================== 3 | 4 | .. figure:: doc/Images/soar_ros_slogan_default.svg 5 | :alt: logo 6 | 7 | This ROS2 package provides an interface for the Soar cognitive 8 | architecture by creating wrappers for ROS2 messages and handling the 9 | Soar kernel in a `continuos 10 | mode `__. 11 | 12 | `Soar `__ is a cognitive architecture 13 | developed at the University of Michigan. It is used in the field of 14 | cognitive robotics in different projects, e.g. a 15 | `drone `__ or a 16 | `robot `__. 17 | However, the integration of Soar and `ROS 2 `__ is 18 | currently difficult for complex projects, which include multiple 19 | publishers, subscribers, services or clients. The main limitation 20 | orginates from the synchronous callback model used by Soar which 21 | inspired the creation of this wrapper. A detailed explanation about the 22 | reason for the development of the package can be read in the `software 23 | architecture <./doc/SoftwareArchitecture.md>`__. 24 | 25 | The package relies on a forked version of Soar. The major changes 26 | include a ``cmake``-based build instead of ``scons`` and removal of SWIG 27 | language interfaces. For a detailed comparison have a look at the commit 28 | history of the `fork `__. 29 | 30 | Features 31 | -------- 32 | 33 | The library is developed targeting ROS 2 Humble on Ubuntu 22.04. Other 34 | configurations were not tested. It provides 35 | 36 | - Non blocking Soar kernel 37 | - Publisher 38 | - Subscriber 39 | - Service 40 | - Client 41 | 42 | The following features are **not supported**, yet. 43 | 44 | - Action Server and Client 45 | - Multiple Soar agents 46 | 47 | Definition and description of the public API 48 | -------------------------------------------- 49 | 50 | The API documentation is generated via 51 | `rosdoc2 `__, cf. 52 | `how to build documentation <#how-to-build-documentation>`__. 53 | 54 | Examples 55 | -------- 56 | 57 | The following examples are an extract of the test cases in 58 | `test/test_soar_ros.cpp <./test/test_soar_ros.cpp>`__. 59 | 60 | Publisher 61 | ~~~~~~~~~ 62 | 63 | The ``soar_ros::Publisher`` extends the ROS ``Publisher`` so the user 64 | only needs to define how data are converted between ROS data types and 65 | Soar data types. 66 | 67 | .. code:: cpp 68 | 69 | class TestOutput : public soar_ros::Publisher 70 | { 71 | public: 72 | TestOutput(sml::Agent * agent, rclcpp::Node::SharedPtr node, const std::string & topic) 73 | : Publisher(agent, node, topic) {} 74 | ~TestOutput() {} 75 | std_msgs::msg::String parse(sml::Identifier * id) override 76 | { 77 | std_msgs::msg::String msg; 78 | msg.data = id->GetParameterValue("data"); 79 | std::cout << id->GetCommandName() << " " << msg.data << std::endl; 80 | return msg; 81 | } 82 | }; 83 | 84 | Service 85 | ~~~~~~~ 86 | 87 | In the following example, the ROS2 example ``AddTwoInts`` is 88 | implemented. Soar adds two integers and sends the result as a ROS2 89 | Service, based on the ``soar_ros::Service`` class. The code is from 90 | `test/test_soar_ros.cpp <./test/test_soar_ros.cpp>`__. 91 | 92 | .. code:: cpp 93 | 94 | class TestService : public soar_ros::Service 95 | { 96 | public: 97 | TestService(sml::Agent * agent, rclcpp::Node::SharedPtr node, const std::string & topic) 98 | : Service(agent, node, topic) {} 99 | ~TestService() {} 100 | 101 | example_interfaces::srv::AddTwoInts::Response::SharedPtr parse(sml::Identifier * id) override 102 | { 103 | example_interfaces::srv::AddTwoInts::Response::SharedPtr response = 104 | std::make_shared(); 105 | auto sum = id->GetParameterValue("sum"); 106 | int32_t num = std::stoi(sum); 107 | response.get()->sum = num; 108 | RCLCPP_INFO(m_node->get_logger(), "Computed: Sum=%ld", response.get()->sum); 109 | std::cout << "Sum=" << response.get()->sum << std::endl; 110 | return response; 111 | } 112 | 113 | void parse(example_interfaces::srv::AddTwoInts::Request::SharedPtr msg) override 114 | { 115 | sml::Identifier * il = getAgent()->GetInputLink(); 116 | sml::Identifier * pId = il->CreateIdWME("AddTwoInts"); 117 | pId->CreateIntWME("a", msg.get()->a); 118 | pId->CreateIntWME("b", msg.get()->b); 119 | } 120 | } 121 | 122 | A second step is required to actually make this interface available - 123 | adding it to the node similar to a builder pattern, cf. 124 | `tes_soar_ros.cpp `__. 125 | 126 | .. code:: cpp 127 | 128 | auto node = std::make_shared("Test Agent", soar_path); 129 | 130 | std::shared_ptr> service = 131 | std::make_shared(node.get()->getAgent(), node, "AddTwoInts"); 132 | node->addService(service); 133 | 134 | node->startThread(); 135 | 136 | rclcpp::executors::MultiThreadedExecutor executor; 137 | executor.add_node(node); 138 | executor.spin(); 139 | 140 | These rules use an operator to add two integer numbers and provide the 141 | sum at the output link. The following rules are availabe in 142 | `main.soar `__. 143 | 144 | .. code:: soar 145 | 146 | sp {any*propose*add_two_ints 147 | (state ^io.input-link.AddTwoInts ) 148 | -( ^status complete) 149 | --> 150 | ( ^operator + =) 151 | ( ^name add_two_ints 152 | ^pAddTwoInts ) 153 | } 154 | 155 | sp {any*apply*add_two_ints 156 | (state ^operator 157 | ^io.output-link
    ) 158 | ( ^name add_two_ints 159 | ^pAddTwoInts ) 160 | ( ^a 161 | ^b ) 162 | --> 163 | (
      ^AddTwoInts.sum (+ )) 164 | ( ^status complete) 165 | } 166 | 167 | How to build and install 168 | ------------------------ 169 | 170 | **Prerequisite**: A ROS2 installation is available. 171 | 172 | 1. Clone this repository in your workspace 173 | 174 | 2. Build via ``colcon build --packages-select soar_ros`` 175 | 176 | 3. Source the ROS workspace 177 | 178 | 4. Run the test executable via ``ros2 run soar_ros test_example``. The 179 | output should look similar to the following: 180 | 181 | .. code:: shell 182 | 183 | $ ros2 run soar_ros test_example 184 | [INFO] [1721823668.516038530] [SoarRunner]: Starting runThread 185 | [INFO] [1721823668.516344554] [SoarRunner]: Test Agent: 1: O: O1 (init-agent) 186 | [INFO] [1721823668.516466911] [SoarRunner]: Test Agent: 2: ==>S: S2 (state no-change) 187 | [WARN] [1721823669.516121281] [SoarRunner]: AddTwoIntsClient service not available, waiting again... 188 | 189 | .. Warning:: 190 | If you would like to use the Java-based debugger, the 191 | installation of the official Soar release is requried: Download and 192 | install the latest Soar release from their 193 | `repository `__. Setting the 194 | ``SOAR_HOME`` environment variable to the ``bin/`` directory of the 195 | insalltion could help to open the debugger. 196 | 197 | How to build and run tests 198 | -------------------------- 199 | 200 | The packages relies on the ``colcon test`` procedure, including launch 201 | testing which is automatically triggered by ``colcon test``. 202 | 203 | .. code:: shell 204 | 205 | colcon test 206 | colcon test-result --verbose 207 | 208 | How to build documentation 209 | -------------------------- 210 | 211 | The documentation is generated via 212 | `rosdoc2 `__. Execute 213 | the following commands in the cloned repository or adjust the path of 214 | ``rosdoc2 build`` accordingly. 215 | 216 | .. code:: shell 217 | 218 | rosdoc2 build --package-path . 219 | rosdoc2 open docs_output/soar_ros/index.html 220 | 221 | How to develop 222 | -------------- 223 | 224 | Clone the package in your ROS2 workspace. 225 | 226 | Usage 227 | ----- 228 | 229 | Include this package as dependency in your ``CMakeLists.txt`` and clone 230 | it your ROS2 workspace. 231 | 232 | .. code:: cmake 233 | 234 | find_package(soar_ros REQUIRED) 235 | ament_target_dependencies( soar_ros) 236 | 237 | For code references have a look at the `examples <#examples>`__. 238 | 239 | License 240 | ------- 241 | 242 | Licensed under the Apache License, Version 2.0 (the “License”); you may 243 | not use this file except in compliance with the License. You may obtain 244 | a copy of the License at 245 | 246 | http://www.apache.org/licenses/LICENSE-2.0 247 | 248 | Unless required by applicable law or agreed to in writing, software 249 | distributed under the License is distributed on an “AS IS” BASIS, 250 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 251 | See the License for the specific language governing permissions and 252 | limitations under the License 253 | 254 | Refer to the `license `__ file. The respective Soar license is 255 | available at their `repository `__. 256 | 257 | Acknowledgements 258 | ---------------- 259 | 260 | This project was developed as part of the `AI Production Network 261 | Augsburg `__ funded by the 262 | `Bavarian State Ministry of Science and the 263 | Arts `__ and the `Bavarian 264 | Ministry of Economic Affairs, Regional Development and 265 | Energy `__, cf. `about <./doc/About.rst>`__. 266 | 267 | `Imprint `__ & 268 | `privacy `__ 269 | -------------------------------------------------------------------------------- /Soar/main.soar: -------------------------------------------------------------------------------- 1 | # If the superstate is nil and the states' name is not TestAgent, then propose an 2 | # operator to name the state TestAgent. 3 | sp {topstate*propose*init-agent 4 | (state ^superstate nil 5 | -^name) 6 | --> 7 | ( ^operator +) 8 | ( ^name init-agent) 9 | } 10 | 11 | sp {topstate*apply*init-agent 12 | (state ^operator.name init-agent) 13 | --> 14 | ( ^name TestAgent) 15 | } 16 | 17 | source wait.soar 18 | 19 | sp {any*propose*test_republisher 20 | (state ^io.input-link.testinput ) 21 | -( ^status complete) 22 | --> 23 | ( ^operator + =) 24 | ( ^name test_republisher 25 | ^pMessage ) 26 | } 27 | 28 | sp {any*apply*test_republisher 29 | (state ^operator 30 | ^io.output-link
        ) 31 | ( ^name test_republisher 32 | ^pMessage ) 33 | ( ^data ) 34 | --> 35 | (
          ^test.data ) 36 | ( ^status complete) 37 | } 38 | 39 | sp {any*propose*add_two_ints_test_server 40 | (state ^io.input-link.AddTwoInts ) 41 | -( ^status complete) 42 | --> 43 | ( ^operator + =) 44 | ( ^name add_two_ints 45 | ^pAddTwoInts ) 46 | } 47 | 48 | sp {any*apply*add_two_ints_test_server 49 | (state ^operator 50 | ^io.output-link
            ) 51 | ( ^name add_two_ints 52 | ^pAddTwoInts ) 53 | ( ^a 54 | ^b ) 55 | --> 56 | (
              ^AddTwoInts.sum (+ )) 57 | ( ^status complete) 58 | } 59 | 60 | sp {any*propose*add_two_ints_client 61 | (state ^io.input-link.Trigger ) 62 | ( -^status complete) 63 | --> 64 | ( ^operator +) 65 | ( ^name add_two_ints_client 66 | ^Trigger ) 67 | } 68 | 69 | sp {any*apply*add_two_ints_client 70 | (state ^operator 71 | ^io.output-link
                ) 72 | ( ^name add_two_ints_client 73 | ^Trigger ) 74 | --> 75 | (
                  ^AddTwoIntsClient ) 76 | ( ^a 6 ^b 6) 77 | ( ^status complete) 78 | } 79 | -------------------------------------------------------------------------------- /Soar/wait.soar: -------------------------------------------------------------------------------- 1 | # Put in a wait for a state-no-change 2 | sp {top-state*propose*wait 3 | (state ^attribute state 4 | ^choices none 5 | -^operator.name wait) 6 | --> 7 | ( ^operator ) 8 | ( ^name wait) 9 | } 10 | 11 | # This avoids a operator no-change after wait is selected 12 | # I've included it just to keep the trace simple 13 | sp {top-state*apply*wait*random 14 | (state ^operator ) 15 | ( ^name wait) 16 | --> 17 | ( ^random elaboration) 18 | } 19 | 20 | sp {anystate*prefer*others*over*wait 21 | (state ^operator + 22 | ^operator +) 23 | ( ^name <> wait) 24 | ( ^name wait) 25 | --> 26 | ( ^operator > ) 27 | } 28 | -------------------------------------------------------------------------------- /doc/About.rst: -------------------------------------------------------------------------------- 1 | About 2 | ----- 3 | 4 | .. image:: Images/THA_FEE_Logo_S_Red_RGB.svg 5 | :alt: THA Faculty of Electrical Engineering 6 | :width: 30% 7 | 8 | .. image:: Images/stmwi.png 9 | :alt: Bavarian Ministry of Economic Affairs, Regional Development and Energy 10 | :width: 30% 11 | 12 | .. image:: Images/stmwk.png 13 | :alt: Bavarian State Ministry of Science and the Arts 14 | :width: 30% 15 | 16 | .. image:: Images/bayern_innovativ.svg 17 | :alt: Bayern Innovativ 18 | :width: 30% 19 | 20 | .. image:: Images/hta.png 21 | :alt: Hightech Agenda Bavaria 22 | :width: 30% 23 | 24 | .. image:: Images/ai_prod_network.png 25 | :alt: AI Production Network Augsburg 26 | :width: 30% 27 | -------------------------------------------------------------------------------- /doc/ClassDiagram.md: -------------------------------------------------------------------------------- 1 | # Class Diagrams 2 | 3 | The following class diagram of the soar_ros::Service class (inheritance diagram) 4 | provides a basic overview of the library architecture. 5 | 6 | All ROS wrapper classes adjusted for usage with Soar are based on the input or 7 | output base classes as well as the shared interface. 8 | 9 | ```{eval-rst} 10 | .. doxygenclass:: soar_ros::Service 11 | :allow-dot-graphs: 12 | ``` 13 | 14 | The difference between service and client from the libraries perspective is 15 | only the reversal of output and input calls. 16 | 17 | ```{eval-rst} 18 | .. doxygenclass:: soar_ros::Client 19 | :allow-dot-graphs: 20 | ``` 21 | 22 | Publisher and subscriber only have either the input or the output functionality: 23 | 24 | ```{eval-rst} 25 | .. doxygenclass:: soar_ros::Publisher 26 | :allow-dot-graphs: 27 | ``` 28 | 29 | ```{eval-rst} 30 | .. doxygenclass:: soar_ros::Subscriber 31 | :allow-dot-graphs: 32 | ``` 33 | -------------------------------------------------------------------------------- /doc/Images/THA_FEE_Logo_S_Red_RGB.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 8 | 9 | 12 | 13 | 14 | 15 | 18 | 20 | 22 | 23 | 25 | 26 | 28 | 30 | 31 | 32 | 35 | 38 | 40 | 41 | 42 | 45 | 48 | 49 | 50 | 52 | 55 | 56 | 58 | 61 | 64 | 65 | 66 | 68 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /doc/Images/ai_prod_network.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THA-Embedded-Systems-Lab/soar_ros/0354080a69adc6ad540a6d4583480d0fc479c03f/doc/Images/ai_prod_network.png -------------------------------------------------------------------------------- /doc/Images/bayern_innovativ.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /doc/Images/hta.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THA-Embedded-Systems-Lab/soar_ros/0354080a69adc6ad540a6d4583480d0fc479c03f/doc/Images/hta.png -------------------------------------------------------------------------------- /doc/Images/soar_ros_icon_black.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /doc/Images/soar_ros_icon_default.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /doc/Images/soar_ros_icon_white.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /doc/Images/soar_ros_slogan_black.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /doc/Images/soar_ros_slogan_white.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /doc/Images/stmwi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THA-Embedded-Systems-Lab/soar_ros/0354080a69adc6ad540a6d4583480d0fc479c03f/doc/Images/stmwi.png -------------------------------------------------------------------------------- /doc/Images/stmwk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/THA-Embedded-Systems-Lab/soar_ros/0354080a69adc6ad540a6d4583480d0fc479c03f/doc/Images/stmwk.png -------------------------------------------------------------------------------- /doc/ROSInterface.md: -------------------------------------------------------------------------------- 1 | # ROS Interface 2 | 3 | ## Soar Kernel Status 4 | 5 | ```{eval-rst} 6 | .. doxygenfunction:: soar_ros::SoarRunner::getSoarKernelStatus 7 | ``` 8 | 9 | ## Run Soar Kernel 10 | 11 | ```{eval-rst} 12 | .. doxygenfunction:: soar_ros::SoarRunner::runSoarKernel 13 | ``` 14 | 15 | ## Stop Soar Kernel 16 | 17 | ```{eval-rst} 18 | .. doxygenfunction:: soar_ros::SoarRunner::stopSoarKernel 19 | ``` 20 | 21 | ## Launch Soar Debugger 22 | 23 | ```{eval-rst} 24 | .. doxygenfunction:: soar_ros::SoarRunner::debuggerLaunch 25 | ``` 26 | -------------------------------------------------------------------------------- /doc/SoftwareArchitecture.md: -------------------------------------------------------------------------------- 1 | # Software Architecture 2 | 3 | The rationale behind this library is also simplifying the use of Soar via ROS2 4 | while the API should remain as simple as possible. The challenges behind 5 | connecting Soar and ROS2 lies in the software architecture of Soar: synchronous 6 | callbacks and events. A detailed explanation of the Soar architecture is 7 | provided on their website in the [Soar manual][soar_manual] or the [Soar 8 | Threading Model][soar_threads]. 9 | 10 | The user should only be required to do two things: Initialize the Soar 11 | kernel with an Agent and add adapted publisher, subscriber, services and 12 | clients afterwards. For each message/ topic, the conversion between Soar 13 | working memory elements (WMEs) and ROS2 message, or vice versa, types must be 14 | implemented manually. 15 | 16 | - The kernel is wrapped in a single class called `SoarRunner`. The main 17 | responsibility is to create, start and maintain the Soar kernel. 18 | 19 | - Publisher, Subscribers, Service and Clients are added via a separate functions 20 | to the ROS2 node. 21 | 22 | ```{mermaid} 23 | :zoom: %% enable for rosdoc2 feature, requires sphinxcontrib.mermaid feature. 24 | sequenceDiagram 25 | box Soar 26 | participant SoarRunner 27 | participant SoarRunner-RunThread 28 | participant SoarKernel 29 | end 30 | 31 | box ROS2 32 | participant Client 33 | participant Client-RunThread 34 | participant ClientInputQueue 35 | participant ClientOutputQueue 36 | end 37 | 38 | SoarRunner ->> SoarKernel: CreateKernelInNewThread() 39 | activate SoarKernel; 40 | activate SoarRunner 41 | 42 | %% INITIALIZE CLIENT 43 | SoarRunner ->> Client : AddClient(client) 44 | activate Client 45 | Client ->> Client-RunThread: InitializeRun 46 | deactivate Client 47 | activate Client-RunThread 48 | par ClientRun 49 | note right of Client: Start client worker thread. 50 | loop Client run thread 51 | Client-RunThread ->>+ ClientInputQueue: try read 52 | ClientInputQueue ->>- Client-RunThread: Element 53 | Client-RunThread ->> Client-RunThread: sent ROS2 request 54 | Client-RunThread ->> Client-RunThread: await ROS2 response 55 | Client-RunThread ->> Client-RunThread: future.get() 56 | Client-RunThread ->> ClientOutputQueue: push(response) 57 | deactivate Client-RunThread 58 | end 59 | and Agent run 60 | note right of SoarRunner: Start agent worker thread. 61 | SoarRunner ->> SoarRunner-RunThread: Run() 62 | deactivate SoarRunner 63 | activate SoarRunner-RunThread 64 | loop continue == true 65 | SoarRunner-RunThread ->> SoarRunner-RunThread: RunAllAgentsForever() 66 | end 67 | deactivate SoarRunner-RunThread 68 | and Kernel run 69 | note right of SoarRunner: Callback for Event is called. 70 | SoarKernel ->> SoarRunner: smlEVENT_AFTER_ALL_OUTPUT_PHASES callback to UpdateWorld() 71 | deactivate SoarKernel 72 | activate SoarRunner 73 | SoarRunner ->> SoarRunner: processOutputLinkChanges() 74 | SoarRunner ->> SoarRunner: processInput() 75 | SoarRunner ->> SoarKernel: UpdateWorld() complete 76 | activate SoarKernel 77 | deactivate SoarRunner 78 | end 79 | 80 | note right of SoarRunner: Example for Client call from Soar: Process output 81 | SoarKernel ->>+ SoarRunner: smlEVENT_AFTER_ALL_OUTPUT_PHASES callback to UpdateWorld() 82 | deactivate SoarKernel; 83 | activate SoarRunner 84 | SoarRunner ->> SoarRunner: outputs[output-link.command] = shared_ptr output 85 | SoarRunner ->>+ Client: process_s2r() 86 | deactivate SoarRunner 87 | Client ->> ClientInputQueue: queue.push(parse(sml:Identifier *)) 88 | note right of ClientInputQueue: Client run thread reads and processes queue. 89 | Client ->>- SoarRunner: void 90 | activate SoarRunner 91 | SoarRunner ->> SoarRunner: output-link.command.AddStatusComplete() 92 | SoarRunner ->> SoarRunner: processInput() 93 | SoarRunner ->>- SoarKernel: UpdateWorld() complete 94 | activate SoarKernel; 95 | 96 | loop SoarKernel and SoarRunner not blocked 97 | SoarKernel ->>+ SoarRunner: smlEVENT_AFTER_ALL_OUTPUT_PHASES callback to UpdateWorld() 98 | SoarRunner ->>- SoarKernel: UpdateWorld() complete 99 | end 100 | 101 | SoarKernel ->> SoarRunner: smlEVENT_AFTER_ALL_OUTPUT_PHASES callback to UpdateWorld() 102 | activate SoarRunner; 103 | deactivate SoarKernel; 104 | SoarRunner ->> SoarRunner: processOutputLinkChanges() 105 | note right of Client: Check if response is available. 106 | SoarRunner ->>+ Client: Process input 107 | deactivate SoarRunner; 108 | Client ->>+ ClientOutputQueue: Read Queue 109 | ClientOutputQueue ->>- Client: Element 110 | Client ->> Client: parse() 111 | Client ->>- SoarRunner: void 112 | activate SoarRunner 113 | SoarRunner ->> SoarRunner: agent.commit() 114 | SoarRunner ->> SoarKernel: UpdateWorld() complete 115 | activate SoarKernel; 116 | 117 | deactivate SoarKernel; 118 | deactivate SoarRunner; 119 | ``` 120 | 121 | [soar_manual]: https://soar.eecs.umich.edu/soar_manual/ 122 | [soar_threads]: https://soar.eecs.umich.edu/development/soar/ThreadsInSML/ 123 | -------------------------------------------------------------------------------- /doc/TestAgent.md: -------------------------------------------------------------------------------- 1 | # Soar Test Agent for Development 2 | 3 | The `test_example` program is a minimum working example of a Soar agent 4 | integrated in ROS2 for communicating. It is used for testing but also as an 5 | example on how to set up the project structure. 6 | 7 | The tests can be executed either via `colcon test` or 8 | `launch_test test/test_launch.py`. 9 | 10 | **Known Bug**: Soar does not exit cleanly after unit tests and the `test_example` 11 | program must be terminated manually. 12 | -------------------------------------------------------------------------------- /doc/_static/custom.css: -------------------------------------------------------------------------------- 1 | .wy-nav-content { 2 | max-width: 80%; 3 | } 4 | -------------------------------------------------------------------------------- /doc/conf.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Moritz Schmidt 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Configuration file for the Sphinx documentation builder. 16 | # This was copied from a generated conf.py, and flake8 does not like it. 17 | # flake8: noqa 18 | # 19 | # This file only contains a selection of the most common options. For a full 20 | # list see the documentation: 21 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 22 | 23 | # -- Project information ----------------------------------------------------- 24 | 25 | project = 'soar_ros' 26 | copyright = 'The soar_ros Contributers. License: Apache License 2.0' 27 | author = 'Moritz Schmidt' 28 | 29 | # The full version, including alpha/beta/rc tags 30 | release = '0.0.0' 31 | 32 | version = '0.0' 33 | 34 | 35 | # -- General configuration --------------------------------------------------- 36 | 37 | # Add any Sphinx extension module names here, as strings. They can be 38 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 39 | # ones. 40 | ## rosdoc2 will extend the extensions to enable Breathe and Exhale if you 41 | ## do not add them here, as well as others, perhaps. 42 | ## If you add them manually rosdoc2 may still try to configure them. 43 | ## See the rosdoc2_settings below for some options on avoiding that. 44 | extensions = [ 45 | 'sphinx_rtd_theme', 46 | 'sphinxcontrib.mermaid', 47 | 'sphinx.ext.intersphinx', 48 | 'sphinx.ext.graphviz', 49 | 'breathe' 50 | ] 51 | 52 | # Add any paths that contain templates here, relative to this directory. 53 | templates_path = ['_templates'] 54 | 55 | # List of patterns, relative to source directory, that match files and 56 | # directories to ignore when looking for source files. 57 | # This pattern also affects html_static_path and html_extra_path. 58 | exclude_patterns = [] 59 | 60 | master_doc = 'index' 61 | html_logo = 'Images/soar_ros_icon_white.svg' 62 | html_favicon = 'Images/soar_ros_icon_default.svg' 63 | 64 | source_suffix = { 65 | '.rst': 'restructuredtext', 66 | '.md': 'markdown', 67 | '.markdown': 'markdown', 68 | } 69 | 70 | # -- Options for HTML output ------------------------------------------------- 71 | 72 | # The theme to use for HTML and HTML Help pages. See the documentation for 73 | # a list of builtin themes. 74 | # 75 | ## rosdoc2 will override the theme, but you may set one here for running Sphinx 76 | ## without the rosdoc2 tool. 77 | html_theme = 'sphinx_rtd_theme' 78 | 79 | html_theme_options = { 80 | # Toc options 81 | 'collapse_navigation': False, 82 | 'sticky_navigation': True, 83 | 'navigation_depth': -1, 84 | 'includehidden': True, 85 | 'titles_only': False, 86 | } 87 | 88 | intersphinx_mapping = {'std_srvs': ('http://docs.ros.org/en/humble/p/std_srvs/', None)} 89 | intersphinx_disabled_reftypes = ["*"] 90 | 91 | # Add the custom CSS file to the HTML context 92 | def setup(app): 93 | app.add_css_file('custom.css') 94 | 95 | # Add any paths that contain custom static files (such as style sheets) here, 96 | # relative to this directory. They are copied after the builtin static files, 97 | # so a file named "default.css" will overwrite the builtin "default.css". 98 | ## rosdoc2 comments this out by default because we're not creating it. 99 | html_static_path = ['_static'] 100 | 101 | # -- Options for rosdoc2 ----------------------------------------------------- 102 | 103 | ## These settings are specific to rosdoc2, and if Sphinx is run without rosdoc2 104 | ## they will be safely ignored. 105 | ## None are required by default, so the lines below show the default values, 106 | ## therefore you will need to uncomment the lines and change their value 107 | ## if you want change the behavior of rosdoc2. 108 | rosdoc2_settings = { 109 | ## This setting, if True, will ensure breathe is part of the 'extensions', 110 | ## and will set all of the breathe configurations, if not set, and override 111 | ## settings as needed if they are set by this configuration. 112 | # 'enable_breathe': True, 113 | 114 | ## This setting, if True, will ensure exhale is part of the 'extensions', 115 | ## and will set all of the exhale configurations, if not set, and override 116 | ## settings as needed if they are set by this configuration. 117 | # 'enable_exhale': True, 118 | 119 | ## This setting, if provided, allows option specification for breathe 120 | ## directives through exhale. If not set, exhale defaults will be used. 121 | ## If an empty dictionary is provided, breathe defaults will be used. 122 | # 'exhale_specs_mapping': {}, 123 | 124 | ## This setting, if True, will ensure autodoc is part of the 'extensions'. 125 | # 'enable_autodoc': True, 126 | 127 | ## This setting, if True, will ensure intersphinx is part of the 'extensions'. 128 | # 'enable_intersphinx': True, 129 | 130 | ## This setting, if True, will have the 'html_theme' overridden to provide 131 | ## a consistent style across all of the ROS documentation. 132 | # 'override_theme': True, 133 | 134 | ## This setting, if True, will automatically extend the intersphinx mapping 135 | ## using inventory files found in the cross-reference directory. 136 | ## If false, the `found_intersphinx_mappings` variable will be in the global 137 | ## scope when run with rosdoc2, and could be conditionally used in your own 138 | ## Sphinx conf.py file. 139 | # 'automatically_extend_intersphinx_mapping': True, 140 | 141 | ## Support markdown 142 | # 'support_markdown': True, 143 | } 144 | -------------------------------------------------------------------------------- /include/soar_ros/Client.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__CLIENT_HPP_ 16 | #define SOAR_ROS__CLIENT_HPP_ 17 | 18 | namespace soar_ros 19 | { 20 | 21 | #include 22 | #include 23 | 24 | #include "Interface.hpp" 25 | #include "SafeQueue.hpp" 26 | #include "sml_Client.h" 27 | 28 | template 30 | class Client : public virtual Output, 31 | public virtual Input, 32 | public Interface 33 | { 34 | protected: 35 | typename rclcpp::Client::SharedPtr m_client; 36 | std::string m_topic; 37 | rclcpp::Node::SharedPtr m_node; 38 | sml::Agent * m_pAgent; 39 | std::thread m_send_client_requests; 40 | std::atomic isRunning; 41 | 42 | /** 43 | * @brief Periodically check the m_s2rQueue and m_r2sQueue for input and 44 | * output. 45 | * 46 | */ 47 | void run() 48 | { 49 | while (isRunning.load()) { 50 | auto timeout_duration = std::chrono::seconds(10); 51 | auto start_time = std::chrono::steady_clock::now(); 52 | while (!m_client->wait_for_service(std::chrono::seconds(1))) { 53 | if (!rclcpp::ok()) { 54 | RCLCPP_ERROR( 55 | this->m_node->get_logger(), 56 | "Interrupted while waiting for the service. Exiting."); 57 | isRunning.store(false); 58 | break; 59 | } 60 | 61 | auto current_time = std::chrono::steady_clock::now(); 62 | if (current_time - start_time >= timeout_duration) { 63 | RCLCPP_ERROR( 64 | this->m_node->get_logger(), 65 | "Timeout while waiting for service %s", m_topic.c_str()); 66 | // rclcpp::shutdown(); 67 | // break; 68 | } 69 | 70 | RCLCPP_WARN( 71 | this->m_node->get_logger(), 72 | "%s service not available, waiting again...", 73 | m_topic.c_str()); 74 | } 75 | 76 | auto msg = this->m_s2rQueue.pop(); 77 | 78 | auto result = m_client->async_send_request(msg); 79 | 80 | auto response = result.get(); 81 | this->m_r2sQueue.push(response); 82 | } 83 | } 84 | 85 | public: 86 | Client( 87 | sml::Agent * agent, rclcpp::Node::SharedPtr node, 88 | const std::string & client_name) 89 | : m_topic(client_name), m_node(node), m_pAgent(agent), isRunning(true) 90 | { 91 | m_client = m_node.get()->create_client(client_name); 92 | m_send_client_requests = std::thread(&Client::run, this); 93 | } 94 | 95 | ~Client() 96 | { 97 | isRunning.store(false); 98 | if (m_send_client_requests.joinable()) { 99 | m_send_client_requests.join(); 100 | } 101 | } 102 | 103 | virtual void parse(pResponseType msg) = 0; 104 | pRequestType parse(sml::Identifier * id) override = 0; 105 | 106 | std::string getTopic() override {return m_topic;} 107 | 108 | sml::Agent * getAgent() override {return m_pAgent;} 109 | }; 110 | } // namespace soar_ros 111 | 112 | #endif // SOAR_ROS__CLIENT_HPP_ 113 | -------------------------------------------------------------------------------- /include/soar_ros/Interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__INTERFACE_HPP_ 16 | #define SOAR_ROS__INTERFACE_HPP_ 17 | 18 | 19 | #include 20 | 21 | #include "SafeQueue.hpp" 22 | 23 | #include "sml_Client.h" 24 | 25 | namespace soar_ros 26 | { 27 | class Interface 28 | { 29 | public: 30 | /// @brief Get the topic of the subscriber 31 | /// @return 32 | virtual std::string getTopic() = 0; 33 | 34 | /// @brief Get the agent of the current interface 35 | /// @return 36 | virtual sml::Agent * getAgent() = 0; 37 | }; 38 | 39 | class OutputBase 40 | { 41 | public: 42 | /// @brief This function is called to process an output link WME and 43 | /// push it to a queue. 44 | /// @param id 45 | virtual void process_s2r(sml::Identifier * id) = 0; 46 | }; 47 | 48 | class InputBase 49 | { 50 | public: 51 | /// @brief This function reads the queue and calls the parse function of 52 | /// the input to create new WM elements via parse function, cf. Input 53 | virtual void process_r2s() = 0; 54 | }; 55 | 56 | template 57 | class Output : public OutputBase 58 | { 59 | protected: 60 | SafeQueue m_s2rQueue; 61 | 62 | public: 63 | Output() {} 64 | ~Output() {} 65 | 66 | void process_s2r(sml::Identifier * id) override 67 | { 68 | m_s2rQueue.push(parse(id)); 69 | } 70 | 71 | /// @brief Parse Soar working memory structure to a ROS message 72 | /// @param id 73 | /// @return 74 | virtual T parse(sml::Identifier * id) = 0; 75 | }; 76 | 77 | template 78 | class Input : public InputBase 79 | { 80 | protected: 81 | /// @brief 82 | SafeQueue m_r2sQueue; 83 | 84 | public: 85 | Input() {} 86 | ~Input() {} 87 | 88 | /// @brief This function must attach the ROS message T from m_r2sQueue to the 89 | /// Soar input link. 90 | /// 91 | /// Soar usually requires a `pAgent->Commit()` to send the changes, but this is 92 | /// handled inside the SoarRunner::processInput() call. 93 | /// 94 | /// The function is called inside the Interface::Input::process_r2s() function. 95 | /// 96 | /// @param msg The ROS2 message. 97 | virtual void parse(T msg) = 0; 98 | 99 | /// @brief 100 | void process_r2s() override 101 | { 102 | auto res = this->m_r2sQueue.tryPop(); 103 | if (res.has_value()) { 104 | this->parse(res.value()); 105 | } 106 | } 107 | }; 108 | 109 | } // namespace soar_ros 110 | #endif // SOAR_ROS__INTERFACE_HPP_ 111 | -------------------------------------------------------------------------------- /include/soar_ros/Publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__PUBLISHER_HPP_ 16 | #define SOAR_ROS__PUBLISHER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "Interface.hpp" 22 | #include "SafeQueue.hpp" 23 | 24 | #include "sml_Client.h" 25 | 26 | namespace soar_ros 27 | { 28 | template 29 | class Publisher : public Output, public Interface 30 | { 31 | private: 32 | std::atomic isRunning; 33 | std::thread publisher; 34 | std::string m_topic; 35 | rclcpp::Node::SharedPtr m_node; 36 | sml::Agent * m_pAgent; 37 | 38 | public: 39 | Publisher(sml::Agent * agent, rclcpp::Node::SharedPtr node, const std::string & topic) 40 | : Output(), isRunning(true), m_topic(topic), m_node(node), m_pAgent(agent) 41 | { 42 | pub = m_node->create_publisher(m_topic, 10); 43 | publisher = std::thread(&Publisher::run, this); 44 | } 45 | ~Publisher() 46 | { 47 | isRunning.store(false); 48 | if (publisher.joinable()) { 49 | publisher.join(); 50 | } 51 | } 52 | typename rclcpp::Publisher::SharedPtr pub; 53 | 54 | void run() 55 | { 56 | while (isRunning.load()) { 57 | auto msg = this->m_s2rQueue.pop(); 58 | RCLCPP_INFO(m_node->get_logger(), "Sending on %s", m_topic.c_str()); 59 | pub->publish(msg); 60 | } 61 | } 62 | 63 | T parse(sml::Identifier * id) override = 0; 64 | 65 | std::string getTopic() override 66 | { 67 | return m_topic; 68 | } 69 | 70 | sml::Agent * getAgent() override 71 | { 72 | return m_pAgent; 73 | } 74 | }; 75 | } // namespace soar_ros 76 | 77 | #endif // SOAR_ROS__PUBLISHER_HPP_ 78 | -------------------------------------------------------------------------------- /include/soar_ros/SafeQueue.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__SAFEQUEUE_HPP_ 16 | #define SOAR_ROS__SAFEQUEUE_HPP_ 17 | 18 | /** 19 | * @file SafeQueue.hpp 20 | * @brief Implementation of a thread safe queue. 21 | * @date 2023-12-08 22 | * 23 | * https://www.geeksforgeeks.org/implement-thread-safe-queue-in-c/ 24 | * 25 | */ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace soar_ros 34 | { 35 | 36 | template 37 | class SafeQueue 38 | { 39 | private: 40 | // Underlying queue 41 | std::queue m_queue; 42 | 43 | // mutex for thread synchronization 44 | std::mutex m_mutex; 45 | 46 | // Condition variable for signaling 47 | std::condition_variable m_cond; 48 | 49 | public: 50 | // Pushes an element to the queue 51 | void push(T item) 52 | { 53 | // Acquire lock 54 | std::unique_lock lock(m_mutex); 55 | 56 | // Add item 57 | m_queue.push(item); 58 | 59 | // Notify one thread that 60 | // is waiting 61 | m_cond.notify_one(); 62 | } 63 | 64 | T pop() 65 | { 66 | // acquire lock 67 | std::unique_lock lock(m_mutex); 68 | 69 | // wait until queue is not empty 70 | m_cond.wait(lock, [this]() {return !m_queue.empty();}); 71 | 72 | // retrieve item 73 | T item = m_queue.front(); 74 | m_queue.pop(); 75 | 76 | // return item 77 | return item; 78 | } 79 | 80 | std::optional tryPop() 81 | { 82 | std::unique_lock lock(m_mutex, std::try_to_lock); 83 | 84 | if (!lock.owns_lock()) { 85 | // Lock not acquired, return an empty optional 86 | return std::nullopt; 87 | } 88 | 89 | if (m_queue.empty()) { 90 | // Queue is empty, return an empty optional 91 | return std::nullopt; 92 | } 93 | 94 | T item = m_queue.front(); 95 | m_queue.pop(); 96 | return item; 97 | } 98 | }; 99 | } // namespace soar_ros 100 | 101 | #endif // SOAR_ROS__SAFEQUEUE_HPP_ 102 | -------------------------------------------------------------------------------- /include/soar_ros/Service.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__SERVICE_HPP_ 16 | #define SOAR_ROS__SERVICE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include "Interface.hpp" 24 | #include "SafeQueue.hpp" 25 | 26 | #include "sml_Client.h" 27 | 28 | namespace soar_ros 29 | { 30 | template 32 | class Service : public virtual Input, 33 | public virtual Output, 34 | public Interface 35 | { 36 | protected: 37 | typename rclcpp::Service::SharedPtr m_service; 38 | std::string m_topic; 39 | rclcpp::Node::SharedPtr m_node; 40 | sml::Agent * m_pAgent; 41 | 42 | void callback( 43 | [[maybe_unused]] const std::shared_ptr request_header, 44 | pRequestType request, pResponseType response) 45 | { 46 | // send data via createWMEs 47 | this->m_r2sQueue.push(request); 48 | // This is a blocking call! 49 | *response = *(this->m_s2rQueue.pop()); 50 | } 51 | 52 | public: 53 | Service( 54 | sml::Agent * agent, rclcpp::Node::SharedPtr node, 55 | const std::string & service_name) 56 | : m_topic(service_name), m_node(node), m_pAgent(agent) 57 | { 58 | m_service = m_node.get()->create_service( 59 | m_topic, std::bind( 60 | &Service::callback, this, std::placeholders::_1, 61 | std::placeholders::_2, std::placeholders::_3)); 62 | } 63 | ~Service() {} 64 | 65 | virtual void parse(pRequestType msg) = 0; 66 | pResponseType parse(sml::Identifier * id) override = 0; 67 | 68 | std::string getTopic() override {return m_topic;} 69 | 70 | sml::Agent * getAgent() override {return m_pAgent;} 71 | }; 72 | } // namespace soar_ros 73 | 74 | #endif // SOAR_ROS__SERVICE_HPP_ 75 | -------------------------------------------------------------------------------- /include/soar_ros/SoarRunner.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__SOARRUNNER_HPP_ 16 | #define SOAR_ROS__SOARRUNNER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "Client.hpp" 34 | #include "Interface.hpp" 35 | #include "Publisher.hpp" 36 | #include "Service.hpp" 37 | #include "Subscriber.hpp" 38 | 39 | namespace soar_ros 40 | { 41 | /// @brief Handle the soar print event via ROS logger 42 | /// @param id sml event id 43 | /// @param pSoarRunner_ reference to SoarRunner class to access the logger. 44 | /// @param pAgent Calling agent 45 | /// @param pMessage Message content 46 | void SoarPrintEventHandler([[maybe_unused]] sml::smlPrintEventId id, 47 | void * pSoarRunner_, sml::Agent * pAgent, 48 | char const * pMessagee); 49 | 50 | /// @brief Called on every loop of the Soar kernel to process input and output 51 | /// link changes 52 | /// 53 | /// Calls the SoarRunner::updateWorld() function, which in turn processes all 54 | /// outputs via SoarRunner::processOutputLinkChanges() and attaches new elements 55 | /// to the Soar input-link via SoarRunner::processInput(). 56 | /// 57 | /// @param id Unused. 58 | /// @param pSoarRunner_ Soar only provides C wrappers for their API and the 59 | /// SoarRunner structure must be referenced via static pointer casting. 60 | /// @param kernel_ptr Unused. 61 | /// @param run_flags Unused. 62 | void updateEventHandler([[maybe_unused]] sml::smlUpdateEventId id, 63 | void * pSoarRunner_, 64 | [[maybe_unused]] sml::Kernel * kernel_ptr, 65 | [[maybe_unused]] sml::smlRunFlags run_flags); 66 | 67 | /// @brief Singelton class to manage the Soar kernel thread, main ROS interface 68 | /// to run/ stop the kernel and to attach interfaces via a builder pattern, e.g. 69 | /// soar_ros::Publisher(), soar_ros::Subscriber(), soar_ros::Service() and 70 | /// soar_ros::Client(). 71 | class SoarRunner : public rclcpp::Node 72 | { 73 | private: 74 | /// @brief Reference to the Soar agent running on the SoarRunner::pKernel. 75 | sml::Agent * pAgent; 76 | 77 | /// @brief Reference to the Soar kernel instaniated in 78 | /// SoarRunner::SoarRunner() 79 | sml::Kernel * pKernel; 80 | 81 | /// @brief Reference to the thread running the Soar instance. 82 | std::thread runThread; 83 | 84 | /// @brief ROS2 Service which provides the current status of the Soar kernel 85 | /// (run/ stop) via SoarRunner::getKernelStatus() 86 | rclcpp::Service::SharedPtr m_getSoarKernelStatus; 87 | 88 | /// @brief ROS2 Service to start/ Restart the Soar kernel via 89 | /// SoarRunner::runSoarKernel() 90 | rclcpp::Service::SharedPtr m_kernelRun; 91 | 92 | /// @brief ROS2 Service to stop the Soar kernel if it is running via 93 | /// SoarRunner::stopSoarKernel() 94 | rclcpp::Service::SharedPtr m_kernelStop; 95 | 96 | /// @brief ROS2 Service to start the Soar debugger from external programs. 97 | rclcpp::Service::SharedPtr m_debuggerLaunch; 98 | 99 | /// @brief All output related ROS2 structures 100 | /// 101 | /// The key is the topic or comamnd name specified when registering new 102 | /// outputs via SoarRunner::addPublisher(), SoarRunner::addService() or any 103 | /// other class that relies on soar_ros::OutputBase. 104 | /// 105 | /// Implemented as map since the SoarRunner::processOutputLinkChanges() looks 106 | /// up the topic or command name to match the output to the Soar WME. 107 | std::map> outputs; 108 | 109 | /// @brief All input related ROS2 structures 110 | /// 111 | /// SoarRunner::inputs is not requried to be a map since there is no lookup 112 | /// based on the topics. All inputs are iterated and attached to the 113 | /// input-link of Soar. 114 | std::vector> inputs; 115 | 116 | /// @brief Variable set via ROS2 params. 117 | bool m_debug; 118 | 119 | /// @brief Permenent reference to the output link. 120 | sml::Identifier * ol; 121 | 122 | /// @brief Called in SoarRunner::updateWorld() 123 | void processOutputLinkChanges(); 124 | 125 | /// @brief Read all input queues and call the process function to 126 | /// attach the structure to the Soar input link. 127 | /// 128 | /// Called in SoarRunner::updateWorld() 129 | void processInput(); 130 | 131 | /// @brief Initialize runThread and execute pAgent->RunSelfForever() in 132 | /// separate thread. 133 | void run(); 134 | 135 | /// @brief Compute filepath for the Soar log in ROS2 ecosystem. 136 | /// @return Filepath with ISO8601 timestamp in UTC 137 | std::string getSoarLogFilePath(); 138 | 139 | /// @brief Flag to start/ stop the SoarRunner::run() function in a separate 140 | /// thread started and stopped via SoarRunner::startThread() and 141 | /// SoarRunner::stopThread() 142 | std::atomic isRunning; 143 | 144 | public: 145 | /// @brief Instantiates the Soar kernel and generates ROS2 interfaces. Further 146 | /// setup in SoarRunner::addAgent() for first agent. 147 | /// 148 | /// 149 | /// @param agent_name The agents name. 150 | /// @param path_productions Filepath to main *.soar file relative to the 151 | /// package root. 152 | /// @throw std::runtime_error on sml::Kernel setup error. 153 | SoarRunner( 154 | const std::string & agent_name, 155 | const std::string & path_productions); 156 | 157 | /// @brief Enable external control to start the debugger and stop the run 158 | /// thread via 159 | /// @verbatim embed:rst:inline :doc:`std_srvs:interfaces/srv/Trigger` 160 | /// @endverbatim via `/soar_ros/debugger/launch` 161 | /// 162 | /// The run thread is stopped automatically since the debugger is not 163 | /// responsive if the thread is running. Reason to be investigated. 164 | /// 165 | /// The thread running the Soar kernel is not automatically restarted once the 166 | /// debugger was closed. Manually restart the thread via the provided 167 | /// SoarRunner::runSoarKernel() interface. 168 | /// 169 | /// @note Launching the Soar debugger without the command requires stopping 170 | /// the kernel via SoarRunner::stopSoarKernel() related topic. Otherwise, the 171 | /// debugger will remain frozen/ unresponsive. 172 | /// 173 | /// @param request_header 174 | /// @param request 175 | /// @param response 176 | void debuggerLaunch( 177 | [[maybe_unused]] const std::shared_ptr request_header, 178 | [[maybe_unused]] std::shared_ptr request, 179 | std::shared_ptr response); 180 | 181 | /// @brief ROS2 interface to get current Soar kernel status (run/ stop) via 182 | /// @verbatim embed:rst:inline :doc:`std_srvs:interfaces/srv/Trigger` 183 | /// @endverbatim via `/soar_ros/kernel/status` 184 | /// 185 | /// @param request_header Unused. 186 | /// @param request Empty. 187 | /// @param response "Soar Kernel isRunning: true" if running, otherwise 188 | /// "Soarkernel isRunning: false". 189 | /// 190 | /// @warning This does not catch the state of the Soar kernel correctly, once 191 | /// the kernel is started/ stopped via the Soar debugger! 192 | void getSoarKernelStatus( 193 | [[maybe_unused]] const std::shared_ptr request_header, 194 | [[maybe_unused]] std::shared_ptr request, 195 | std::shared_ptr response); 196 | 197 | /// @brief ROS2 Service interface to stop Soar kernel if it is currently 198 | /// running via 199 | /// @verbatim embed:rst:inline :doc:`std_srvs:interfaces/srv/Trigger` 200 | /// @endverbatim via `/soar_ros/kernel/run` 201 | /// 202 | /// @param request_header Unused. 203 | /// @param request Empty. 204 | /// @param response "Soar kernel isRunning: true" if started succesfully, 205 | /// otherwise "Soar kernel isRunning: false". 206 | void runSoarKernel( 207 | [[maybe_unused]] const std::shared_ptr request_header, 208 | [[maybe_unused]] std::shared_ptr request, 209 | std::shared_ptr response); 210 | 211 | /// @brief ROS2 Service interface to start (run) Soar kernel if it is 212 | /// currenlty stopped. 213 | /// @verbatim embed:rst:inline :doc:`std_srvs:interfaces/srv/Trigger` 214 | /// @endverbatim via `/soar_ros/kernel/stop` 215 | /// 216 | /// @param request_header 217 | /// @param request Empty. 218 | /// @param response "Soar Kernel isRunning: false" if stopped succesfully, 219 | /// otherwise "Soarkernel isRunning: true". 220 | void stopSoarKernel( 221 | [[maybe_unused]] const std::shared_ptr request_header, 222 | [[maybe_unused]] std::shared_ptr request, 223 | std::shared_ptr response); 224 | 225 | /// @brief Add a new Soar agent and register callbacks related to ROS. 226 | /// @warning Handling multiple agents in one SoarRunner instance is not 227 | /// implemented yet! 228 | /// @param agent_name The agents name 229 | /// @param path_productions Path to the Soar source files. 230 | /// @warning The path might depend on the install location specified in cmake 231 | /// sctipts 232 | /// @throw std::runtime_error on sml::Agent::loadProductions() error. 233 | /// @return 234 | sml::Agent * addAgent( 235 | const std::string & agent_name, 236 | const std::string & path_productions); 237 | 238 | /// @brief Stops the Kernel thread and wait until joined, closes Soar Debugger 239 | /// in debug mode. 240 | ~SoarRunner(); 241 | 242 | sml::Agent * getAgent() {return pAgent;} 243 | 244 | /// @brief Adds a soar_ros::Publisher() to the SoarRunner. 245 | /// 246 | /// The output command is assumed to be the topic name of the Publisher. 247 | /// 248 | /// @tparam T ROS2 message type 249 | /// @param output 250 | /// @return 251 | template 252 | bool addPublisher(std::shared_ptr> output) 253 | { 254 | outputs[output.get()->getTopic()] = output; 255 | return true; 256 | } 257 | 258 | /// @brief Adds a soar_ros::Publisher() to the SoarRunner. 259 | /// @tparam T 260 | /// @param output 261 | /// @param commandName Matching of the Soar output-link command name, e.g. 262 | /// io.output-link.move matches to commandName "move". 263 | /// @return 264 | template 265 | bool addPublisher( 266 | std::shared_ptr> output, 267 | const std::string & commandName) 268 | { 269 | outputs[commandName] = output; 270 | return true; 271 | } 272 | 273 | template 274 | bool addSubscriber(std::shared_ptr> input) 275 | { 276 | inputs.push_back(input); 277 | return true; 278 | } 279 | 280 | /// @brief Add a new soar_ros::Service. The callback on the output link is the 281 | /// service's topic name 282 | /// @tparam T ROS2 service message type definition 283 | /// @param service 284 | /// @return 285 | template 286 | bool addService(std::shared_ptr> service) 287 | { 288 | return addService(service, service.get()->getTopic()); 289 | } 290 | 291 | /// @brief Add a new ROS2 Service with a custom callback command 292 | /// @tparam T 293 | /// @param service 294 | /// @param commandName 295 | /// @return 296 | template 297 | bool addService( 298 | std::shared_ptr> service, 299 | const std::string & commandName) 300 | { 301 | outputs[commandName] = 302 | std::static_pointer_cast(service); 303 | inputs.push_back(std::static_pointer_cast(service)); 304 | return true; 305 | } 306 | 307 | /// @brief Add a new soar_ros::Client. The callback on the output link is the 308 | /// service's topic name 309 | /// @tparam T ROS2 Service Message type definition 310 | /// @param service 311 | /// @return 312 | template 313 | bool addClient(std::shared_ptr> client) 314 | { 315 | return addClient(client, client.get()->getTopic()); 316 | } 317 | 318 | /// @brief Add a new soar_ros::Client with a custom callback command 319 | /// @tparam T 320 | /// @param service 321 | /// @param commandName 322 | /// @return 323 | template 324 | bool addClient( 325 | std::shared_ptr> client, 326 | const std::string & commandName) 327 | { 328 | outputs[commandName] = 329 | std::static_pointer_cast(client); 330 | inputs.push_back(std::static_pointer_cast(client)); 331 | return true; 332 | } 333 | 334 | /// @brief Creates a new thread based on the SoarRunner::run() function that 335 | /// executes the Soar kernel. 336 | /// 337 | /// Soar has a synchronous threading model, 338 | /// cf. https://soar.eecs.umich.edu/development/soar/ThreadsInSML/ 339 | void startThread() 340 | { 341 | // If thread is already running, skip 342 | if (isRunning.load() == true) { 343 | RCLCPP_WARN(this->get_logger(), "runThread already running"); 344 | return; 345 | } else { 346 | isRunning.store(true); 347 | } 348 | 349 | RCLCPP_INFO(this->get_logger(), "Starting runThread"); 350 | runThread = std::thread(&SoarRunner::run, this); 351 | } 352 | 353 | /// @brief Stops and joins SoarRunner::run() thread of Soar kernel via the 354 | /// SoarRunner::isRunning flag. 355 | void stopThread() 356 | { 357 | if (isRunning.load() == false) { 358 | RCLCPP_INFO(this->get_logger(), "Run thread already stopped!"); 359 | return; 360 | } 361 | 362 | isRunning.store(false); 363 | RCLCPP_WARN(this->get_logger(), "Stopping runThread"); 364 | 365 | if (runThread.joinable()) { 366 | RCLCPP_INFO(this->get_logger(), "Waiting for run thread to join"); 367 | runThread.join(); 368 | } 369 | } 370 | 371 | /// @brief Soar update World function executed every step in the Soar 372 | /// execution cycle in SoarRunner::run() 373 | /// 374 | /// Processes all outputs via SoarRunner::processOutputLinkChanges() and 375 | /// attaches new elements to the Soar input-link via 376 | /// SoarRunner::processInput(). 377 | void updateWorld() 378 | { 379 | // Read Soar output 380 | processOutputLinkChanges(); 381 | 382 | if (ol == NULL) { 383 | ol = pAgent->GetOutputLink(); 384 | RCLCPP_DEBUG(this->get_logger(), "Output link reference invalid!"); 385 | } 386 | 387 | // Write to Soar input-link 388 | processInput(); 389 | } 390 | }; // class SoarRunner 391 | 392 | } // namespace soar_ros 393 | 394 | #endif // SOAR_ROS__SOARRUNNER_HPP_ 395 | -------------------------------------------------------------------------------- /include/soar_ros/Subscriber.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__SUBSCRIBER_HPP_ 16 | #define SOAR_ROS__SUBSCRIBER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "Interface.hpp" 22 | #include "SafeQueue.hpp" 23 | 24 | #include "sml_Client.h" 25 | 26 | namespace soar_ros 27 | { 28 | template 29 | class Subscriber : public Input, public Interface 30 | { 31 | protected: 32 | typename rclcpp::Subscription::SharedPtr sub; 33 | std::string m_topic; 34 | rclcpp::Node::SharedPtr m_node; 35 | sml::Agent * m_pAgent; 36 | 37 | public: 38 | Subscriber(sml::Agent * agent, rclcpp::Node::SharedPtr node, const std::string & topic) 39 | : Input(), m_topic(topic), m_node(node), m_pAgent(agent) 40 | { 41 | sub = 42 | m_node->create_subscription( 43 | topic, 10, 44 | std::bind(&Subscriber::callback, this, std::placeholders::_1)); 45 | } 46 | ~Subscriber() {} 47 | 48 | void callback(const T & msg) 49 | { 50 | RCLCPP_INFO(m_node->get_logger(), "Received subscription msg on %s", m_topic.c_str()); 51 | this->m_r2sQueue.push(msg); 52 | } 53 | 54 | std::string getTopic() override 55 | { 56 | return m_topic; 57 | } 58 | 59 | sml::Agent * getAgent() override 60 | { 61 | return m_pAgent; 62 | } 63 | }; 64 | } // namespace soar_ros 65 | 66 | #endif // SOAR_ROS__SUBSCRIBER_HPP_ 67 | -------------------------------------------------------------------------------- /include/soar_ros/soar_ros.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SOAR_ROS__SOAR_ROS_HPP_ 16 | #define SOAR_ROS__SOAR_ROS_HPP_ 17 | 18 | #include "Interface.hpp" 19 | #include "SafeQueue.hpp" 20 | #include "SoarRunner.hpp" 21 | #include "Publisher.hpp" 22 | #include "Subscriber.hpp" 23 | #include "Service.hpp" 24 | #include "Client.hpp" 25 | 26 | 27 | #endif // SOAR_ROS__SOAR_ROS_HPP_ 28 | -------------------------------------------------------------------------------- /launch/soar_ros.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Moritz Schmidt 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument 17 | from launch_ros.actions import Node 18 | from launch.substitutions import LaunchConfiguration 19 | 20 | 21 | def generate_launch_description(): 22 | declared_arguments = [] 23 | declared_arguments.append( 24 | DeclareLaunchArgument( 25 | "debug", 26 | description="Set the system in a debug mode.", 27 | default_value="False" 28 | ) 29 | ) 30 | debug = LaunchConfiguration("debug") 31 | 32 | return LaunchDescription(declared_arguments + [ 33 | Node( 34 | package='soar_ros', 35 | executable='main', 36 | name='soar_ros', 37 | emulate_tty=True, 38 | output={'screen'}, 39 | parameters=[ 40 | {'debug': debug} 41 | ] 42 | ), 43 | ]) 44 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | soar_ros 5 | 0.0.2 6 | An interface library to connect ROS2 with the Soar cognitive architecture. 7 | Moritz Schmidt 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | example_interfaces 14 | std_msgs 15 | std_srvs 16 | launch_testing_ament_cmake 17 | ament_index_cpp 18 | 19 | libsqlite3-dev 20 | 21 | launch_ros 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pre-commit==3.* 2 | pygments @ git+https://github.com/moschmdt/pygments@feature/soar 3 | sphinxcontrib-mermaid 4 | sphinx 5 | breathe 6 | rosdoc2==0.* 7 | -------------------------------------------------------------------------------- /src/SoarRunner.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | 17 | #include "soar_ros/Publisher.hpp" 18 | #include "soar_ros/Subscriber.hpp" 19 | #include "soar_ros/Client.hpp" 20 | #include "soar_ros/Service.hpp" 21 | #include "soar_ros/SoarRunner.hpp" 22 | 23 | namespace soar_ros 24 | { 25 | void SoarRunner::processOutputLinkChanges() 26 | { 27 | // Go through all the commands we've received (if any) since we last ran 28 | // Soar. 29 | int numberCommands = pAgent->GetNumberCommands(); 30 | for (int i = 0; i < numberCommands; i++) { 31 | sml::Identifier * pId = pAgent->GetCommand(i); 32 | 33 | std::string name = pId->GetCommandName(); 34 | auto it = outputs.find(name); 35 | if (it == outputs.end()) { 36 | RCLCPP_ERROR( 37 | this->get_logger(), "No mapping for topic: %s found!", 38 | name.c_str()); 39 | continue; 40 | } 41 | it->second.get()->process_s2r(pId); 42 | pId->AddStatusComplete(); 43 | } 44 | } 45 | 46 | void SoarRunner::processInput() 47 | { 48 | for (std::shared_ptr input : inputs) { 49 | input.get()->process_r2s(); 50 | } 51 | pAgent->Commit(); 52 | } 53 | 54 | void SoarRunner::run() 55 | { 56 | while (isRunning.load()) { 57 | pAgent->RunSelf(1); 58 | } 59 | } 60 | 61 | std::string SoarRunner::getSoarLogFilePath() 62 | { 63 | auto currentTime = std::chrono::system_clock::now(); 64 | std::time_t currentTime_t = 65 | std::chrono::system_clock::to_time_t(currentTime); 66 | std::tm * currentTime_tm = std::gmtime(¤tTime_t); 67 | std::stringstream ss; 68 | ss << std::put_time(currentTime_tm, "%Y_%m_%d-%H_%M_%S"); 69 | 70 | std::string timestamp = ss.str(); 71 | 72 | std::string root_file_path = ""; 73 | 74 | const char * ros_log_dir = std::getenv("ROS_LOG_DIR"); 75 | const char * ros_home = std::getenv("ROS_HOME"); 76 | const char * home = std::getenv("HOME"); 77 | if (ros_log_dir != nullptr) { 78 | root_file_path = std::getenv("ROS_LOG_DIR"); 79 | RCLCPP_INFO_STREAM( 80 | this->get_logger(), 81 | "ROS_LOG_DIR defined: Log directory:" << root_file_path); 82 | } else if (ros_home != nullptr) { 83 | std::string ros_home_path(ros_home); 84 | root_file_path = ros_home_path + "/log/"; 85 | RCLCPP_INFO_STREAM( 86 | this->get_logger(), 87 | "ROS_HOME defined: Log directory: " << root_file_path); 88 | } else { 89 | if (home == nullptr) { 90 | RCLCPP_ERROR_STREAM( 91 | this->get_logger(), 92 | "HOME ENV not found/ defined. Log file might be " 93 | "relative to Soar Java debugger."); 94 | } else { 95 | std::string home_path(home); 96 | root_file_path = home_path + "/.ros/log/"; 97 | RCLCPP_WARN_STREAM( 98 | this->get_logger(), 99 | "No logging directory via ENV defined; Defaulting to: " 100 | << root_file_path); 101 | } 102 | } 103 | return root_file_path; 104 | } 105 | 106 | SoarRunner::SoarRunner(const std::string & agent_name, const std::string & path_productions) 107 | : rclcpp::Node("SoarRunner"), isRunning(false) 108 | { 109 | // Sometimes the error binding the listener socket to its port number (Soar) 110 | // occurs from: Origin: 111 | // https://github.com/SoarGroup/Soar/blob/9530289cc5452fc3a4c75cf5b5f4ca25cb46fde4/Core/ConnectionSML/src/sock_ListenerSocket.cpp#L146C1-L146C93 112 | // Potential reason: Soar Java Debugger is openened and has a kernel running 113 | // blocking the port. 114 | 115 | auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; 116 | param_desc.description = "Enable debug mode."; 117 | this->declare_parameter("debug", false, param_desc); 118 | m_debug = this->get_parameter("debug").as_bool(); 119 | 120 | pKernel = sml::Kernel::CreateKernelInNewThread(); 121 | if (pKernel->HadError()) { 122 | std::string err_msg = pKernel->GetLastErrorDescription(); 123 | RCLCPP_ERROR_STREAM(this->get_logger(), err_msg); 124 | throw std::runtime_error(err_msg); 125 | } 126 | 127 | pKernel->RegisterForUpdateEvent( 128 | sml::smlEVENT_AFTER_ALL_OUTPUT_PHASES, 129 | updateEventHandler, this); 130 | 131 | pAgent = addAgent(agent_name, path_productions); 132 | 133 | m_getSoarKernelStatus = this->create_service( 134 | "/soar_ros/kernel/status", 135 | std::bind( 136 | &SoarRunner::getSoarKernelStatus, this, std::placeholders::_1, 137 | std::placeholders::_2, std::placeholders::_3)); 138 | 139 | m_kernelRun = this->create_service( 140 | "/soar_ros/kernel/run", 141 | std::bind( 142 | &SoarRunner::runSoarKernel, this, std::placeholders::_1, 143 | std::placeholders::_2, std::placeholders::_3)); 144 | 145 | m_kernelStop = this->create_service( 146 | "/soar_ros/kernel/stop", 147 | std::bind( 148 | &SoarRunner::stopSoarKernel, this, std::placeholders::_1, 149 | std::placeholders::_2, std::placeholders::_3)); 150 | 151 | m_debuggerLaunch = this->create_service( 152 | "/soar_ros/debugger/launch", 153 | std::bind( 154 | &SoarRunner::debuggerLaunch, this, std::placeholders::_1, 155 | std::placeholders::_2, std::placeholders::_3)); 156 | } 157 | 158 | void SoarRunner::debuggerLaunch( 159 | [[maybe_unused]] const std::shared_ptr request_header, 160 | [[maybe_unused]] std::shared_ptr request, 161 | std::shared_ptr response) 162 | { 163 | stopThread(); 164 | if (!pAgent->SpawnDebugger()) { 165 | std::string err_msg = "Unable to open Soar Java debugger. Is Java and " 166 | "the Soar debugger installed?"; 167 | RCLCPP_ERROR_STREAM(this->get_logger(), err_msg); 168 | response->success = false; 169 | response->message = err_msg; 170 | } else { 171 | RCLCPP_INFO_STREAM(this->get_logger(), "Spawning Soar debugger."); 172 | response->success = true; 173 | } 174 | } 175 | 176 | void SoarRunner::getSoarKernelStatus( 177 | [[maybe_unused]] const std::shared_ptr request_header, 178 | [[maybe_unused]] std::shared_ptr request, 179 | std::shared_ptr response) 180 | { 181 | if (isRunning.load()) { 182 | response->message = "Soar kernel isRunning: true"; 183 | } else { 184 | response->message = "Soar kernel isRunnings: false"; 185 | } 186 | response->success = true; 187 | } 188 | 189 | void SoarRunner::runSoarKernel( 190 | [[maybe_unused]] const std::shared_ptr request_header, 191 | [[maybe_unused]] std::shared_ptr request, 192 | std::shared_ptr response) 193 | { 194 | RCLCPP_INFO(this->get_logger(), "Attempting to run Soar kernel from ROS."); 195 | startThread(); 196 | if (isRunning.load()) { 197 | response->message = "Soar kernel isRunning: true"; 198 | response->success = true; 199 | } else { 200 | response->message = "Soar kernel isRunnings: false"; 201 | response->success = false; 202 | } 203 | } 204 | 205 | void SoarRunner::stopSoarKernel( 206 | [[maybe_unused]] const std::shared_ptr request_header, 207 | [[maybe_unused]] std::shared_ptr request, 208 | std::shared_ptr response) 209 | { 210 | RCLCPP_INFO(this->get_logger(), "Attempting to stop Soar kernel from ROS."); 211 | stopThread(); 212 | if (isRunning.load()) { 213 | response->message = "Soar kernel isRunning: true"; 214 | response->success = false; 215 | } else { 216 | response->message = "Soar kernel isRunnings: false"; 217 | response->success = true; 218 | } 219 | } 220 | 221 | sml::Agent * SoarRunner::addAgent( 222 | const std::string & agent_name, 223 | const std::string & path_productions) 224 | { 225 | sml::Agent * pAgent = pKernel->CreateAgent(agent_name.c_str()); 226 | if (pKernel->HadError()) { 227 | std::string err_msg = pKernel->GetLastErrorDescription(); 228 | RCLCPP_ERROR_STREAM(this->get_logger(), err_msg); 229 | throw std::runtime_error(err_msg); 230 | } 231 | 232 | pAgent->LoadProductions(path_productions.c_str()); 233 | if (pAgent->HadError()) { 234 | std::string err_msg = pKernel->GetLastErrorDescription(); 235 | RCLCPP_ERROR_STREAM(this->get_logger(), err_msg); 236 | throw std::runtime_error(err_msg); 237 | } 238 | 239 | pAgent->RegisterForPrintEvent( 240 | sml::smlEVENT_PRINT, SoarPrintEventHandler, 241 | this); 242 | 243 | std::string filepath = getSoarLogFilePath(); 244 | std::string cmd = "output log " + filepath; 245 | pAgent->ExecuteCommandLine(cmd.c_str()); 246 | 247 | if (m_debug) { 248 | // TODO(moschmdt): Check if another debug window is still open. 249 | // Force close it prior to reopening another window. 250 | if (pAgent->SpawnDebugger() == false) { 251 | // BUG This is not triggered when another window is already open: 252 | // Error: Error binding the listener socket to its port number 253 | // Failed to create the internet listener socket. Shutting down 254 | // listener thread. 255 | RCLCPP_ERROR(this->get_logger(), "Spawning Soar debugger failed."); 256 | } 257 | 258 | pAgent->ExecuteCommandLine("watch 5"); 259 | } 260 | 261 | return pAgent; 262 | } 263 | 264 | SoarRunner::~SoarRunner() 265 | { 266 | stopThread(); 267 | 268 | pAgent->ExecuteCommandLine("output log --close"); 269 | 270 | if (m_debug) { 271 | pAgent->KillDebugger(); 272 | } 273 | pKernel->Shutdown(); 274 | delete pKernel; 275 | } 276 | 277 | void SoarPrintEventHandler( 278 | [[maybe_unused]] sml::smlPrintEventId id, 279 | void * pSoarRunner_, sml::Agent * pAgent, 280 | char const * pMessage) 281 | { 282 | SoarRunner * pSoarRunner = static_cast(pSoarRunner_); 283 | 284 | std::string str(pMessage); 285 | 286 | if (str.find("(wait)") != std::string::npos) { 287 | return; 288 | } 289 | 290 | if (str.find("O:") != std::string::npos || 291 | str.find("==>") != std::string::npos) 292 | { 293 | str.erase( 294 | std::remove_if( 295 | str.begin(), str.end(), 296 | [&](char ch) { 297 | return std::iscntrl( 298 | static_cast(ch)); 299 | }), 300 | str.end()); 301 | } 302 | 303 | std::stringstream ss(str); 304 | std::string line; 305 | 306 | while (std::getline(ss, line, '\n')) { 307 | std::string message = std::string(pAgent->GetAgentName()) + ": " + str; 308 | auto logger = pSoarRunner->get_logger(); 309 | if (str.find("System halted.") != std::string::npos) { 310 | RCLCPP_ERROR(logger, message.c_str()); 311 | pSoarRunner->stopThread(); 312 | } else { 313 | RCLCPP_INFO(logger, message.c_str()); 314 | } 315 | } 316 | } 317 | 318 | void updateEventHandler( 319 | [[maybe_unused]] sml::smlUpdateEventId id, 320 | void * pSoarRunner_, 321 | [[maybe_unused]] sml::Kernel * kernel_ptr, 322 | [[maybe_unused]] sml::smlRunFlags run_flags) 323 | { 324 | SoarRunner * pSoarRunner = static_cast(pSoarRunner_); 325 | pSoarRunner->updateWorld(); 326 | } 327 | } // namespace soar_ros 328 | -------------------------------------------------------------------------------- /src/soar_ros.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "soar_ros/soar_ros.hpp" 16 | 17 | namespace soar_ros 18 | { 19 | 20 | } 21 | -------------------------------------------------------------------------------- /test/test_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Moritz Schmidt 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import unittest 16 | import uuid 17 | 18 | import launch 19 | import launch_ros 20 | import launch_ros.actions 21 | import launch_testing.actions 22 | import launch_testing.markers 23 | import pytest 24 | 25 | import rclpy 26 | 27 | import std_msgs.msg 28 | import example_interfaces.srv 29 | 30 | 31 | @pytest.mark.launch_test 32 | def generate_test_description(): 33 | soar_ros_node = launch_ros.actions.Node( 34 | package="soar_ros", 35 | executable="test_example", 36 | shell=True, 37 | emulate_tty=True, 38 | output='screen' 39 | ) 40 | 41 | return launch.LaunchDescription([ 42 | soar_ros_node, 43 | launch_testing.actions.ReadyToTest() 44 | ]) 45 | 46 | 47 | class TestSoaos(unittest.TestCase): 48 | @classmethod 49 | def setUpClass(cls): 50 | rclpy.init() 51 | 52 | @classmethod 53 | def tearDownClass(cls): 54 | rclpy.shutdown() 55 | 56 | def setUp(self): 57 | self.node = rclpy.create_node('test_soar_ros') 58 | # The service is required by the test setup to be available. Create it, 59 | # but callback does nothing. 60 | self.srv = self.node.create_service( 61 | example_interfaces.srv.AddTwoInts, "AddTwoIntsClient", self.add_two_ints_callback) 62 | 63 | def tearDown(self): 64 | self.node.destroy_node() 65 | 66 | def test_republisher(self, proc_output): 67 | pub = self.node.create_publisher( 68 | std_msgs.msg.String, 69 | "testinput", 70 | 10 71 | ) 72 | 73 | msg = std_msgs.msg.String() 74 | msg.data = str(uuid.uuid4()) 75 | for _ in range(10): 76 | pub.publish(msg) 77 | 78 | proc_output.assertWaitFor(msg.data, timeout=2, stream='stdout') 79 | 80 | def test_service(self, proc_output): 81 | cli = self.node.create_client( 82 | example_interfaces.srv.AddTwoInts, "AddTwoInts") 83 | while not cli.wait_for_service(timeout_sec=1.0): 84 | self.get_logger().info('service not available, waiting again...') 85 | req = example_interfaces.srv.AddTwoInts.Request() 86 | req.a = 2 87 | req.b = 2 88 | future = cli.call_async(req) 89 | rclpy.spin_until_future_complete(self.node, future) 90 | 91 | proc_output.assertWaitFor("Sum=4", timeout=10, stream='stdout') 92 | 93 | def add_two_ints_callback(self, request, response): 94 | pass 95 | -------------------------------------------------------------------------------- /test/test_launch_client.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Moritz Schmidt 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import unittest 16 | 17 | import launch 18 | import launch_ros 19 | import launch_ros.actions 20 | import launch_testing.actions 21 | import launch_testing.markers 22 | import pytest 23 | 24 | import rclpy 25 | 26 | import std_msgs.msg 27 | import example_interfaces.srv 28 | import threading 29 | 30 | 31 | @pytest.mark.launch_test 32 | def generate_test_description(): 33 | soar_ros_node = launch_ros.actions.Node( 34 | package="soar_ros", 35 | executable="test_example", 36 | shell=True, 37 | emulate_tty=True, 38 | output='screen' 39 | ) 40 | 41 | return launch.LaunchDescription([ 42 | soar_ros_node, 43 | launch_testing.actions.ReadyToTest() 44 | ]) 45 | 46 | 47 | def spin_node(node, stop_event): 48 | while not stop_event.is_set(): 49 | rclpy.spin_once(node) 50 | 51 | 52 | class TestSoarRos(unittest.TestCase): 53 | @classmethod 54 | def setUpClass(cls): 55 | rclpy.init() 56 | 57 | @classmethod 58 | def tearDownClass(cls): 59 | rclpy.shutdown() 60 | 61 | def setUp(self): 62 | self.node = rclpy.create_node('test_soar_ros') 63 | self.srv = self.node.create_service( 64 | example_interfaces.srv.AddTwoInts, "AddTwoIntsClient", self.add_two_ints_callback) 65 | 66 | def tearDown(self): 67 | self.node.destroy_node() 68 | 69 | def test_client(self, proc_output): 70 | pub = self.node.create_publisher(std_msgs.msg.String, "Trigger", 10) 71 | msg = std_msgs.msg.String() 72 | msg.data = "start" 73 | 74 | # Start the event loop via spin, otherwise the service does not react. 75 | self.stop_event = threading.Event() 76 | self.spin_thread = threading.Thread( 77 | target=spin_node, args=(self.node, self.stop_event)) 78 | self.spin_thread.start() 79 | 80 | for _ in range(10): 81 | pub.publish(msg) 82 | print(f'Published Trigger message: {msg.data}') 83 | 84 | proc_output.assertWaitFor("12", timeout=5) 85 | 86 | # Stop spin thread and wait until stopped until further tests are executed. 87 | self.stop_event.set() 88 | self.spin_thread.join() 89 | 90 | def add_two_ints_callback(self, request, response): 91 | response.sum = request.a + request.b 92 | self.node.get_logger().info( 93 | f"AddTwoIntsClient a: {request.a} b: {request.b} = {response.sum}") 94 | return response 95 | -------------------------------------------------------------------------------- /test/test_soar_ros.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Moritz Schmidt 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "soar_ros/soar_ros.hpp" 22 | #include 23 | 24 | class TestOutput : public soar_ros::Publisher 25 | { 26 | public: 27 | TestOutput( 28 | sml::Agent * agent, rclcpp::Node::SharedPtr node, 29 | const std::string & topic) 30 | : Publisher(agent, node, topic) {} 31 | ~TestOutput() {} 32 | std_msgs::msg::String parse(sml::Identifier * id) override 33 | { 34 | std_msgs::msg::String msg; 35 | msg.data = id->GetParameterValue("data"); 36 | std::cout << id->GetCommandName() << " " << msg.data << std::endl; 37 | return msg; 38 | } 39 | }; 40 | 41 | class TestInput : public soar_ros::Subscriber 42 | { 43 | public: 44 | TestInput( 45 | sml::Agent * agent, rclcpp::Node::SharedPtr node, 46 | const std::string & topic) 47 | : Subscriber(agent, node, topic) {} 48 | ~TestInput() {} 49 | 50 | void parse(std_msgs::msg::String msg) override 51 | { 52 | sml::Identifier * il = this->m_pAgent->GetInputLink(); 53 | sml::Identifier * pId = il->CreateIdWME(this->m_topic.c_str()); 54 | pId->CreateStringWME("data", msg.data.c_str()); 55 | } 56 | }; 57 | 58 | class TestService 59 | : public soar_ros::Service 60 | { 61 | public: 62 | TestService( 63 | sml::Agent * agent, rclcpp::Node::SharedPtr node, 64 | const std::string & topic) 65 | : Service(agent, node, topic) {} 66 | ~TestService() {} 67 | 68 | example_interfaces::srv::AddTwoInts::Response::SharedPtr 69 | parse(sml::Identifier * id) override 70 | { 71 | example_interfaces::srv::AddTwoInts::Response::SharedPtr response = 72 | std::make_shared(); 73 | auto sum = id->GetParameterValue("sum"); 74 | int32_t num = std::stoi(sum); 75 | response.get()->sum = num; 76 | RCLCPP_INFO(m_node->get_logger(), "Computed: Sum=%ld", response.get()->sum); 77 | std::cout << "Sum=" << response.get()->sum << std::endl; 78 | return response; 79 | } 80 | 81 | void 82 | parse(example_interfaces::srv::AddTwoInts::Request::SharedPtr msg) override 83 | { 84 | sml::Identifier * il = getAgent()->GetInputLink(); 85 | sml::Identifier * pId = il->CreateIdWME("AddTwoInts"); 86 | pId->CreateIntWME("a", msg.get()->a); 87 | pId->CreateIntWME("b", msg.get()->b); 88 | } 89 | }; 90 | 91 | class TestClient 92 | : public soar_ros::Client 93 | { 94 | public: 95 | TestClient( 96 | sml::Agent * agent, rclcpp::Node::SharedPtr node, 97 | const std::string & topic) 98 | : Client(agent, node, topic) {} 99 | ~TestClient() {} 100 | 101 | example_interfaces::srv::AddTwoInts::Request::SharedPtr 102 | parse(sml::Identifier * id) override 103 | { 104 | example_interfaces::srv::AddTwoInts::Request::SharedPtr request = 105 | std::make_shared(); 106 | auto a = std::stoi(id->GetParameterValue("a")); 107 | auto b = std::stoi(id->GetParameterValue("b")); 108 | request.get()->a = a; 109 | request.get()->b = b; 110 | RCLCPP_INFO(m_node->get_logger(), "Request computation: %d + %d", a, b); 111 | return request; 112 | } 113 | 114 | void 115 | parse(example_interfaces::srv::AddTwoInts::Response::SharedPtr msg) override 116 | { 117 | sml::Identifier * il = getAgent()->GetInputLink(); 118 | sml::Identifier * pId = il->CreateIdWME("AddTwoIntsClient"); 119 | pId->CreateIntWME("sum", msg.get()->sum); 120 | RCLCPP_INFO(m_node->get_logger(), "Result: %ld", msg.get()->sum); 121 | } 122 | }; 123 | 124 | class Trigger : public soar_ros::Subscriber 125 | { 126 | public: 127 | Trigger( 128 | sml::Agent * agent, rclcpp::Node::SharedPtr node, 129 | const std::string & topic) 130 | : Subscriber(agent, node, topic) {} 131 | ~Trigger() {} 132 | 133 | void parse(std_msgs::msg::String msg) override 134 | { 135 | sml::Identifier * il = this->m_pAgent->GetInputLink(); 136 | sml::Identifier * pId = il->CreateIdWME(this->m_topic.c_str()); 137 | pId->CreateStringWME("data", msg.data.c_str()); 138 | } 139 | }; 140 | 141 | int main(int argc, char * argv[]) 142 | { 143 | rclcpp::init(argc, argv); 144 | 145 | const std::string package_name = "soar_ros"; 146 | const std::string share_directory = 147 | ament_index_cpp::get_package_share_directory(package_name); 148 | 149 | std::string soar_path = share_directory + "/Soar/main.soar"; 150 | auto node = std::make_shared("Test Agent", soar_path); 151 | 152 | std::shared_ptr> p = 153 | std::make_shared(node.get()->getAgent(), node, "test"); 154 | node->addPublisher(p); 155 | 156 | std::shared_ptr> s = 157 | std::make_shared(node.get()->getAgent(), node, "testinput"); 158 | node->addSubscriber(s); 159 | 160 | std::shared_ptr> trigger = 161 | std::make_shared(node.get()->getAgent(), node, "Trigger"); 162 | node->addSubscriber(trigger); 163 | 164 | std::shared_ptr> 165 | service = std::make_shared( 166 | node.get()->getAgent(), node, 167 | "AddTwoInts"); 168 | node->addService(service); 169 | 170 | std::shared_ptr> 171 | client = std::make_shared( 172 | node.get()->getAgent(), node, 173 | "AddTwoIntsClient"); 174 | node->addClient(client, "AddTwoIntsClient"); 175 | 176 | if (!node->get_parameter("debug").as_bool()) { 177 | node->startThread(); 178 | } 179 | 180 | rclcpp::executors::MultiThreadedExecutor executor; 181 | executor.add_node(node); 182 | executor.spin(); 183 | rclcpp::shutdown(); 184 | 185 | return 0; 186 | } 187 | --------------------------------------------------------------------------------